[记录二]学习扩展卡尔曼滤波器
程序员文章站
2022-07-12 09:36:24
...
下面是一平抛物体的matlab应用EKF的程序(复制过来的,可以运行,原理上似乎有点问题):
function test_ekf
kx = .01; ky = .05; % 阻尼系数
g = 9.8; % 重力
t = 10; % 仿真时间
Ts = 0.1; % 采样周期
len = fix(t/Ts); % 仿真步数
% 真实轨迹模拟
dax = 1.5; day = 1.5; % 系统噪声
X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值
for k=2:len
x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);
x = x + vx*Ts;
vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;
y = y + vy*Ts;
vy = vy + (ky*vy^2-g+day*randn(1))*Ts;
X(k,:) = [x, vx, y, vy];
end
figure(1), hold off, plot(X(:,1),X(:,3),'-b'), grid on
%
figure(2), plot(X(:,2:2:4))
% 构造量测量
mrad = 0.001;
dr = 10; dafa = 10*mrad; % 量测噪声
for k=1:len
r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
a = atan(X(k,1)/X(k,3)) + dafa*randn(1,1);
Z(k,:) = [r, a];
end
figure(1), hold on, plot(Z(:,1).*sin(Z(:,2)), Z(:,1).*cos(Z(:,2)),'*')
% ekf 滤波
Qk = diag([0; dax; 0; day])^2;
Rk = diag([dr; dafa])^2;
Xk = zeros(4,1);
Pk = 100*eye(4);
X_est = X;
for k=1:len
Ft = JacobianF(X(k,:), kx, ky, g);
Hk = JacobianH(X(k,:));
fX = fff(X(k,:), kx, ky, g, Ts);
hfX = hhh(fX, Ts);
[Xk, Pk, Kk] = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,:)'-hfX);
X_est(k,:) = Xk';
end
figure(1), plot(X_est(:,1),X_est(:,3), '+r')
xlabel('X'); ylabel('Y'); title('ekf simulation');
legend('real', 'measurement', 'ekf estimated');
function F = JacobianF(X, kx, ky, g) % 系统状态雅可比函数
vx = X(2); vy = X(4);
F = zeros(4,4);
F(2,2) = -2*kx*vx;
F(3,4) = 1;
F(4,4) = 2*ky*vy;
F(1, 2) = 1;
function H = JacobianH(X) % 量测雅可比函数
x = X(1); y = X(3);
H = zeros(2,4);
r = sqrt(x^2+y^2);
H(1,1) = 1/r; H(1,3) = 1/r;
xy2 = 1+(x/y)^2;
H(2,1) = 1/xy2*1/y; H(2,3) = 1/xy2*x*(-1/y^2);
function fX = fff(X, kx, ky, g, Ts) % 系统状态非线性函数
x = X(1); vx = X(2); y = X(3); vy = X(4);
x1 = x + vx*Ts;
vx1 = vx + (-kx*vx^2)*Ts;
y1 = y + vy*Ts;
vy1 = vy + (ky*vy^2-g)*Ts;
fX = [x1; vx1; y1; vy1];
function hfX = hhh(fX, Ts) % 量测非线性函数
x = fX(1); y = fX(3);
r = sqrt(x^2+y^2);
a = atan(x/y);
hfX = [r; a];
function [Xk, Pk, Kk] = ekf(Phikk_1, Qk, fXk_1, Pk_1, Hk, Rk, Zk_hfX) % ekf 滤波函数
%时间更新方程(2.15)
Pkk_1 = Phikk_1*Pk_1*Phikk_1' + Qk;
%状态更新方程(2.16)
Pxz = Pkk_1*Hk'; Pzz = Hk*Pxz + Rk; Kk = Pxz*Pzz^-1;
%状态更新方程(2.17)
Xk = fXk_1 + Kk*Zk_hfX;
%状态更新方程(2.18)
Pk = Pkk_1 - Kk*Pzz*Kk';
----分割线2017-05-22
我分析一一下
系统的数学模型如下:
X(n-1) = [x,vx,y,vy] '= [x(n-1)+vx*Ts; vx+(-kx*vx^2)*Ts; y(n-1)+vy*Ts vy+(-ky*vy^2)*Ts;] + w(n);
Z(n-1) = [r,a] =[ sqrt(X(k,1)^2+X(k,3)^2); atan(X(k,1)/X(k,3)) ;] + v(n);
问题一:
我很好奇 Z为什么是这样的???这样似乎和X的意义没有统一;
问题二:
JacobianF求雅可比矩阵是不是有问题??
问题三:
[Xk, Pk, Kk] = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,:)'-hfX);
这个语句中第一个参数如果联系上下文,这个参数应该直接就是F的雅可比举证,为什么会是eye(4)+Ft*Ts?看不明!
这样 %状态更新方程(2.17)
Xk = fXk_1 + Kk*Zk_hfX;
这一句应该才是F的更新。。。。。
问题四:
Qk = diag([0; dax; 0; day])^2;
Rk = diag([dr; dafa])^2;
原谅我《概率论与数理统计》没学好。有大神告诉我么?
----分割线,mark 2017-5-23
上一篇: C++下扩展卡尔曼类(EKF)的实现
下一篇: BRDF Explained
推荐阅读