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

Question

HF 2020-11-7

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/639510-kalman-filter-can-someone-help-me-with-to-find-the-bug-in-my-program

评论： Rena Berman 2021-5-6

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 个评论
显示 1更早的评论隐藏 1更早的评论

Stephen23 2021-3-8

编辑：Rik 2021-3-8

在 MATLAB Online 中打开

Original question on 7th November 2020 by [original poster] retrieved from Google Cache:

http://web.archive.org/web/20210308123607/https://webcache.googleusercontent.com/search?q=cache%3ABDkIB_o-ht0J%3Ahttps%3A%2F%2Fla.mathworks.com%2Fmatlabcentral%2Fanswers%2F639510-kalman-filter-can-someone-help-me-with-to-find-the-bug-in-my-program

http://web.archive.org/web/20210308123903/https://www.mathworks.com/matlabcentral/answers/639510-

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

Rik 2021-3-8

@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.

Rena Berman 2021-5-6

(Answers Dev) Restored edit

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Image Analyst 2020-11-12

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/639510-kalman-filter-can-someone-help-me-with-to-find-the-bug-in-my-program#answer_542508

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

2 个评论
显示无隐藏无

HF 2020-11-12

Hi thanks for your advice! e and g are already assigned in the workspace

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).

请先登录，再进行评论。