kalman.m

研究生时的现代信号处理作业

clear;
clc;
N=100;
v=randn(1,N);
a1=-1.352;a2=1.338;a3=-0.662;a4=0.240;
x(1)=v(1);
x(2)=-a1*x(1)+v(2);
x(3)=-a1*x(2)-a2*x(1)+v(3);
x(4)=-a1*x(3)-a2*x(2)-a3*x(1)+v(4);
for n=1:N-4;
    x(n+4)=-a1*x(n+3)-a2*x(n+2)-a3*x(n+1)-a4*x(n)+v(n+4);   %产生真实数据
end
v2=randn(1,N);%观测噪声
z=x+v2;%产生观测数据
%卡尔曼滤波  
%赋初值
A=[-a1 -a2 -a3 -a4;1 0 0 0;0 1 0 0;0 0 1 0];%状态转移矩阵 
H=[1 0 0 0];%观测矩阵
Q=[1 0 0 0;0 0 0 0;0 0 0 0;0 0 0 0];%状态噪声方差阵
R=1;%观测噪声方差阵
X(:,1)=[z(4);z(3);z(2);z(1)];
p(:,:,1)=[10 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];%一步预测误差方差阵
%开始滤波
for k=2:N
    p1(:,:,k)=A*p(:,:,k-1)*A'+Q;      %p1(:,:,k)即是一步预测误差的自相关矩阵，它是4*4的矩阵，取不同的k值就构成了一个三维矩阵
    K(:,k)=p1(:,:,k)*H'/(H*p1(:,:,k)*H'+R); %K(:,:,k)是增益矩阵,对于固定的k值它是4*1矩阵，取不同的k值就是三维矩阵
    X(:,k)=A*X(:,k-1)+K(:,k)*[z(k)-H*A*X(:,k-1)]; %   X(:,k)是估计值,4*1矩阵
    p(:,:,k)=p1(:,:,k)-K(:,k)*H*p1(:,:,k);    %p(:,:,k)是估计误差的自相关矩阵，4*4矩阵的三维矩阵
end   %结束一次滤波
%估计值与真实值之差
error_px=X(1,:)-x;%x距离误差
%绘图
t=1:N;
figure(1);
plot(t,x,'k-',t,z,'g:',t,X(1,:),'r-.');
legend('真实轨迹','观测样本','估计轨迹');
figure(2)
plot(error_px);
%legend('估计误差');