clc;
clear;
%%%%%%%%%%%%%%%%%%%%%%%%
N=100;   
x=zeros(N,1);
y=zeros(N,1);
var=2;
I=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];%I為四階單位陣
%%%%%%%%%%%%%%%%%%%%%%%%%列出狀態(tài)方程
x(1)=randn(1,1);   %令x(-1)=x(-2)=x(-3)=x(-4)=0
x(2)=randn(1,1)+1.352*x(1);
x(3)=randn(1,1)+1.352*x(2)-1.338*x(1);
x(4)=randn(1,1)+1.352*x(3)-1.338*x(2)+0.602*x(1);
for n=5:N   
    x(n)=1.352*x(n-1)-1.338*x(n-2)+0.602*x(n-3)-0.24*x(n-4)+randn(1,1); %x為真實(shí)值
end;
v=4*randn(N,1);
y=x+v;  %z_x為觀測(cè)樣本值=真值+噪聲
%%%%%%%%%%%%%%%%%%%%%%%%濾波
    xk_s(1)=y(1);    %賦初值
    xk_s(2)=y(2);
    xk_s(3)=y(3);
    xk_s(4)=y(4);
Ak=[1.352,-1.338,0.662,0.240;1,0,0,0;0,1,0,0;0,0,1,0];%狀態(tài)變量之間的增益矩陣Ak
Ck=[1 0 0 0];%狀態(tài)變量與輸出信號(hào)之間的增益矩陣Ck,(一維:僅僅對(duì)x方向進(jìn)行估計(jì))
Rk=[1];%量測(cè)噪聲協(xié)方差陣
Pk=[1 0 0 0
    0 1 0 0
    0 0 1 0
    0 0 0 1] ;%噪聲的均方誤差陣
 xk=[y(1);y(2);y(3);y(4)];
Qk = [1];
%%%%%%%%%%%%%%%%%%%%%%%%Kalman濾波開(kāi)始,估計(jì)循環(huán)
for r=5:N
    yk=y(r);
    Pk1=Ak*Pk*Ak'+Qk;%(未考慮噪聲)k時(shí)刻濾波的均方誤差矩陣
    %Pk1=Ak*Pk*Ak';%(未考慮噪聲)k時(shí)刻濾波的均方誤差矩陣
    Hk=Pk1*Ck'*inv(Ck*Pk1*Ck'+Rk); %增益方程
    xk=Ak*xk+Hk*(yk-Ck*Ak*xk);     %遞推公式
    Pk=(I-Hk*Ck)*Pk1;%濾波后的均方誤差矩陣
    xk_s(r)=xk(1,1);      %xk_s為估計(jì)值
end
%end
e_x=0;
eq_x=0;
e_x1=N:1;
%計(jì)算濾波的均值,計(jì)算濾波誤差的均值
for i=1:N
    e_x(i)=x(i)-xk_s(i); %誤差=真實(shí)值-濾波估計(jì)值
end
%%%%%%%%%%%%%%%%%%%%%%%作圖
t=1:N;
figure(1);
plot(t,x,'r-',t,y,'g:',t,xk_s,'b-.');
legend('真實(shí)軌跡','觀測(cè)樣本','估計(jì)軌跡'); 
figure(2);
plot(e_x);          
legend('平均誤差');