# Kalman Filter - Can someone help me with to find the bug in my program?

1 view (last 30 days)

Show older comments

Hello everybody, I am trying to implement the Kalman filter. I found a really good paper, which gives me all the equations I need. However I am not quite sure what value should n have (second equation 9.a) ?

Can someone help me with this?

My code is the following:

%% Kalman Filter

% Parameters

x1 = 1;

x2 = 1;

x3 = 1;

w = 0;

X = [x1, x2, x3];

alpha = 1;

n = 1;

k = 1;

lambda = alpha^2 *(n + k) - n;

betha = 1;

Q = 0;

roh = 1;

R = 0;

W_0_m = lambda / (lambda + n);

W_i = 1/(2*(lambda + n));

W_0_c = (lambda / (lambda + n)) + 1 - alpha^2 + betha;

y_t(:,1) = e; % x-acceleration from the sensor

y_t(:,2) = g; % y-acceleration from the sensor

x_dach = zeros;

L = 200000; % length of the simulation

for z = 1:L

%% A priori estimation

[X_est(1,1), X_est(2,1), X_est(3,1)] = stateequation(X(1), X(2), X(3), w);

for i = 0: 2*n

if i == 0

x_t_dach_m = W_0_m * X_est(i+1);

else

x_t_dach_m = W_i * X_est(i+1)+ x_t_dach_m;

end

end

for j = 0: 2*n

if i== 0

P_t_m = W_0_c * (X_est(i+1)-x_t_dach_m) * (X_est(i+1)-x_t_dach_m);

else

P_t_m = W_i * (X_est(i+1)-x_t_dach_m) * (X_est(i+1)-x_t_dach_m) + P_t_m;

end

end

P_t_m = P_t_m + Q;

X_t_t1 = [x_t_dach_m, x_t_dach_m + gamma * sqrt(P_t_m), x_t_dach_m - gamma * sqrt(P_t_m)];

Y_t_t1 = outputtransformation(X_t_t1(1), X_t_t1(2), roh, v);

for i = 0: 2*n

if i == 0

y_t_dach_m = W_0_m * Y_t_t1(i+1);

else

y_t_dach_m = W_i * Y_t_t1(i+1) + y_t_dach_m;

end

end

%% A posteriori estimation

for i = 0:2*n

if i == 0

Py_y = W_0_c * (Y_t_t1(i+1)-y_t_dach_m) * (Y_t_t1(i+1)-y_t_dach_m);

else

Py_y = W_i * (Y_t_t1(i+1)-y_t_dach_m) * (Y_t_t1(i+1)-y_t_dach_m) + Py_y;

end

end

Py_y = Py_y + R;

for i = 0:2*n

if i == 0

Px_y = W_0_c * (X_t_t1(i+1)-x_t_dach_m) * (X_t_t1(i+1)-x_t_dach_m);

else

Px_y = W_i * (X_t_t1(i+1)-x_t_dach_m) * (X_t_t1(i+1)-x_t_dach_m) + Px_y;

end

end

K_t = Px_y * inv(Py_y);

x_dach(z) = x_t_dach_m + K_t * (y_t - y_t_dach_m);

P_t = P_t_m - K_t * Py_y * K_t;

X = X_t_t1;

end

%% Functions

function [x1_new, x2_new, x3_new] = stateequation (x1, x2, x3, w)

x1_new = 2*cos(x3) * x1 -x2;

x2_new = x1;

x3_new = x3 + w;

end

function y_new = outputtransformation (x1, x2, roh, v)

y_new = -(1 + roh^2) * x1 + roh^2 * x2 + v;

end

##### 3 Comments

Rik
on 8 Mar 2021

@HF You are allowed to ask for removal of your name from a post (I have so often seen Mathworks staff do this, that I have taken the liberty of removing it from Stephen's post).

However, it is extremely rude to remove your question. Why should your rudeness be answered by civility? Please don't edit away your question after someone has spent the time answering it.

### Answers (1)

Image Analyst
on 12 Nov 2020

The program never assigns e and g, so it fails at that point. You need to assign them some values.

##### 2 Comments

Image Analyst
on 12 Nov 2020

### See Also

### Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!