kaermanlvbo.m

卡尔曼滤波用matlab作仿真

clear;
e=0.005;%xx.yy.
%e2=0.006白噪声的均方误差
W=[1 1 0 0;0 1 0 0;0 0 1 1;0 0 0 1];%状态转移矩阵
G=[0 0.5 0 0;0 1 0 0;0 0 0.5 0;0 0 1 0];%模型噪声对状态估计影响的转移矩阵
Q=[0.25 0.5 0 0;0.5 1 0 0;0 0 0.25 0.5;0 0 0.5 1]*(e^2);%输入扰动的协方差矩阵
R=[e^2,0;0,e^2];%观测噪声协方差矩阵

%获得观测值
for k=1:3000
x(k)=k;%实际值
%y(k)=0;%实际值
y(k)=k;%实际值
a(k)=atan(y(k)/x(k))+normrnd(0,e,1);%观测角度值
r(k)=sqrt(x(k)^2+y(k)^2)+normrnd(0,e,1);%观测距离值

%a(k)=atan(yy(k)/xx(k));%观测值
%r(k)=sqrt(xx(k)^2+yy(k)^2);%观测值
yy(k)=r(k)*sin(a(k));%转化到直角坐标系下的观测值
xx(k)=r(k)*cos(a(k));%转化到直角坐标系下的观测值
end


%两点启动
x0=[r(2)*cos(a(2));r(2)*cos(a(2))-r(1)*cos(a(1));r(2)*sin(a(2));r(2)*sin(a(2))-r(1)*sin(a(1))];%航迹初始的状态估计
p1=[e^2 e^2 0 0;e^2 (e^2)*3 0 0;0 0 e^2 e^2;0 0 e^2 3*(e^2)];%初始状态协方差矩阵
%p1=[1 0 0 0;0 1 0 0;0 0 0.01 0;0 0 0 0.01];
%rr=sqrt(x0(1)^2+x0(2)^2);
%H=[-x0(2)/(rr^2) 0  x0(1)/(rr^2)  0;x0(1)/rr 0 x0(2)/rr  0];%线性化


z=[r;a];
xxxx=W*x0;%预测值 
for k=3:3000   
    rr=sqrt(yy(k)^2+xx(k)^2);
    H=[xx(k)/rr 0 yy(k)/rr  0;-yy(k)/(rr^2) 0  xx(k)/(rr^2)  0];%线性化
  %z1(:,k)=z(:,k)+normrnd(0,0.005,2,1);
  p2=W*p1*W'+G*Q*G';
  %  p2=W*p1*W'+G*Q*G';%预测协方差阵
    
% s=H*p2*(H')+R
    K=p2*(H')*(inv(H*p2*(H')+R));%滤波增益阵
    p1=[eye(4)-K*H]*p2;%估计协方差
    %zz=H*xxxx
    zz=[sqrt((xxxx(1)^2+xxxx(3)^2));atan(xxxx(3)/xxxx(1))];
    %e=z1(:,k)-zz;
    E=z(:,k)-zz;
  K*E;
    xxx=xxxx+K*E;%估计值
    i(k)=xxx(1);
    j(k)=xxx(3);
    %x0=W*xxx;%预测值 
    xxxx=W*xxx;%预测值 
   
    ii(k)=xxxx(1);
    jj(k)=xxxx(3);
end


subplot(2,1,1);
plot(x,y,'r',i,j,'g')
%plot(x,y,'r',ii,jj,'g')

%plot(xx,yy)
subplot(2,1,2);
plot(x-i);,
%plot(i(3:10),j(3:10),'r',ii(3:10),jj(3:10),'g')
%plot(xx,yy,'r',x,y,'g')