kalmanpzvar.m

卡尔曼滤波的一个应用实例.AR(4)模型

clc;
clear;
%%%%%%%%%%%%%%%%%%%%%%%%
N=100;   
x=zeros(N,1);
y=zeros(N,1);
noise_var=1;
%noise_var=4;
I=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];%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为真实值
end;

V=sqrt(noise_var)*randn(N,1); 

y=x+V;  %z_x为观测样本值=真值+噪声
%%%%%%%%%%%%%%%%%%%%%%%%滤波
    xk_s(1)=y(1);    %赋初值
    xk_s(2)=y(2);
    xk_s(3)=y(3);
    xk_s(4)=y(4);
A=[1.352,-1.338,0.662,0.240;1,0,0,0;0,1,0,0;0,0,1,0];%状态变量之间的增益矩阵Ak
C=[1 0 0 0];%状态变量与输出信号之间的增益矩阵Ck
%Rk=[1];%量测噪声协方差阵
R=var(V);
pk=[noise_var 0 0 0
    0 noise_var 0 0
    0 0 noise_var 0
    0 0 0 noise_var] ;%噪声的均方误差阵
 xk=[y(1);y(2);y(3);y(4)];
Qk = [1];
%%%%%%%%%%%%%%%%%%%%%%%%Kalman滤波开始,估计循环
for r=2:N
    yk=y(r);
     xk1=A*xk;
    pkn=A*pk*A'+Qk;%P'
    Hk=pkn*C'*inv(C*pkn*C'+R);%H
    pk=(I-Hk*C)*pkn;%P
    xk=xk1+Hk*(yk-C*xk1);%xk
    xk_s(r)=xk(1,1);
    Perrn(r)=pk(1,1);
end
e_x=0;
eq_x=0;
e_x1=N:1;
%%%%%%%%%%%%%%%%%%%%%%%作图
t=1:N;
figure(1);
plot(t,x,'r-',t,y,'g:',t,xk_s,'b-.');
legend('真实轨迹','观测样本','估计轨迹'); 
figure(2);
plot(t, Perrn);legend('估计值与真实值的均方误差');