卡尔曼滤波基本方程

TheWorld

%Title卡尔曼滤波基本方程
%A1初始化
; z. o; m% a) Y, E- ~' L, ~, C, d
9 h7 H! I4 V8 h2 R8 w9 j%A1.1参数初始化
. _  ^" x, i2 ~2 q6 U# TN=200;
# x/ R. |5 d5 q9 U
Xa=zeros(N,1);%预估值
X=zeros(N,1);%当前时刻最优估计值
" w% Q6 \! ]1 v+ q8 t6 H; TPa=zeros(N,1);%Xa的方差矩阵
* \0 e' i( C! c, q6 t: i! QP=zeros(N,1);%X的方差矩阵
( _7 ~+ d( D) k  P, h, bKg=ones(N,1);%卡尔曼增益
$ t- e; C# ?% U; \4 |; j6 B6 E7 F
0 t' B& P- Y  Q1 N& u* jZ=zeros(N,1);%观测值
- l  H6 G. }4 `' S% _4 S9 o" S
, d9 G) j$ {7 n3 RW=randn(N,1);%过程噪声
1 i$ \) X. w8 \* BstdW=std(W);%标准差
varW=stdW.^2;%方差
& ]$ ^4 d# t: w" w$ X* yV=0.5*randn(N,1);%观测噪声
9 G) ?# ?( S% Z; @) qstdV=std(V);%标准差
. ^/ {. y7 ^# ]+ p' PvarV=stdV.^2;%方差
, I+ B% g# C4 w( }  w* P% x
$ g& C- K. `3 e$ j$ I" iA=1;
/ }" ?$ z$ ]. q8 A- K4 SH=1;

%A1.2状态初始化
X(1)=23;%初始估计温度
P(1)=10;%初始估计方差
Z(1)=25+V(1);
0 D+ M/ g4 q0 [, L/ j! dXa(1)=23+W(1);

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

" \/ {' p  j; p% Z8 q* R    %校正
    Kg(k)=Pa(k)*H'/(H*Pa(k)*H'+varV);
0 C* j0 r, q; s' P    Z(k)=25+V(k);%25是真实温度值
3 G9 E' p- C8 B5 \1 _6 _' S9 x1 G    X(k)=Xa(k)+Kg(k)*(Z(k)-H*Xa(k));
3 [( K4 c9 G7 I3 f* F/ Q4 X    P(k)=(1-Kg(k)*H)*Pa(k);
end

, c' p, j) ~+ W' F* R# B5 C4 c2 V' N/ Dt=1:N;
realVal=25*ones(N,1);

plot(t,realVal,'y',t,Z,'c',t,Xa,'b',t,X,'r');
4 q1 D( d5 m3 {2 d# z, J# Vaxis([0 N 20 30]);
legend('真实值','观测值','预估值','估计值');
grid on;
0 t1 d8 Z, u0 H6 X- T
5 o& H1 l# Y& }: |/ R; Ctitle('Kalman Filter');

3 j# [8 N2 S$ D# a. H/ S( c: h%realVal真实值；Z观测值；Xa预估值；X最优估计值；W过程噪声；V观测噪声

IC老和尚

很好，看看。