卡尔曼滤波基本方程

TheWorld

%Title卡尔曼滤波基本方程
%A1初始化
6 ^$ C" V- i3 S# N. S/ V
%A1.1参数初始化
N=200;

8 l* v3 S% H3 ?2 b% ^Xa=zeros(N,1);%预估值
X=zeros(N,1);%当前时刻最优估计值
( s  t3 ]5 T- H4 p  lPa=zeros(N,1);%Xa的方差矩阵
P=zeros(N,1);%X的方差矩阵
Kg=ones(N,1);%卡尔曼增益
' ~: I! |# {+ \+ ^* P& O1 N) C
Z=zeros(N,1);%观测值

- z" r0 M4 E. S) k6 MW=randn(N,1);%过程噪声
stdW=std(W);%标准差
varW=stdW.^2;%方差
V=0.5*randn(N,1);%观测噪声
9 w! \4 j& K4 M- j  V6 wstdV=std(V);%标准差
varV=stdV.^2;%方差

A=1;
H=1;
) W( k, A4 H7 H% C- ]
' K+ z7 F4 b. y/ j( q" e%A1.2状态初始化
  j8 y2 x" t* O- b6 K7 sX(1)=23;%初始估计温度
P(1)=10;%初始估计方差
% P7 N* l$ r3 f" E/ E+ \Z(1)=25+V(1);
# ]# \  q. P$ C4 p: rXa(1)=23+W(1);
; L* K# e' x2 W: B
' k8 X' ?& z5 s8 H3 q" A* |* I%卡尔曼滤波
& ^9 l' l& H' b& Y7 c: qfor k=2:N
8 N9 F7 Z; Z: K/ v% c    %预估
" ^# `5 [" k# v& Q! B, l    Xa(k)=A*X(k-1);
    Pa(k)=A*P(k-1)*A'+varW;

    %校正
9 Q$ a$ ]. k! M    Kg(k)=Pa(k)*H'/(H*Pa(k)*H'+varV);
    Z(k)=25+V(k);%25是真实温度值
. k& |1 }" E; L8 b    X(k)=Xa(k)+Kg(k)*(Z(k)-H*Xa(k));
    P(k)=(1-Kg(k)*H)*Pa(k);
; Q& ~) w( p, ^( u( ?/ r) D; eend

4 I( z- s: b/ L$ L; I- Nt=1:N;
6 B- D- w( s2 q' ZrealVal=25*ones(N,1);

' R& ?: r- F! w* g: yplot(t,realVal,'y',t,Z,'c',t,Xa,'b',t,X,'r');
. j" W' t: t/ kaxis([0 N 20 30]);
! Q9 v- i0 w/ d' `. A! D( }legend('真实值','观测值','预估值','估计值');
  M( [) Z1 h/ ~" B/ I2 o. Ugrid on;

1 s+ ^8 y  v2 r5 J6 Ltitle('Kalman Filter');

% U. I$ Z9 C" W%realVal真实值；Z观测值；Xa预估值；X最优估计值；W过程噪声；V观测噪声
8 n, |- L' A  a( X. U1 _

IC老和尚

很好，看看。