%CS_KF_1維      線性  KF   X=[x xv xa]   CS     觀測(cè)噪聲為定值,r=100     機(jī)動(dòng)頻率與極限加速度固定
clear all;
clc;
T=0.1;   N=20;   g=9.8;
r=100;   alpha=0.9;   Amax=8*g;    Amax_=-8*g;
F=[1 T (-1+alpha*T+exp(-alpha*T))/alpha^2;0 1 (1-exp(-alpha*T))/alpha;0 0 exp(-alpha*T)];
U=[(-T+alpha*T^2/2+(1-exp(-alpha*T))/alpha)/alpha;T-(1-exp(-alpha*T))/alpha;1-exp(-alpha*T)];
q11=(1-exp(-2*alpha*T)+2*alpha*T+2*alpha^3*T^3/3-2*alpha^2*T^2-4*alpha*T*exp(-alpha*T))/2/alpha^5;
q12=(exp(-2*alpha*T)+1-2*exp(-alpha*T)+2*alpha*T*exp(-alpha*T)-2*alpha*T+alpha^2*T^2)/2/alpha^4;
q13=(1-exp(-2*alpha*T)-2*alpha*T*exp(-alpha*T))/2/alpha^3;
q22=(4*exp(-alpha*T)-3-exp(-2*alpha*T)+2*alpha*T)/2/alpha^3;
q23=(exp(-2*alpha*T)+1-2*exp(-alpha*T))/2/alpha^2;
q33=(1-exp(-2*alpha*T))/2/alpha;
H=[1 0 0];    R=r^2;
%模擬目標(biāo)真實(shí)運(yùn)動(dòng)軌跡
x0=50000; v0=-150;
a1x=-5;   a2x=60;a3x=-25;
t1=50;    K1=t1/T;
t2=100;   K2=(t2-t1)/T;
t3=150;   K3=(t3-t2)/T;
t4=155;   K4=(t4-t3)/T;
t5=200;   K5=(t5-t4)/T;
t6=220;   K6=(t6-t5)/T;
t7=250;   K7=(t7-t6)/T;
% 0---50s，勻速運(yùn)動(dòng)
k1=1:K1;
x1=x0+v0*(k1*T);
v1=v0*ones(1,length(k1));
a1= 0*ones(1,length(k1));
% 50---100s，慢加速
k2=1:K2;
x2=x1(K1)+v1(K1)*(k2*T)+a1x*(k2*T).^2/2;
v2=v1(K1)+a1x*(k2*T);
a2=a1x*ones(1,length(k2));
% 100---150s，勻速運(yùn)動(dòng)
k3=1:K3;
x3=x2(K2)+v2(K2)*(k3*T);
v3=v2(K2)*ones(1,length(k3));
a3= 0*ones(1,length(k3));
% 150---155s，快減速
k4=1:K4;
x4=x3(K3)+v3(K3)*(k4*T)+a2x*(k4*T).^2/2;
v4=v3(K3)+a2x*(k4*T);
a4=a2x*ones(1,length(k4));
% 155---200s，勻速運(yùn)動(dòng)
k5=1:K5;
x5=x4(K4)+v4(K4)*(k5*T);
v5=v4(K4)*ones(1,length(k5));
a5=0*ones(1,length(k5));
% 200---220s，中加運(yùn)動(dòng)
k6=1:K6;
x6=x5(K5)+v5(K5)*(k6*T)+a3x*(k6*T).^2/2;
v6=v5(K5)+a3x*(k6*T);
a6=a3x*ones(1,length(k6));
% 220---250s，勻速運(yùn)動(dòng)
k7=1:K7;
x7=x6(K6)+v6(K6)*(k7*T);
v7=v6(K6)*ones(1,length(k7));
a7=0*ones(1,length(k7));

x=[x1 x2 x3 x4 x5 x6 x7];
v=[v1 v2 v3 v4 v5 v6 v7];
a=[a1 a2 a3 a4 a5 a6 a7];
K=K1+K2+K3+K4+K5+K6+K7;
xx=zeros(1,K);    vv=zeros(1,K);    aa=zeros(1,K);
ex1=zeros(1,K);   ex2=zeros(1,K);
ev1=zeros(1,K);   ev2=zeros(1,K);
ea1=zeros(1,K);   ea2=zeros(1,K);
%進(jìn)行N次仿真
for i=1:N
    %產(chǎn)生觀測(cè)數(shù)據(jù)
    zx=randn(1,K)*r+x;  
    Z=zx;    
    %兩點(diǎn)起始法確定初值
    LX=[zx(2) (zx(2)-zx(1))/T 0]';
    LP=[r^2 r^2/T 0;r^2/T 2*r^2/T^2 0;0 0 0];
    %畫(huà)圖所需量初值
    xx(1)=xx(1)+zx(1); 
    ex1(1)=ex1(1)+x(1)-zx(1);    ex2(1)=ex2(1)+(x(1)-zx(1))^2; 
   
    %開(kāi)始濾波
    for k=2:K
        xx(k)=xx(k)+LX(1);vv(k)=vv(k)+LX(2);aa(k)=aa(k)+LX(3);
        ex1(k)=ex1(k)+x(k)-LX(1);            ex2(k)=ex2(k)+(x(k)-LX(1))^2; 
        ev1(k)=ev1(k)+v(k)-LX(2);            ev2(k)=ev2(k)+(v(k)-LX(2))^2; 
        ea1(k)=ea1(k)+a(k)-LX(3);            ea2(k)=ea2(k)+(a(k)-LX(3))^2; 
        %%%%%%%%%%%%%%%%%%%%  濾波   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if (LX(3)>0)
            sigma_2=(4-pi)*(Amax-LX(3))^2/pi;
        else
            sigma_2=(4-pi)*(Amax_-LX(3))^2/pi;
        end
        Q=2*alpha*sigma_2*[q11 q12 q13;q12 q22 q23;q13 q23 q33];
        X=F*LX+U*LX(3);
        P=F*LP*F'+Q;
        Kk=P*H'*inv(H*P*H'+R);
        LX=X+Kk*(Z(k)-H*X);
        LP=P-Kk*H*P;        
    end  
end
%計(jì)算濾波誤差
ex_=zeros(1,K);ex=zeros(1,K);ev_=zeros(1,K);ev=zeros(1,K);ea_=zeros(1,K);ea=zeros(1,K);
for j=1:K
    xx(j)=xx(j)/N;  vv(j)=vv(j)/N;  aa(j)=aa(j)/N;     
    ex_(j)=ex1(j)/N;
    ex(j)=sqrt(ex2(j)/N-ex_(j)^2);
    ev_(j)=ev1(j)/N;
    ev(j)=sqrt(ev2(j)/N-ev_(j)^2);
    ea_(j)=ea1(j)/N;
    ea(j)=sqrt(ea2(j)/N-ea_(j)^2);    
end
% 輸出圖形
figure(1);
plot(x,'k');hold on;
plot(zx,'g');hold on;
plot(xx,'r');

figure(2);
plot(ex);
title('位置');axis([0,2000,0,150]);

figure(3);
plot(ev);
title('速度');axis([0,2000,0,100]);

figure(4);
plot(ea);
title ('加速度');