卡尔曼滤波基本方程

TheWorld

%Title卡尔曼滤波基本方程
2 Z' g; S" O2 i7 q%A1初始化

%A1.1参数初始化
4 G/ Q& l" c* X# }+ z% B; BN=200;

Xa=zeros(N,1);%预估值
, ]8 p7 B; G* O8 N7 IX=zeros(N,1);%当前时刻最优估计值
) p% h+ G+ c( W3 jPa=zeros(N,1);%Xa的方差矩阵
' D- ~# Y7 e# H9 `4 rP=zeros(N,1);%X的方差矩阵
! R- r: i8 z& BKg=ones(N,1);%卡尔曼增益

Z=zeros(N,1);%观测值

W=randn(N,1);%过程噪声
! @1 z8 n4 r9 L1 ~( e% y( s% wstdW=std(W);%标准差
  d! D7 m: r! [  F3 D; t( h) v' DvarW=stdW.^2;%方差
0 W) c8 q8 ?+ c  w4 FV=0.5*randn(N,1);%观测噪声
: B+ b2 ~# a  O, e3 L' D8 PstdV=std(V);%标准差
3 g! A" {7 A: o) R6 f& YvarV=stdV.^2;%方差

A=1;
- i  Z/ B' m' {) M4 eH=1;
8 C; ^2 Q) P/ E- k, J7 }
%A1.2状态初始化
X(1)=23;%初始估计温度
0 v/ f6 l" k# s" P4 I+ NP(1)=10;%初始估计方差
Z(1)=25+V(1);
Xa(1)=23+W(1);

%卡尔曼滤波
for k=2:N
, X2 A5 u+ Z* F3 J4 t    %预估
4 u0 I( H) _, ?    Xa(k)=A*X(k-1);
    Pa(k)=A*P(k-1)*A'+varW;
* ]& \4 w2 B- R. I+ d2 `# i
    %校正
    Kg(k)=Pa(k)*H'/(H*Pa(k)*H'+varV);
    Z(k)=25+V(k);%25是真实温度值
  p! J5 N- q! s7 L3 G    X(k)=Xa(k)+Kg(k)*(Z(k)-H*Xa(k));
    P(k)=(1-Kg(k)*H)*Pa(k);
end
% l, E+ _, \- t
t=1:N;
2 {* @9 a/ V) m0 ?  J6 i- zrealVal=25*ones(N,1);

plot(t,realVal,'y',t,Z,'c',t,Xa,'b',t,X,'r');
axis([0 N 20 30]);
legend('真实值','观测值','预估值','估计值');
grid on;
) t3 r, `7 n+ w$ H" ?! |
' W6 M3 }: G1 ititle('Kalman Filter');

%realVal真实值；Z观测值；Xa预估值；X最优估计值；W过程噪声；V观测噪声

IC老和尚

很好，看看。