MATLAB Answers

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

7 ビュー (過去 30 日間)
HF
HF 2020 年 11 月 7 日
コメント済み: Rena Berman 2021 年 5 月 6 日 20:22
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 件のコメント
Rena Berman
Rena Berman 2021 年 5 月 6 日 20:22
(Answers Dev) Restored edit

サインインしてコメントする。

回答 (1 件)

Image Analyst
Image Analyst 2020 年 11 月 12 日
The program never assigns e and g, so it fails at that point. You need to assign them some values.
  2 件のコメント
Image Analyst
Image Analyst 2020 年 11 月 12 日
If you still have problems, then post the missing code where these other variables are mysteriously put into the workspace. Otherwise people can't run your code and reproduce your error(s).

サインインしてコメントする。

カテゴリ

Community Treasure Hunt

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

Start Hunting!

Translated by