% 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);
评论0