卡尔曼滤波基本方程

TheWorld

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

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

- s: l% g. M2 F6 x  X9 k  WXa=zeros(N,1);%预估值
X=zeros(N,1);%当前时刻最优估计值
* M8 [' z1 R- J& f( TPa=zeros(N,1);%Xa的方差矩阵
P=zeros(N,1);%X的方差矩阵
Kg=ones(N,1);%卡尔曼增益

4 o9 `# L' X+ y/ B# U5 {, NZ=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;%方差

# B: q! I3 w: n$ }* _3 \A=1;
H=1;
$ S$ W8 h% }9 H, `$ A# Y) \
$ i! k5 T% a  w%A1.2状态初始化
X(1)=23;%初始估计温度
P(1)=10;%初始估计方差
Z(1)=25+V(1);
Xa(1)=23+W(1);

%卡尔曼滤波
4 l1 ^, Y1 }( Gfor k=2:N
8 }! p/ }# ^0 y, m+ x. I7 b8 k( J    %预估
  f, q( ^/ W1 v! R    Xa(k)=A*X(k-1);
    Pa(k)=A*P(k-1)*A'+varW;

( Y7 B: \! O# R- X& x    %校正
' L7 u" ~$ f- f/ ~( o( F, k    Kg(k)=Pa(k)*H'/(H*Pa(k)*H'+varV);
    Z(k)=25+V(k);%25是真实温度值
6 K4 @4 M/ S% R, c" W6 W    X(k)=Xa(k)+Kg(k)*(Z(k)-H*Xa(k));
8 w8 e: n, J. q( o; j$ ^5 ?    P(k)=(1-Kg(k)*H)*Pa(k);
end

( ?" W2 R- }$ B' u' Pt=1:N;
  R2 ~2 u* F; |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;
9 @* C# c9 S' U8 X% ^$ y
title('Kalman Filter');

%realVal真实值；Z观测值；Xa预估值；X最优估计值；W过程噪声；V观测噪声
* N5 [. K5 G  m; v- P3 L

IC老和尚

很好，看看。