kalman_fliter_rlf_bta.m

关于机动目标跟踪的α

function kalman_fliter(T,N,Nvar,Wvar,X0)

%%%初始条件给定
%T=1;
%N=100;
%Nvar=9;
%Wvar=100;
%X0=[8 66]';

%%%%给预存变量存储空间
X=zeros(2,100);
X1=zeros(2,100);
X2=zeros(2,100);
I=eye(2);

V=sqrt(Nvar)*randn(1,N) ;             %%产生方差为Nvar的正态分布的过程噪声数据
W=sqrt(Wvar)*randn(1,N) ;             %%产生方差为Wvar的正态分布的量测噪声数据 


%%%%初始值设定
F=[1 T;0 1] ;     %%%系统方程的一步转移矩阵
H=[T^2/2 T]';       %%%%%系统方程的噪声输入矩阵
C=[1 0];          %%%%%量测矩阵
Q=Nvar;            %%%%%过程噪声的协方差矩阵
R=Wvar;            %%%%量测噪声的协方差矩阵
X1(:,1)=X0;
P0=[Wvar Wvar/T;Wvar/T 2*Wvar/T^2];      %%%%%初始协方差
L=T^2*sqrt(Nvar)/sqrt(Wvar);                   %%%%%滤波增益K
rlf=-[L^2+8*L-(L+4)*sqrt(L^2+8*L)]/8;
bta=[L^2+4*L-L*sqrt(L^2+8*L)]/4;
K=[rlf bta]';


%%%%%真实轨迹循环
X2(:,1)=X0;
for k=1:N
    X2(:,k+1)=F*X2(:,k)+H*V(k);    %%%%系统状态方程
    Z(:,k)=C*X2(:,k)+W(k);       %%%%%%系统量测方程
end

%%%%%滤波算法循环
for k=2:N
    X1(:,k+1)=F*X(:,k);
    P=F*P0*F'+H*Q*H';
    a=Z(:,k+1)-C*X1(:,k+1);
    X(:,k+1)=X1(:,k+1)+K*a;
    P0=[I-K*C]*P;
end

%%%%仿真图
%plot(X2(1,:))
%hold on
%plot(X1(1,:),'r')