卡尔曼滤波基本方程

TheWorld

%Title卡尔曼滤波基本方程
%A1初始化

%A1.1参数初始化
# U# d0 x$ d( P2 O! g- }N=200;
) P+ Y5 j' G2 h; N! |
Xa=zeros(N,1);%预估值
X=zeros(N,1);%当前时刻最优估计值
Pa=zeros(N,1);%Xa的方差矩阵
+ \; L: V7 Y2 G9 a0 ZP=zeros(N,1);%X的方差矩阵
Kg=ones(N,1);%卡尔曼增益
- {$ |7 t5 k3 u' f' y  A& \) I# c
1 u0 j" v8 M! ?* U8 CZ=zeros(N,1);%观测值

W=randn(N,1);%过程噪声
- t3 }0 \4 J' w  dstdW=std(W);%标准差
2 u2 _( L7 H2 h/ @& ]0 t. v' p1 evarW=stdW.^2;%方差
V=0.5*randn(N,1);%观测噪声
+ Q/ F4 _6 k( p6 A- V/ ?stdV=std(V);%标准差
5 x" P$ e( M  y5 `  xvarV=stdV.^2;%方差

A=1;
H=1;

%A1.2状态初始化
7 S( L6 T. D0 E  @3 rX(1)=23;%初始估计温度
P(1)=10;%初始估计方差
6 l% y8 k4 R9 }) B, ?Z(1)=25+V(1);
* D/ G* s9 J, X! \Xa(1)=23+W(1);

%卡尔曼滤波
for k=2:N
$ y, ]. Q" D/ k8 J/ V( [2 a    %预估
5 H  r1 B- t+ L7 i" A9 j    Xa(k)=A*X(k-1);
& \( m0 {( X1 U' c7 E    Pa(k)=A*P(k-1)*A'+varW;
# ^/ ~. C# y& U; L8 G$ n$ u
    %校正
7 u! l7 v, R' ?2 O3 S% i/ ]    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(k)=(1-Kg(k)*H)*Pa(k);
end
+ w' p  n) B  \
9 l  @% o( A7 M* L0 d4 E) R. |8 m: h- rt=1:N;
realVal=25*ones(N,1);

4 {7 u8 G" x& s. N4 wplot(t,realVal,'y',t,Z,'c',t,Xa,'b',t,X,'r');
% m. D7 K, R' P& ?" maxis([0 N 20 30]);
legend('真实值','观测值','预估值','估计值');
- E' k7 O  U3 i2 n1 _/ Ugrid on;
' Q# w: X! r4 B6 {+ d3 R) r# r- V
title('Kalman Filter');

%realVal真实值；Z观测值；Xa预估值；X最优估计值；W过程噪声；V观测噪声
7 L3 T6 x0 Q9 T9 D' t

IC老和尚

很好，看看。