无迹卡尔曼滤波 | 易学教程

无迹卡尔曼滤波在生成sigma点集的时候，要用到一个P矩阵，这个P矩阵的初值是怎样设置的，是随便进行设定的吗？还有就是这个P矩阵怎样保证每次迭代更新后P矩阵都是正定的，有没有大佬可以解释以下啊。
下边附上黄小平和王岩老师编著的《卡尔曼滤波原理及应用》中的一段程序，希望有大佬可以给与解释。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 无迹Kalman滤波在目标跟踪中的应用
% 详细原理介绍及中文注释请参考：
% 《卡尔曼滤波原理及应用-MATLAB仿真》，电子工业出版社，黄小平著。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function UKF
clc;clear;
T=1;
N=60/T;
X=zeros(4,N);
X(:,1)=[-100,2,200,20]; %x位置，x速度，y位置，y速度
Z=zeros(1,N); %传感器对位置的观测
delta_w=1e-3; %如果增大这个数，目标真实轨迹就是曲线了
Q=delta_wdiag([0.5,1]) ;%过程噪声均值
G=[T^{2/2,0;T,0;0,T}2/2;0,T];%过程噪声驱动矩阵
R=5; %观测噪声方差
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
x0=200;
y0=300;
Xstation=[x0,y0]; %表示位置
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
v=sqrtm®randn(1,N);
for t=2:N
X(:,t)=FX(:,t-1)+Gsqrtm(Q)randn(2,1);
end
for t=1:N
Z(t)=Dist(X(:,t),Xstation)+v(t);
end
L=4;
alpha=1;
kalpha=0;
belta=2;
ramda=3-L;
for j=1:2L+1
Wm(j)=1/(2*(L+ramda));
Wc(j)=1/(2*(L+ramda));
end
Wm(1)=ramda/(L+ramda);
Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Xukf=zeros(4,N);
Xukf(:,1)=X(:,1);
P0=eye(4);
for t=2:N
xestimate=Xukf(:,t-1);
P=P0;
cho=(chol(P*(L+ramda)))’;
for k=1:L
xgamaP1(:,k)=xestimate+cho(:,k);
xgamaP2(:,k)=xestimate-cho(:,k);
end
Xsigma=[xestimate,xgamaP1,xgamaP2];
Xsigmapre=FXsigma;
Xpred=zeros(4,1);
for k=1:2L+1
Xpred=Xpred+Wm(k)Xsigmapre(:,k);
end
Ppred=zeros(4,4);
for k=1:2L+1
Ppred=Ppred+Wc(k)(Xsigmapre(:,k)-Xpred)(Xsigmapre(:,k)-Xpred)’;
end
Ppred=Ppred+GQG’;
chor=(chol((L+ramda)Ppred))’;
for k=1:L
XaugsigmaP1(:,k)=Xpred+chor(:,k);
XaugsigmaP2(:,k)=Xpred-chor(:,k);
end
Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
for k=1:2L+1
Zsigmapre(1,k)=hfun(Xaugsigma(:,k),Xstation);
end
Zpred=0;
for k=1:2L+1
Zpred=Zpred+Wm(k)Zsigmapre(1,k);
end
Pzz=0;
for k=1:2L+1
Pzz=Pzz+Wc(k)(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)’;
end
Pzz=Pzz+R;

Pxz=zeros(4,1);
for k=1:2*L+1
    Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)';
end
K=Pxz*inv(Pzz);
xestimate=Xpred+K*(Z(t)-Zpred);
P=Ppred-K*Pzz*K';
P0=P;
Xukf(:,t)=xestimate;

end
for i=1:N
Err_KalmanFilter(i)=Dist(X(:,i),Xukf(:,i));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
hold on;box on;
plot(X(1,:),X(3,:),’-k.’);
plot(Xukf(1,:),Xukf(3,:),’-r+’);
legend(‘真实轨迹’,‘UKF轨迹’)
figure
hold on; box on;
plot(Err_KalmanFilter,’-ks’,‘MarkerFace’,‘r’)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function d=Dist(X1,X2)
if length(X2)<=2
d=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
else
d=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end
function [y]=hfun(x,xx)
y=sqrt((x(1)-xx(1))^{2+(x(3)-xx(2))}2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

来源：CSDN

作者：歪猴儿2.0

链接：https://blog.csdn.net/weixin_44365345/article/details/103705884

标签

卡尔曼滤波