kalmanpzvar2.m

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

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为四阶单位阵
%%%%%%%%%%%%%%%%%%%%%%%%%列出状态方程
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=4*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);
Ak=[1.352,-1.338,0.662,0.240;1,0,0,0;0,1,0,0;0,0,1,0];%状态变量之间的增益矩阵Ak
Ck=[1 0 0 0];%状态变量与输出信号之间的增益矩阵Ck,(一维:仅仅对x方向进行估计)
Rk=[1];%量测噪声协方差阵
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滤波开始,估计循环
for r=5:N
    yk=y(r);
    Pk1=Ak*Pk*Ak'+Qk;%(未考虑噪声)k时刻滤波的均方误差矩阵
    %Pk1=Ak*Pk*Ak';%(未考虑噪声)k时刻滤波的均方误差矩阵
    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为估计值
end
%end
e_x=0;
eq_x=0;
e_x1=N:1;
%计算滤波的均值,计算滤波误差的均值
for i=1:N
    e_x(i)=x(i)-xk_s(i); %误差=真实值-滤波估计值
end
%%%%%%%%%%%%%%%%%%%%%%%作图
t=1:N;
figure(1);
plot(t,x,'r-',t,y,'g:',t,xk_s,'b-.');
legend('真实轨迹','观测样本','估计轨迹'); 
figure(2);
plot(e_x);          
legend('平均误差');