sjlb.m

卡尔曼滤波值计算方法 一飞行物

N=100;
x(:,1)=[30;300;20];
T=0.5;
F=[1,T,T^2/2;0,1,T;0,0,1];
Gama=[T^3/6,T^2/2,T]';
H=[1  0  0];
A=x(:,1);
Q=1;
R=100;
for k=2:N
    w=normrnd(0,1);
    v=normrnd(0,10);
    x(:,k)=F*x(:,k-1)+Gama*w;
    A=[A,x(:,k-1)];
    z(:,k)=H*x(:,k)+v;
end
%初始化%
p0=zeros(3);
x0=[1;1;1];
x_hat(:,1)=x0;
B=x0;
C=z(:,1);
P=p0;
RR=zeros(1,N);
VV=zeros(1,N);
for m=2:N
    xx_hat(:,m-1)=F*x_hat(:,m-1);
    B=[B,xx_hat(:,m-1)];
    z1(:,m)=H*xx_hat(:,m-1);
    C=[C,z1(:,m)];
    PP=F*P*F'+Gama*Q*Gama';
    S=H*PP*H'+R;
    KK=PP*H'*inv(S);
    z1_tutor(:,m)=z(:,m)-C(:,m);
    x_hat(:,m)=xx_hat(:,m-1)+KK*z1_tutor(:,m);
    P=PP-KK*S*KK';
    RR(1,m)=P(1,1);
    VV(1,m)=P(2,2);
end
k=1:N;
    plot(k,B(1,k),'b*'),title('实际轨迹（.）和卡尔曼滤波估计轨迹(*)的比较'),hold on
    plot(k,A(1,k),'r.'),hold on
figure    
    plot(k,B(2,k),'b*'), title('实际速度（.）和卡尔曼滤波估计速度(*)的比较'),hold on
    plot(k,A(2,k),'r.'),hold on
figure
    plot(RR,'b.'),title('卡尔曼滤波位置估计方差(.)和速度估计方差(*)'),hold on
    plot(VV,'r*'),hold on