卡尔曼滤波基本方程

TheWorld

%Title卡尔曼滤波基本方程
( b+ Z3 Q" k5 o%A1初始化

& r) Q. t. `4 G- q3 f7 a( g) F%A1.1参数初始化
8 Q+ p; }. I4 V( Z7 x5 ON=200;
( z' P5 T/ A- U8 Q- b6 M  U
Xa=zeros(N,1);%预估值
, I5 K6 P+ J6 v6 c. rX=zeros(N,1);%当前时刻最优估计值
4 O5 x. g, K/ {  Q9 hPa=zeros(N,1);%Xa的方差矩阵
# U5 M, p' U) d# _/ B7 ~& [P=zeros(N,1);%X的方差矩阵
3 p8 g/ |, P# J/ C$ K& FKg=ones(N,1);%卡尔曼增益
( z# C/ @" ]( `8 f' l" ^
Z=zeros(N,1);%观测值

5 C; f! e9 }) X. B' }* eW=randn(N,1);%过程噪声
stdW=std(W);%标准差
8 H0 U7 F1 v0 P& ~* }5 T. mvarW=stdW.^2;%方差
: l6 l! w/ L3 [! W5 K# VV=0.5*randn(N,1);%观测噪声
stdV=std(V);%标准差
varV=stdV.^2;%方差

A=1;
H=1;
9 W* }$ k  \3 o- E2 f4 g
9 w  W- n0 F, S& z8 {, z%A1.2状态初始化
) s4 x" L  d6 f9 YX(1)=23;%初始估计温度
' a+ K( K: x( M! E$ y/ u! _' v1 mP(1)=10;%初始估计方差
Z(1)=25+V(1);
Xa(1)=23+W(1);
+ v$ s1 N& o$ _1 q  ]+ k/ {
5 ~1 @9 D: p7 C  W. z9 w* o/ j4 M%卡尔曼滤波
; f( K  X8 `5 E4 A' n5 }% I+ tfor 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是真实温度值
, {+ R7 k1 L9 q  C2 z* Y7 O1 C    X(k)=Xa(k)+Kg(k)*(Z(k)-H*Xa(k));
    P(k)=(1-Kg(k)*H)*Pa(k);
4 t" v5 b' Q5 }end
* E) W" j) n+ N) J
1 w5 d1 L9 n" D0 l$ y( _4 Qt=1:N;
realVal=25*ones(N,1);
( [8 B1 g& j8 c
( c# S# ~! n: t4 K' M" \# Cplot(t,realVal,'y',t,Z,'c',t,Xa,'b',t,X,'r');
axis([0 N 20 30]);
legend('真实值','观测值','预估值','估计值');
5 P& K8 Q9 {- ~; l2 u+ W# x8 mgrid on;

/ O9 t1 S7 `- V# K" c, {title('Kalman Filter');
) H* J! ?9 C. d+ S/ ]8 Y
%realVal真实值；Z观测值；Xa预估值；X最优估计值；W过程噪声；V观测噪声
! N! e$ D/ ]  M1 S: o" v! B5 \1 e
" o( }+ G& X2 F$ O4 [

IC老和尚

很好，看看。