Original question on 7th November 2020 by [original poster] retrieved from Google Cache:
Kalman Filter - Can someone help me with to find the bug in my program?
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
