EKF matlab

卡尔曼是真的强，我只能这么说，前面说了卡尔曼滤波，它是针对线性系统的滤波方法。但在实际工作中，我们的系统几乎都是非线性的，所以如何解决非线性系统的滤波问题呢，这就是我们要讲的EKF（扩展卡尔曼滤波），它的原理很简单，就是在估计状态的地方进行线性化，线性化的方法也很简单，只需要进行泰勒的一阶展开就行。当然我们也要说一下缺点，因为我们选择的线性化方法，所以只能达到二阶的精度（UKF可以达到四阶的精度），所以要求我们的系统是弱线性化的。其实UKF也是对非线性系统的卡尔曼滤波，主要本人对其线性化方法（UT变换）不是很理解，等考完试再说。
 

首先，系统的状态方程和观测方程如下：


我们知道，在更新误差协方差矩阵的时候，不能直接用f和h的，所以我们要进行泰勒展开，也就是要求雅克比矩阵。再利用线性情况下的卡尔曼滤波进行计算更新。
预测：


利用雅克比矩阵进行更新模型：



更新：





下面我会展示一个目标追踪的EKF例子：

12/05/16 01:22:15 /home/fc/桌面/EKF/EKF.m

1  % 扩展Kalman滤波在目标跟踪中的应用   2  % functionEKF_For_TargetTracking   3  clc ; clear ;   4  T = 1 ; % 雷达扫描周期   5  N = 60 / T ; % 总的采样次数   6  X = zeros ( 4 , N ) ; % 目标真实位置、速度   7  X ( : , 1 ) = [ - 100 , 2 , 200 , 20 ] ; % 目标初始位置、速度   8  Z = zeros ( 1 , N ) ; % 传感器对位置的观测   9  delta_w = 1e-3 ; % 如果增大这个参数，目标的真实轨迹就是曲线了  10  Q = delta_w * diag ( [ 0.5 , 1 ] ) ; % 过程噪声方差  11  G = [ T ^ 2 / 2 , 0 ; T , 0 ; 0 , T ^ 2 / 2 ; 0 , T ] ; % 过程噪声驱动矩阵  12  R = 5 ; % 观测噪声方差  13  F = [ 1 , T , 0 , 0 ; 0 , 1 , 0 , 0 ; 0 , 0 , 1 , T ; 0 , 0 , 0 , 1 ] ; % 状态转移矩阵  14  x0 = 200 ; % 观测站的位置  15  y0 = 300 ;  16  Xstation = [ x0 , y0 ] ;  17  for t = 2 : N  18      X ( : , t ) = F * X ( : , t - 1 ) + G * sqrtm ( Q ) * randn ( 2 , 1 ) ; % 目标真实轨迹  19  end  20     21  for t = 1 : N  22      Z ( t ) = Dist ( X ( : , t ) , Xstation ) + sqrtm ( R ) * randn ; % 观测值  23  end  24  % EKF  25  Xekf = zeros ( 4 , N ) ;  26  Xekf ( : , 1 ) = X ( : , 1 ) ; % Kalman滤波的状态初始化  27  P0 = eye ( 4 ) ; % 误差协方差矩阵的初始化  28  for i = 2 : N  29     Xn = F * Xekf ( : , i - 1 ) ; % 一步预测  30     P1 = F * P0 * F ' +G*Q*G ' ; % 预测误差协方差  31     dd = Dist ( Xn , Xstation ) ; % 观测预测  32      % 求解雅克比矩阵H  33     H = [ ( Xn ( 1 , 1 ) - x0 ) / dd , 0 , ( Xn ( 3 , 1 ) - y0 ) / dd , 0 ] ; % 泰勒展开的一阶近似  34     K = P1 * H ' *inv(H*P1*H ' + R ) ; % 卡尔曼最优增益  35      Xekf ( : , i ) = Xn + K * ( Z ( : , i ) - dd ) ; % 状态更新  36     P0 = ( eye ( 4 ) - K * H ) * P1 ; % 滤波误差协方差更新  37  end  38  % 误差分析  39  for i = 1 : N  40      Err_KalmanFilter ( i ) = Dist ( X ( : , i ) , Xekf ( : , i ) ) ; %  41  end  42  % 画图  43  figure  44  hold on ; boxon ;  45  plot ( X ( 1 , : ) , X ( 3 , : ) , ' -k ' ) ; % 真实轨迹  46  plot ( Xekf ( 1 , : ) , Xekf ( 3 , : ) , ' -r ' ) ; % 扩展Kalman滤波轨迹  47  legend ( ' real trajectory ' , ' EKFtrajectory ' ) ;  48  xlabel ( ' X-axis X/m ' ) ;  49  ylabel ( ' Y-axisY/m ' ) ;  50     51  figure  52  hold on ; boxon ;  53  plot ( Err_KalmanFilter , ' -ks ' , ' MarkerFace ' , ' r ' )  54  xlabel ( ' Time/s ' ) ;  55  ylabel ( ' Positionestimation bias  /m ' ) ;  56     57  % 子函数求欧氏距离  58  function d = Dist ( X1 , X2 ) ;  59  if length ( X2 ) <= 2  60     d = sqrt ( ( X1 ( 1 ) - X2 ( 1 ) ) ^ 2 + ( X1 ( 3 ) - X2 ( 2 ) ) ^ 2 ) ;  61  else  62     d = sqrt ( ( X1 ( 1 ) - X2 ( 1 ) ) ^ 2 + ( X1 ( 3 ) - X2 ( 3 ) ) ^ 2 ) ;  63  end  64     65  % 子函数求欧氏距离  66  function d = Dist ( X1 , X2 ) ;  67  if length ( X2 ) <= 2  68     d = sqrt ( ( X1 ( 1 ) - X2 ( 1 ) ) ^ 2 + ( X1 ( 3 ) - X2 ( 2 ) ) ^ 2 ) ;  69  else  70     d = sqrt ( ( X1 ( 1 ) - X2 ( 1 ) ) ^ 2 + ( X1 ( 3 ) - X2 ( 3 ) ) ^ 2 ) ;  71  end  72     73     74     75     76     77     78

结果如下：