卡尔曼滤波基本方程

TheWorld

%Title卡尔曼滤波基本方程
%A1初始化
- j# T. t5 ^+ j! H: M
%A1.1参数初始化
N=200;
+ \9 o2 a6 g( u3 j! J* V$ z
Xa=zeros(N,1);%预估值
X=zeros(N,1);%当前时刻最优估计值
7 H# T3 @/ Z! q- V! ~( gPa=zeros(N,1);%Xa的方差矩阵
2 K# ~( l4 v4 ?" z# d& JP=zeros(N,1);%X的方差矩阵
7 V* S& C! [) o" ?2 {Kg=ones(N,1);%卡尔曼增益
: j% U% |( T- {0 E8 e
Z=zeros(N,1);%观测值
$ l* p! O0 m$ b4 A3 B
W=randn(N,1);%过程噪声
/ o$ D+ j) ^9 [/ tstdW=std(W);%标准差
. S, @% [, q" x. ?( {varW=stdW.^2;%方差
" r2 K8 `* k9 o( |; J2 S' VV=0.5*randn(N,1);%观测噪声
stdV=std(V);%标准差
varV=stdV.^2;%方差

A=1;
H=1;

%A1.2状态初始化
2 `5 z" i6 I% {. DX(1)=23;%初始估计温度
8 f3 \! V! ~+ M) }" ^( w# RP(1)=10;%初始估计方差
Z(1)=25+V(1);
; Z  ^; L& g5 u, C7 k' NXa(1)=23+W(1);

( l9 V, f8 `6 U. K+ I  d%卡尔曼滤波
for k=2:N
% K! T. ]) E1 L. f7 y' |    %预估
. x" }8 l8 L. p+ ~    Xa(k)=A*X(k-1);
+ Y$ ?+ F9 S( w8 y" {    Pa(k)=A*P(k-1)*A'+varW;
" E$ }3 u6 I0 ~% {( D5 ?
    %校正
    Kg(k)=Pa(k)*H'/(H*Pa(k)*H'+varV);
    Z(k)=25+V(k);%25是真实温度值
    X(k)=Xa(k)+Kg(k)*(Z(k)-H*Xa(k));
% ?$ p& V6 M  Q" E! h- r    P(k)=(1-Kg(k)*H)*Pa(k);
end
' m2 c! U/ n1 c+ D9 c) c, `# }* X2 i
t=1:N;
  B9 Q0 \% N9 ~0 u$ s& C, zrealVal=25*ones(N,1);
/ n  [' X2 q- W
plot(t,realVal,'y',t,Z,'c',t,Xa,'b',t,X,'r');
axis([0 N 20 30]);
legend('真实值','观测值','预估值','估计值');
grid on;

+ [; X; ]! `! p# Ttitle('Kalman Filter');
& E" r6 `' \# }/ g+ L1 l
%realVal真实值；Z观测值；Xa预估值；X最优估计值；W过程噪声；V观测噪声

) X6 x" @. A7 _9 O) o

IC老和尚

很好，看看。