卡尔曼滤波基本方程

TheWorld

%Title卡尔曼滤波基本方程
( f( u6 D7 K# y# n: \. H%A1初始化
) t$ i! T+ u. p  c1 x7 P
  ~" A' X$ n. A& m7 e. d! I. |$ u%A1.1参数初始化
N=200;
$ W+ W' f) v) n  M
0 j" D3 j* Q( XXa=zeros(N,1);%预估值
) s% g. \$ p0 qX=zeros(N,1);%当前时刻最优估计值
Pa=zeros(N,1);%Xa的方差矩阵
P=zeros(N,1);%X的方差矩阵
6 x$ F$ T' |; e1 |' B2 _Kg=ones(N,1);%卡尔曼增益

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

W=randn(N,1);%过程噪声
( o1 Y( [7 [2 e$ z. d, JstdW=std(W);%标准差
$ ^4 a6 Q6 C% b1 u7 F6 Y) g$ B, |varW=stdW.^2;%方差
V=0.5*randn(N,1);%观测噪声
stdV=std(V);%标准差
' c6 v7 A% u( |) Q, O  q& |$ }varV=stdV.^2;%方差
9 ^% O/ a8 |2 k2 H! c
A=1;
; m  `' X+ N4 s3 C- d* aH=1;
, I' N, O& y( c( a$ K
%A1.2状态初始化
X(1)=23;%初始估计温度
P(1)=10;%初始估计方差
& D6 O$ Z3 _! UZ(1)=25+V(1);
% r( Z6 p) E2 f* O5 wXa(1)=23+W(1);
) P; B. E, @$ |1 v) P
%卡尔曼滤波
6 W2 r9 D' n, e% a" R4 @# _for k=2:N
    %预估
3 c) K* Y& a  b7 h, W& f    Xa(k)=A*X(k-1);
    Pa(k)=A*P(k-1)*A'+varW;
: v' B) _' V) D+ Y0 Y
    %校正
    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);
+ L' r7 G/ x8 m2 c3 _end

9 e4 O+ I( g; ?+ j6 a# Xt=1:N;
realVal=25*ones(N,1);
$ ~% \" ]7 y9 R9 T* z# w8 a
plot(t,realVal,'y',t,Z,'c',t,Xa,'b',t,X,'r');
axis([0 N 20 30]);
legend('真实值','观测值','预估值','估计值');
grid on;
1 D( |8 f: Z. U- i
title('Kalman Filter');
' H, _0 Y2 k. t# u! ~! Y
& N. N- R; f; h/ o; j" A: ]2 |, `%realVal真实值；Z观测值；Xa预估值；X最优估计值；W过程噪声；V观测噪声
% o, M3 G2 S7 h' H

IC老和尚

很好，看看。