卡尔曼滤波基本方程

TheWorld

%Title卡尔曼滤波基本方程
) T$ u2 j( g9 S, f( g8 i2 Q/ w3 ?%A1初始化
7 ~+ K" g# K/ m$ s
%A1.1参数初始化
N=200;
6 r. P* \( c1 ~, r6 g$ Z4 e- V
  }) f$ n) U8 q# ^Xa=zeros(N,1);%预估值
; x5 z2 J  d4 O  J1 b# CX=zeros(N,1);%当前时刻最优估计值
Pa=zeros(N,1);%Xa的方差矩阵
P=zeros(N,1);%X的方差矩阵
0 F! a, d  p  RKg=ones(N,1);%卡尔曼增益

- b. G; D$ w  ~1 @8 eZ=zeros(N,1);%观测值

. ^! j0 U( z7 a& T4 ]+ O9 T# IW=randn(N,1);%过程噪声
% |; b; j. o8 F. V" ]6 \! sstdW=std(W);%标准差
varW=stdW.^2;%方差
V=0.5*randn(N,1);%观测噪声
stdV=std(V);%标准差
varV=stdV.^2;%方差
6 {* r/ v3 o& M, @) M- M
A=1;
H=1;

%A1.2状态初始化
X(1)=23;%初始估计温度
2 R8 ]2 `0 ~; dP(1)=10;%初始估计方差
Z(1)=25+V(1);
! |# V, C( q0 l7 iXa(1)=23+W(1);
* h. u0 m  `9 }, s- b, T
$ s* f* T3 X, L8 N%卡尔曼滤波
for k=2:N
# @& }5 h$ n$ P    %预估
    Xa(k)=A*X(k-1);
. I2 }" a' R) T: ~    Pa(k)=A*P(k-1)*A'+varW;

6 t, i) T. ^: l    %校正
    Kg(k)=Pa(k)*H'/(H*Pa(k)*H'+varV);
0 s" o- ?8 H1 W* Z, L$ N5 {& o    Z(k)=25+V(k);%25是真实温度值
) ^8 e& z5 x. E$ B- M  P    X(k)=Xa(k)+Kg(k)*(Z(k)-H*Xa(k));
9 R* ~# b5 W: C5 S) u    P(k)=(1-Kg(k)*H)*Pa(k);
: E3 S  Q' n+ w, ]9 ?5 `+ x3 Lend
( R' Y+ s7 f6 h6 L
0 U+ P  @0 b- Z- y3 }" z  Nt=1:N;
realVal=25*ones(N,1);

0 }5 P0 q/ x  M1 W. P3 `1 Jplot(t,realVal,'y',t,Z,'c',t,Xa,'b',t,X,'r');
axis([0 N 20 30]);
. V' S/ S# t4 C  glegend('真实值','观测值','预估值','估计值');
grid on;
* @" U! y% U& R' x2 C
title('Kalman Filter');

%realVal真实值；Z观测值；Xa预估值；X最优估计值；W过程噪声；V观测噪声
% W( ]8 ?- x( Q+ L! @4 J
5 a" @0 M- i2 G+ a) H% X

IC老和尚

很好，看看。