卡尔曼滤波基本方程

%Title卡尔曼滤波基本方程
%A1初始化

%A1.1参数初始化
N=200;

Xa=zeros(N,1);%预估值
X=zeros(N,1);%当前时刻最优估计值
Pa=zeros(N,1);%Xa的方差矩阵
P=zeros(N,1);%X的方差矩阵
Kg=ones(N,1);%卡尔曼增益

Z=zeros(N,1);%观测值

W=randn(N,1);%过程噪声
stdW=std(W);%标准差
varW=stdW.^2;%方差
V=0.5*randn(N,1);%观测噪声
stdV=std(V);%标准差
varV=stdV.^2;%方差

A=1;
H=1;

%A1.2状态初始化
X(1)=23;%初始估计温度
P(1)=10;%初始估计方差
Z(1)=25+V(1);
Xa(1)=23+W(1);

%卡尔曼滤波
for k=2:N
%预估
Xa(k)=A*X(k-1);
Pa(k)=A*P(k-1)*A'+varW;

%校正
Kg(k)=Pa(k)*H'/(H*Pa(k)*H'+varV);
Z(k)=25+V(k);%25是真实温度值
X(k)=Xa(k)+Kg(k)*(Z(k)-H*Xa(k));
P(k)=(1-Kg(k)*H)*Pa(k);
end

t=1:N;
realVal=25*ones(N,1);

plot(t,realVal,'y',t,Z,'c',t,Xa,'b',t,X,'r');
axis([0 N 20 30]);
legend('真实值','观测值','预估值','估计值');
grid on;

title('Kalman Filter');

%realVal真实值；Z观测值；Xa预估值；X最优估计值；W过程噪声；V观测噪声