卡尔曼滤波

clc;clear all; close all;
%%
%---------------卡尔曼滤波-----------------
%-----说明
%X(k+1)=Ak*X(k)+W(k);
%Y(k)=Ck*X(k)+V(k)
%%
clear;clc;
%基本参数值
Ak=exp(-0.02);Ck=1;
Qk=1-exp(-0.04);Rk=1;
%初始值设置
X0=0;P0=1;
%观测值y(k)
Y=[-3.2 -0.8 -14 -16 -17 -18 -3.3 -2.4 -18 -0.3 -0.4 -0.8 -19 -2.0 -1.2 ...
-11 -14 -0.9 0.8 10 0.2 0.5 2.4 -0.5 0.5 -13 0.5 10 -12 0.5 -0.6 ...
-15 -0.7 15 ...
0.5 -0.7 -2.0 -19 -17 -11 -14];
%数据长度
N=length(Y);
for k=1:N
if k==1 %k=1 时由初值开始计算
P_(k)=Ak*P0*Ak'+Qk;
H(k)=P_(k)*Ck'*inv(Ck*P_(k)*Ck'+Rk);
X(k)=Ak*X0+H(k)*(Y(k)-Ck*Ak*X0);
I=eye(size(H(k)));
P(k)=(I-H(k)*Ck)*P_(k);
else%k>1 时，开始递推
%递推公式
P_(k)=Ak*P(k-1)*Ak'+Qk;
H(k)=P_(k)*Ck'*inv(Ck*P_(k)*Ck'+Rk);
X(k)=Ak*X(k-1)+H(k)*(Y(k)-Ck*Ak*X(k-1));
I=eye(size(H(k)));
P(k)=(I-H(k)*Ck)*P_(k);
end
end
M=1:N;T=0.02*M;
%作图，画出x(t)的波形
subplot(3,1,1);

plot(T,Y,'r','LineWidth',1);
xlabel('t');ylabel('y(t)');
title('卡尔曼滤波-测量信号y(t)波形');
grid;
%figure(2)
subplot(3,1,2);
plot(T,X,'b','LineWidth',1);
xlabel('t');ylabel('x(t)');
title('卡尔曼滤波-估计信号x(t)波形');
grid;
%figure(3)
subplot(3,1,3);
plot(T,X,'b', T,Y,'r','LineWidth',2);
xlabel('t');ylabel('x(t)');
legend('估计信号x(t)','测量信号Z(t)');
grid;

N=200;
w(1)=0;
x(1)=5;
a=1;
c=1;
%高斯白噪声
Q1 = randn(1,N)*1;%过程噪声
Q2 = randn(1,N);%测量噪声
%卡尔曼滤波的5 个方程
%x(k+1)=x(k)+w(k)

for k=2:N;x(k)=a*x(k-1)+Q1(k-1); end%状态矩阵
%z(k)=x(k)+v(k)
for k=1:N;Y(k)=c*x(k)+Q2(k);end
p(1)=0;
s(1)=1;
for t=2:N;
Rww=cov(Q1(1:t)); % covariance matrix 协方差矩阵
Rvv=cov(Q2(1:t));
%P(k+1|k)=P(k|k)+Q
p1(t)=a.^2*p(t-1)+Rww;
%Kg=P(k+1|k)/[P(k+1|k)+R]
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);%kalman 增益

%Kg=P(k+1|k)/[P(k+1|k)+R]
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);%kalman 增益
%x(k+1|k+1)=x(k+1|k)+Kg[z(k+1)-x(k+1|k)] 输出的即为经过卡尔曼滤波后的数据


s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
%P(k+1|k+1)=(I-Kg)P(k+1}k)
p(t)=p1(t)-c*b(t)*p1(t);
end
t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');%红色卡尔曼，绿色观测值，蓝色状态值
legend('kalman estimate','ovservations','truth');