www.pudn.com > IMM_AFUKF.rar > CAEKF.m, change:2009-09-12,size:1265b


 
clear all 
close all 
clc 
%%%加载数据 
load Measure.mat%加载量测数据 
load Real.mat%加载真实值数据 
Z=TimeRPhi(2:3,:);%量测值 
Xr=TXYdXdYAxAyTPhi;%真值 
%%%设置初值 
T=0.1;%采样时间 
m=10;%斜距标准差 
n=0.1/57.3;%方位角标准差 
X=[10000;20;0;0;0;0];%状态变量初值 
P=diag([10,10,10,10,10,10]);%误差协方差初值 
R=diag([m^2,n^2]);%量测噪声方差 
 
F=[1 0 T 0 T^2/2 0;%状态转移矩阵 
   0 1 0 T 0 T^2/2; 
   0 0 1 0 T 0; 
   0 0 0 1 0 T; 
   0 0 0 0 1 0; 
   0 0 0 0 0 1]; 
Ta=[T^2/2 0 T 0 1 0; 
    0 T^2/2 0 T 0 1]; 
Ta=Ta';%噪声阵 
% sigam=60; 
sigam=20; 
Q=Ta*Ta'*sigam^2;%系统噪声方差 
 
for k=1:501 
    Xk=F*X; 
    Pk=F*P*F'+Q; 
    Zk=[((Xk(1))^2+(Xk(2))^2)^0.5;atan((Xk(2))/(Xk(1)))]; 
    H11=Xk(1)/((Xk(1))^2+(Xk(2))^2)^0.5; 
    H12=Xk(2)/((Xk(1))^2+(Xk(2))^2)^0.5; 
    H21=-Xk(2)/((Xk(1))^2+(Xk(2))^2); 
    H22=Xk(1)/((Xk(1))^2+(Xk(2))^2); 
    H=[H11 H12 0 0 0 0; 
       H21 H22 0 0 0 0]; 
    
    K=Pk*H'*inv(H*Pk*H'+R); 
    X=Xk+K*(Z(:,k)-Zk); 
    P=Pk-K*H*Pk; 
    Xe(:,k)=X; 
    Re=((X(1))^2+(X(2))^2)^0.5; 
    Te=atan(X(2)/X(1)); 
    Ze(:,k)=[Re;Te]; 
     
end 
 
figure(1) 
plot(Xe(1,:),Xe(2,:),Xr(2,:),Xr(3,:),'r') 
figure(2) 
plot(Xr(7,:)-Xe(6,:),'r') 
 
% figure(2) 
% plot(Xr(9,:)-Ze(2,:),'r') 
% hold on 
% plot(Xr(9,:)-Z(2,:))