卡尔曼滤波基本方程

TheWorld

%Title卡尔曼滤波基本方程
+ z$ Z7 P8 D- V. W# Z( U%A1初始化
! j7 g1 O( t( D/ y# J. T
%A1.1参数初始化
N=200;
& V! h  d% e# Z4 ]
- I7 z) S. U- M2 K* dXa=zeros(N,1);%预估值
X=zeros(N,1);%当前时刻最优估计值
3 n% b0 }5 l% L4 {+ ^1 i5 a7 N$ xPa=zeros(N,1);%Xa的方差矩阵
. V8 K0 {. I3 m8 D1 E& HP=zeros(N,1);%X的方差矩阵
- d, o5 Y. ^  W& W4 }  uKg=ones(N,1);%卡尔曼增益

  m, k. c' p+ o! dZ=zeros(N,1);%观测值
& K: C. P. T! h0 k5 w" N  d
8 {5 v! b* W; l- u9 M5 BW=randn(N,1);%过程噪声
stdW=std(W);%标准差
varW=stdW.^2;%方差
V=0.5*randn(N,1);%观测噪声
stdV=std(V);%标准差
9 i5 T) p$ _4 e$ I$ w% s! |. u$ \varV=stdV.^2;%方差
: A, ~: @! M2 v
9 V- v; [1 b- I# mA=1;
) o8 Z% T* O- nH=1;
8 P8 y# o0 _' K7 u/ [7 F
%A1.2状态初始化
X(1)=23;%初始估计温度
P(1)=10;%初始估计方差
Z(1)=25+V(1);
* d8 U& m3 O5 m2 G1 [Xa(1)=23+W(1);
9 y/ Q* ~, @, J! X9 k7 T) j; X
* V. J$ o' ?* k- Y1 d%卡尔曼滤波
2 b: x7 [6 Z4 ifor k=2:N
    %预估
% N  ?2 g, t2 }2 K" F! o' ]& n3 x    Xa(k)=A*X(k-1);
    Pa(k)=A*P(k-1)*A'+varW;
+ x$ x- F; s; ^* U3 Q# {' J7 G
    %校正
) Y5 s6 a& e9 F! A2 s/ W    Kg(k)=Pa(k)*H'/(H*Pa(k)*H'+varV);
    Z(k)=25+V(k);%25是真实温度值
3 @9 i2 m4 B5 J& B) u    X(k)=Xa(k)+Kg(k)*(Z(k)-H*Xa(k));
* z& {/ W1 r2 f3 r& h0 I    P(k)=(1-Kg(k)*H)*Pa(k);
end

, u% q/ r) p, Z( D& ]; s) o9 H' c# bt=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]);
6 b& k- G; j* q& Y5 @$ F! V/ Zlegend('真实值','观测值','预估值','估计值');
grid on;

! k5 G! f" M; Q1 Btitle('Kalman Filter');

%realVal真实值；Z观测值；Xa预估值；X最优估计值；W过程噪声；V观测噪声
# k9 I+ R% F/ k9 \. Y

IC老和尚

很好，看看。