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

2 views (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

### 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

### Categories

### Community Treasure Hunt

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

Start Hunting!