gxfkalman.m

今天好心情

clear
%初始化
x0=[10;1]; % x(0|0), 0时刻估计值,也是0时刻的状态值x(0)
p0=[100,0;0,10];% p(0|0), 0时刻的滤波估计误差协方差阵

Fai=[1,2;0,1];  %转移阵，从一个时刻状态转移到另一个时刻的状态
Q=[1,0;0,1];    %状态过程的方差
H=[1,0];        %观测阵
R=[10];         %观测过程的方差
Gama = eye(2);  %干扰阵
I = eye(2);     %单位阵 

x1k=[];
x2k=[];
x3k=[];
x4k=[];
for k=1:100
     w = normrnd(0,10,2,1);            %系统噪声 w(k)     
     v = normrnd(0,10,1,1);            %观测噪声 v(k) 
     
  if  k ==1
      x_y1 = Fai * x0 + Gama * w;      %由状态方程得出第一时刻的状态x(1) 
      z_y1 = H * x_y1 + v;           % 第1时刻的观测值                               

     x_yc1 = Fai * x0;             %对第一时刻的状态值的预测。由于w是零均值白噪声，故在预测值中噪声部分是0 
     p_yc1 = Fai * p0 * Fai' + Gama * Q * Gama';  %对第一时刻的预测误差协方差的预测
     z_yc1 = H * x_yc1;           %由第1时刻的状态预测值通过观测阵得到第1时刻的预测测量值
                                           %由于w是零均值白噪声，故在预测值中噪声部分是0 
      Kg1 =  p_yc1 * H' * inv( H * p_yc1 * H'+ R );      %增益阵        
      x_gj1 = x_yc1 + Kg1 * (z_y1 - z_yc1);     %通过第1时刻的观测值对第1时刻的预测状态值进行修正，得到第1时刻的估计值
      p_gj1 = (I - Kg1 * H) *  p_yc1;             %滤波估计误差协方差
      x_yk=x_y1
      x_gjk=x_gj1
      p_gjk=p_gj1
  else
    x_yk = Fai * x_yk + Gama * w;
    z_yk = H * x_yk + v; 
    x_yck = Fai * x_gjk;    %  x(k|k-1); 
    p_yck = Fai * p_gjk * Fai' + Gama * Q * Gama';      %  p(k|k-1);
    z_yck = H * x_yck;
    Kgk=p_yck * H' * inv(H * p_yck * H'+ R);
    x_gjk = x_yck + Kgk *(z_yk - z_yck);   %  x(k|k);
    p_gjk = (I - Kgk * H) * p_yck;         %  p(k|k); 
    x1k=[x1k x_yk];
    x2k=[x2k x_yck];
    x3k=[x3k x_gjk];
    x4k=[x4k p_gjk];
  end
end
 %估计值和预测值的比较
e_gj = x1k - x3k; %估计误差
e_yc = x1k - x2k; %预测误差
figure
plot(e_gj(2,:),'b-')                                                      
hold on
plot(e_yc(2,:),'r-')
legend('速度滤波估计误差','速度预测估计误差')
title('速度误差比较')
hold off

%状态值与估计值的比较
figure
hold on
plot(x1k(2,:),'b-')
plot(x3k(2,:),'r-')
legend('状态值','状态估计值')
title('状态值与估计值的比较')
hold off

%估计误差协方差
figure
plot(x4k(2,:),'b-')
title('估计误差协方差')