clear all
close all
TT=1;
N=30;%观测点数
Te=1;
Tn=1;
De=9;
Dn=9;
%模拟真值，xx为位置真值,vv为速度真值,aa为加速度真值
for i=1:N
xxe(i)=0;
vve(i)=0;
xxn(i)=0;
vvn(i)=0;
end
aae=randn(1,N);
aae=aae*1.5;
aan=randn(1,N);
aan=aan*1.5;
xxe(1)=100; xxn(1)=30;
vve(1)=5; vvn(1)=5;
aae(1)=0.5; aan(1)=0.5;
for k=1:N-1
vve(k+1)=vve(k)+aae(k);
xxe(k+1)=xxe(k)+vve(k)+0.5*aae(k)
vvn(k+1)=vvn(k)+aan(k);
xxn(k+1)=xxn(k)+vvn(k)+0.5*aan(k);
end
%测量值，zx为位置测量值,zv为速度测量值
v1=randn(1,N);
v2=randn(1,N);
for k=1:N
zxe(k)=xxe(k)+v1(k);
zve(k)=vve(k)+v2(k);
zxn(k)=xxn(k)+v1(k);
zvn(k)=vvn(k)+v2(k);
end
%初始化各矩阵
for i=1:N
xe(i)=0; xn(i)=0;%位置的估计值
ve(i)=0; vn(i)=0;%速度的估计值
ae(i)=0; an(i)=0;%加速度的估计值
P(:,:,i)=zeros(6,6);
end
xe(1)=100; xn(1)=30;%X(0)
ve(1)=5; vn(1)=5;
ae(1)=0.5; an(1)=0.5;
X=[xe;ve;ae;xn;vn;an];%状态向量
FX=[100;5;0.5;30;5;0.5];%状态向量的一步预测
Z=[zxe;zve;zxn;zvn];%观测向量
P=[10,0,0,0,0,0;0,10,0,0,0,0;0,0,10,0,0,0;0,0,0,10,0,0;0,0,0,0,10,0;0,0,0,0,0,10];%估计误差方差阵
H=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0];%观测矩阵
R=[9,0,0,0;0,5,0,0;0,0,9,0;0,0,0,5];
%R=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];
K=zeros(6,4);%卡尔曼滤波增益矩阵阵
PF=zeros(6,6);%估计误差方差的一步预测阵
I=eye(size(P));%六维的单位矩阵
Q11=De*TT.^5/10/Te;
Q12=De*TT.^4/4/Te;
Q13=De*TT.^3/3/Te;
Q21=De*TT.^4/4/Te;
Q22=2*De*TT.^2/3/Te;
Q23=De*TT.^2/Te;
Q31=De*TT.^3/3/Te;
Q32=De*TT.^2/Te;
Q33=2*De*TT/Te;
Q44=Dn*TT.^5/10/Tn;
Q45=Dn*TT.^4/4/Tn;
Q46=Dn*TT.^3/3/Tn;
Q54=Dn*TT.^4/4/Tn;
Q55=2*Dn*TT.^2/3/Tn;
Q56=Dn*TT.^2/Tn;
Q64=Dn*TT.^3/3/Tn;
Q65=Dn*TT.^2/Tn;
Q66=2*Dn*TT/Tn;
Q=[Q11,Q12,Q13,0,0,0;Q21,Q22,Q23,0,0,0;Q31,Q32,Q33,0,0,0;
0,0,0,Q44,Q45,Q46;0,0,0,Q54,Q55,Q56;0,0,0,Q64,Q65,Q66];
A1=[1,TT,0.5*TT.^2,0,0,0;0,1,TT,0,0,0;0,0,1,0,0,0;
0,0,0,1,TT,0.5*TT.^2;0,0,0,0,1,TT;0,0,0,0,0,1];
exp1=1/exp(TT/Te);
exp2=1/exp(TT/Tn);
A13=Te.^2*(TT/Te-1+exp1);
A23=Te*(1-exp1);
A33=exp1;
A46=Tn.^2*(TT/Tn-1+exp2);
A56=Tn*(1-exp2);
A66=exp2;
A=[1,TT,A13,0,0,0;0,1,A23,0,0,0;0,0,A33,0,0,0;
0,0,0,1,TT,A46;0,0,0,0,1,A56;0,0,0,0,0,A66];
%卡尔曼滤波
for i=2:N
FX=A1*X(:,i-1);%求一步预测
PF=A*P*A'+Q;
Temp=inv(H*PF*H'+R);
K=PF*H'*Temp;
X(:,i)=FX+K*(Z(:,i)-H*FX);
R2=I-K*H;
P=R2*PF;
%P(:,:,i)=R2*PF*R2'+K*R*K';
end
for i=2:N
xe(i)=X(1,i); xn(i)=X(4,i);
ve(i)=X(2,i); vn(i)=X(5,i);
ae(i)=X(3,i); an(i)=X(6,i);
end
%输出结果
t=0:N-1;
figure(1);
plot(t,xxe,'b:',t,xe,'r',t,zxe,'k--');
title('东向位置估计');
xlabel('时刻(s)');
ylabel('东向位置(m)');
legend('真实值','估计值','测量值');
figure(2);
plot(t,xxn,'b:',t,xn,'r',t,zxn,'k--');
title('北向位置估计');
xlabel('时刻(s)');
ylabel('北向位置(m)');
legend('真实值','估计值','测量值');
figure(3);
plot(t,xe-xxe,'b:',t,xn-xxn,'r');
title('估计误差');
xlabel('时刻(s)');
ylabel('估计误差(m)');
legend('东向估计误差','北向估计误差');
