www.pudn.com > ekf_4d.rar > ekf_4d.m

% function  EXT Kalman Filtering 
% vx[n]=vx[n-1]+ux[n] 
% vy[n]=vy[n-1]+uy[n] 
% rx[n]=rx[n-1]+vx[n-1]*t 
% ry[n]=ry[n-1]+vy[n-1]*t 
% s[n]=[rx[n];ry[n];vx[n];vy[n]] 
% A=[1 0 t 0;0 1 0 t;0 0 1 0;0 0 0 1] 
% s[n-1]=[rx[n-1];ry[n-1];vx[n-1];vy[n-1]] 
% u[n]=[0;0;ux[n];uy[n]] 
%       s[n]=As*[n-1]+u[n] 
%       x[n]=h(s[n])+w[n] 
 
% initialization 
clear; 
n=100; 
A=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1]; 
rx=zeros(1,n); % 实际位置 
ry=zeros(1,n); % 实际位置 
vx=zeros(1,n); % 实际速度 
vy=zeros(1,n); % 实际速度 
rxe=zeros(1,n); % 精确位置 
rye=zeros(1,n); % 精确位置 
vxe=zeros(1,n); % 精确速度 
vye=zeros(1,n); % 精确速度 
rx(1,1)=10; % 位置初始 
ry(1,1)=-5; % 位置初始 
vx(1,1)=-0.2; % 速度初始 
vy(1,1)=0.2; % 速度初始 
rxe(1,1)=10; 
rye(1,1)=-5; 
vxe(1,1)=-0.2; 
vye(1,1)=0.2; 
s=[rx;ry;vx;vy]; 
se=[rxe;rye;vxe;vye]; 
r=zeros(1,n); 
b=zeros(1,n); 
rn=zeros(1,n); 
re=zeros(1,n); 
bn=zeros(1,n); 
x=[rn;bn]; 
H=zeros(2,4,n); 
 
ux=sqrt(0.00001)*randn(1,n); 
uy=sqrt(0.00001)*randn(1,n); 
wr=sqrt(0.001)*randn(1,n); 
wb=sqrt(0.0001)*randn(1,n); 
 
Q=[0 0 0 0;0 0 0 0;0 0 cov(ux) 0;0 0 0 cov(uy)]; % 过程噪声矩阵 
C=[cov(wr) 0; 0 cov(wb)]; % 观测噪声矩阵 
 
ss=zeros(4,n);% 精确值 
sn=zeros(4,n); % 估计修正值 
sn(:,1)=sn(:,1)+[15;5;0;0]; % 估计修正初始值 
ss=zeros(4,n); % 一步预测 
hs=zeros(2,n); % 观测值预测 
inn=zeros(2,n); % 新息 
M=zeros(4,4,n); % 估计均方误差 
M(:,:,1)=M(:,:,1)+[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; % 估计均方误差初始值 
MM=zeros(4,4,n); % 一步预测均方误差 
K=zeros(4,2,n); % 卡尔曼增益 
 
% kalman filtering 
 
for t=1:n-1 
%            s[n]=As*[n-1]+u[n] 
%            x[n]=h(s[n])+w[n] 
 
    vx(1,t+1)=vx(1,t)+ux(1,t+1); % 速度过程方程 
    vy(1,t+1)=vy(1,t)+uy(1,t+1); % 速度过程方程 
    rx(1,t+1)=rx(1,t)+vx(1,t); % 位置过程方程 
    ry(1,t+1)=ry(1,t)+vy(1,t); % 位置过程方程 
    s(:,t+1)=[rx(1,t+1);ry(1,t+1);vx(1,t+1);vy(1,t+1)]; % 过程矩阵，维数4*1 
     
    vxe(1,t+1)=vxe(1,t); % 理论精确值 
    vye(1,t+1)=vye(1,t); % 理论精确值 
    rxe(1,t+1)=rxe(1,t)+vxe(1,t); % 理论精确值 
    rye(1,t+1)=rye(1,t)+vye(1,t); % 理论精确值 
    se(:,t+1)=[rxe(1,t+1);rye(1,t+1);vxe(1,t+1);vye(1,t+1)]; % 过程矩阵，维数4*1 
     
    r(1,t+1)=(rx(1,t+1)^2+ry(1,t+1)^2)^(1/2); % 理论测量距离（含测量误差） 
    b(1,t+1)=atan(ry(1,t+1)/rx(1,t+1)); % 理论测量方位（含测量误差） 
    rn(1,t+1)=r(1,t+1)+wr(1,t+1); % 实际测量距离 
    re(1,t+1)=(rxe(1,t+1)^2+rye(1,t+1)^2)^(1/2); % 理论测量距离（含测量误差） 
    bn(1,t+1)=b(1,t+1)+wb(1,t+1); % 实际测量方位 
    x(:,t+1)=[rn(1,t+1);bn(1,t+1)]; % 观测矩阵,维数2*1 
 
    % 对观测方程求导,得到雅可比矩阵H，维数2*4 
    H(:,:,t+1)=[rx(1,t+1)/((rx(1,t+1)^2+ry(1,t+1)^2)^(1/2)) ry(1,t+1)/((rx(1,t+1)^2+ry(1,t+1)^2)^(1/2)) 0 0;-ry(1,t+1)/(rx(1,t+1)^2+ry(1,t+1)^2) rx(1,t+1)/(rx(1,t+1)^2+ry(1,t+1)^2) 0 0]; 
         
     
    MM(:,:,t+1)=A*M(:,:,t)*A'+Q; % 一步预测均方误差 
    K(:,:,t+1)=MM(:,:,t+1)*H(:,:,t+1)'/(C+H(:,:,t+1)*MM(:,:,t+1)*H(:,:,t+1)'); % 卡尔曼增益 
    M(:,:,t+1)=(eye(4)-K(:,:,t+1)*H(:,:,t+1))*MM(:,:,t+1);% 估计均方误差 
    ss(:,t+1)=A*sn(:,t); % 一步预测 
    hs(:,t+1)=[((ss(1,t+1))^2+(ss(2,t+1))^2)^(1/2);atan(ss(2,t+1)/ss(1,t+1))]; % 观测值预测 
    inn(:,t+1)=x(:,t+1)-hs(:,t+1); % 新息 
    sn(:,t+1)=ss(:,t+1)+K(:,:,t+1)*inn(:,t+1); % 预测修正 
end 
 
subplot(2,1,1); 
plot(sn(1,:),sn(2,:)); % kalman估计值 
hold on; 
plot(se(1,:),se(2,:),'-r'); % 理论精确值 
 
subplot(2,1,2);  
plot(rn); % 实际测量距离 
hold on; 
plot(re,'-g'); % 理论精确值