扩展卡尔曼滤波

太过爱你忘了你带给我的痛 2022-10-22 07:56 253阅读 0赞

扩展卡尔曼滤波（Extended Kalman Filter，EKF）是标准卡尔曼滤波在非线性情形下的一种扩展形式，**EKF算法是将非线性函数进行泰勒展开，省略高阶项，保留展开项的一阶项，以此来实现非线性函数线性化，最后通过卡尔曼滤波算法近似计算系统的状态估计值和方差估计值,**对信号进行滤波。

### 泰勒级数展开 ###

泰勒级数展开是将一个在![x=x\_\{0\}][x_x_0]处具有![n][]阶导数的函数![f(x)][f_x]，利用关于![(x-x\_\{0\})][x-x_0]的![n][]次多项式逼近函数值的方法。

若函数![f(x)][f_x]在包含![x\_\{0\}][x_0]的某个闭区间![\[a,b\]][a_b]上具有![n][]阶导数，且在开区间![(a,b)][a_b 1]上具有![(n+1)][n_1]阶导数，则对闭区间![\[a,b\]][a_b]上的任意一点![x][]，都有：

![20210405195913761.png][]

其中![\{\{f\}^\{(n)\}\}(\{\{x\}\_\{0\}\})][f_n_x_0]表示函数![f(x)][f_x]在![x=x\_\{0\}][x_x_0]处的![n][]阶导数，等式右边成为泰勒展开式，剩余的![\{\{R\}\_\{n\}\}(x)][R_n_x]是泰勒展开式的余项，是![(x-x\_\{0\})^\{n\}][x-x_0_n]的高阶无穷小。

当变量是多维向量时，一维的泰勒展开就需要做拓展，具体形式如下：

![20210405195957185.png][]

其中，![\{\{\[\\nabla f(\{\{\\mathbf\{x\}\}\_\{k\}\})\]\}^\{T\}\}=\{\{\\mathbf\{J\}\}\_\{F\}\}][nabla f_mathbf_x_k_T_mathbf_J_F]表示雅克比矩阵，![\\mathbf\{H\}(\{\{\\mathbf\{x\}\}\_\{k\}\})][mathbf_H_mathbf_x_k]表示海塞矩阵，![\{\{\\mathbf\{o\}\}^\{n\}\}][mathbf_o_n]表示高阶无穷小。

一般来说，EKF在对非线性函数做泰勒展开时，实际应用只取到一阶导，同样也能有较好的结果。取一阶导时，状态转移方程和观测方程就近似为线性方程，高斯分布的变量经过线性变换之后仍然是高斯分布，这样就能够延用标准卡尔曼滤波的框架。

### EKF ###

扩展卡尔曼滤波EKF的状态转移方程和观测方程为：

![20210405200405398.png][] ![20210405201752103.png][],

利用泰勒展开式对上式在***上一次的估计值处![<\\theta \_\{k-1\}>][theta _k-1]***展开得

![20210405200439953.png][] ,![20210405202718597.png][]

再利用泰勒展开式，在***本轮的状态预测值处![\\theta^\{'\} \_k][theta_ _k]***展开得

![20210405200500126.png][], ![20210405202901268.png][]

基于以上的公式，给出EKF的预测（Predict）和更新（Update）两个步骤：

Propagation：

![20210405200816883.png][] ![20210405202803521.png][]

Update：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMwODE1MjM3_size_16_color_FFFFFF_t_70][] ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMwODE1MjM3_size_16_color_FFFFFF_t_70 1][]

其中的雅克比矩阵和分别为

![20210405200847911.png][]**（H的导数只能在预测值处取，因为这个时候你还不知道估计值，是先有k时刻的H，然后有K，最后才有k时刻的估计值）**

雅可比矩阵的计算，在MATLAB中可以利用对自变量加上一个eps（极小数），然后用因变量的变化量去除以eps即可得到雅可比矩阵的每一个元素值。

from :[https://blog.csdn.net/weixin\_42647783/article/details/89054641][https_blog.csdn.net_weixin_42647783_article_details_89054641]

from :[https://blog.csdn.net/gangdanerya/article/details/105105611][https_blog.csdn.net_gangdanerya_article_details_105105611]

--------------------

这是书上给出的一个例子，我希望从中能归纳一种套路可以用在 大部分EKF问题中

一：建立数学模型

（一）：建立状态方程

状态方程是由具体问题的物理意义抽象出来的，不同问题具有不同的状态方程，是已知量。

（二）：建立观测方程

观测方程也是由实际物理意义抽象出来的，已知量。

（三）：一阶线性化状态方程，求状态转移矩阵F(k)。（其实就是把状态方程求偏导的过程）

（四）：一阶线性化观测方程，求解观测矩阵H(k)

接下来进入卡尔曼滤波，首先时间更新

（五）：状态预测

EKF的状态预测和KF的状态预测其实没有差别，都是假设没有过程噪声w，然后使用状态方程将上一时刻的后验估计值代入，X(k|k-1)是指用k-1时刻估计出来的k时刻的值，也即k时刻的先验估计值，同理X(k-1|k-1)就是指k-1时刻的后验估计值

（六）：观测预测（下面状态更新时要用）

说实话当时看参考书这里一直没看懂，y不应该是传感器测出来的值吗，为什么要预测？这里我的理解是，“预测”指的是第5步的预测，这里只是把预测结果转化到测量度量下。

![20210407200546953.png][]

（七）：求协方差预测，EKF的协方差预测和KF也是基本没差别，只不过是F（k）需要用偏导先求出来

![20210405202803521.png][]

接下来进入状态更新

（八）：求卡尔曼滤波增益

（九）：求状态后验估计值

![20210407200756687.png][]

（十）：协方差更新

至此，卡尔曼滤波需要的所有方程推导完毕，从（三）到（十）为一次EKF迭代

from：[https://blog.csdn.net/buaazyp/article/details/79814195][https_blog.csdn.net_buaazyp_article_details_79814195]

--------------------

雅克比矩阵计算参考[https://blog.csdn.net/O\_MMMM\_O/article/details/106078679?utm\_medium=distribute.pc\_relevant.none-task-blog-baidujs\_title-2&spm=1001.2101.3001.4242][https_blog.csdn.net_O_MMMM_O_article_details_106078679_utm_medium_distribute.pc_relevant.none-task-blog-baidujs_title-2_spm_1001.2101.3001.4242]（有空记得看这篇博文）

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMwODE1MjM3_size_16_color_FFFFFF_t_70 2][]

### matlab示例： ###

% author :  Perry.Li  @USTC
    % function: simulating the process of EKF
    % date:     04/28/2015
    % 
    N = 50;         %计算连续N个时刻 
    n=3;            %状态维度
    q=0.1;          %过程标准差
    r=0.2;          %测量标准差
    Q=q^2*eye(n);   %过程方差
    R=r^2;          %测量值的方差 
    f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];  %状态方程
    h=@(x)[x(1);x(2);x(3)];                   %测量方程
    s=[0;0;1];                                %初始状态
    %初始化状态
    x=s+q*randn(3,1);                         
    P = eye(n);                               
    xV = zeros(n,N);          
    sV = zeros(n,N);         
    zV = zeros(n,N);
    for k=1:N
      z = h(s) + r*randn;                     
      sV(:,k)= s;                             %实际状态
      zV(:,k)  = z;                           %状态测量值
      [x1,A]=jaccsd(f,x); %计算f的雅可比矩阵，其中x1对应黄金公式line2
      P=A*P*A'+Q;         %过程方差预测，对应line3
      [z1,H]=jaccsd(h,x1); %计算h的雅可比矩阵
      K=P*H'*inv(H*P*H'+R); %卡尔曼增益，对应line4
      x=x1+K*(z-z1);        %状态EKF估计值，对应line5
      P=P-K*H*P;            %EKF方差，对应line6
      xV(:,k) = x;          %save
      s = f(s) + q*randn(3,1);  %update process 
    end
    for k=1:3
      FontSize=14;
      LineWidth=1;
      figure();
      plot(sV(k,:),'g-'); %画出真实值
      hold on;
      plot(xV(k,:),'b-','LineWidth',LineWidth) %画出最优估计值
      hold on;
      plot(zV(k,:),'k+'); %画出状态测量值
      hold on;
      legend('真实状态', 'EKF最优估计估计值','状态测量值');
      xl=xlabel('时间(分钟)');
      t=['状态 ',num2str(k)] ;
      yl=ylabel(t);
      set(xl,'fontsize',FontSize);
      set(yl,'fontsize',FontSize);
      hold off;
      set(gca,'FontSize',FontSize);
    end
    
    
    function [z,A]=jaccsd(fun,x)
    % JACCSD Jacobian through complex step differentiation
    % [z J] = jaccsd(f,x)
    % z = f(x)
    % J = f'(x)
    %
    z=fun(x);
    n=numel(x);
    m=numel(z);
    A=zeros(m,n);
    h=n*eps;
    for k=1:n
        x1=x;
        x1(k)=x1(k)+h*i;
        A(:,k)=imag(fun(x1))/h;
    end

from :[https://blog.csdn.net/u011598824/article/details/56282618][https_blog.csdn.net_u011598824_article_details_56282618]

[x_x_0]: https://private.codecogs.com/gif.latex?x%3Dx_%7B0%7D
[n]: https://private.codecogs.com/gif.latex?n
[f_x]: https://private.codecogs.com/gif.latex?f%28x%29
[x-x_0]: https://private.codecogs.com/gif.latex?%28x-x_%7B0%7D%29
[x_0]: https://private.codecogs.com/gif.latex?x_%7B0%7D
[a_b]: https://private.codecogs.com/gif.latex?%5Ba%2Cb%5D
[a_b 1]: https://private.codecogs.com/gif.latex?%28a%2Cb%29
[n_1]: https://private.codecogs.com/gif.latex?%28n&plus;1%29
[x]: https://private.codecogs.com/gif.latex?x
[20210405195913761.png]: /images/20221022/a289077d243041cbb75de2bc465a7600.png
[f_n_x_0]: https://private.codecogs.com/gif.latex?%7B%7Bf%7D%5E%7B%28n%29%7D%7D%28%7B%7Bx%7D_%7B0%7D%7D%29
[R_n_x]: https://private.codecogs.com/gif.latex?%7B%7BR%7D_%7Bn%7D%7D%28x%29
[x-x_0_n]: https://private.codecogs.com/gif.latex?%28x-x_%7B0%7D%29%5E%7Bn%7D
[20210405195957185.png]: /images/20221022/69966ca739ce4a14ba5c00fc406f6ef5.png
[nabla f_mathbf_x_k_T_mathbf_J_F]: https://private.codecogs.com/gif.latex?%7B%7B%5B%5Cnabla%20f%28%7B%7B%5Cmathbf%7Bx%7D%7D_%7Bk%7D%7D%29%5D%7D%5E%7BT%7D%7D%3D%7B%7B%5Cmathbf%7BJ%7D%7D_%7BF%7D%7D
[mathbf_H_mathbf_x_k]: https://private.codecogs.com/gif.latex?%5Cmathbf%7BH%7D%28%7B%7B%5Cmathbf%7Bx%7D%7D_%7Bk%7D%7D%29
[mathbf_o_n]: https://private.codecogs.com/gif.latex?%7B%7B%5Cmathbf%7Bo%7D%7D%5E%7Bn%7D%7D
[20210405200405398.png]: https://img-blog.csdnimg.cn/20210405200405398.png
[20210405201752103.png]: /images/20221022/37b231211ad74c459d689ff54be45e9e.png
[theta _k-1]: https://latex.codecogs.com/gif.latex?%3C%5Ctheta%20_%7Bk-1%7D%3E
[20210405200439953.png]: https://img-blog.csdnimg.cn/20210405200439953.png
[20210405202718597.png]: /images/20221022/ce39512bf4374cc281807f120736895c.png
[theta_ _k]: https://latex.codecogs.com/gif.latex?%5Ctheta%5E%7B%27%7D%20_k
[20210405200500126.png]: https://img-blog.csdnimg.cn/20210405200500126.png
[20210405202901268.png]: /images/20221022/2d96ae97a0d6434fa26643c9504fcfb2.png
[20210405200816883.png]: https://img-blog.csdnimg.cn/20210405200816883.png
[20210405202803521.png]: /images/20221022/4599389a1e0b4d7ab29be4c6aaad340d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMwODE1MjM3_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/202104052008331.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMwODE1MjM3,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMwODE1MjM3_size_16_color_FFFFFF_t_70 1]: /images/20221022/58341812d0464a0f83d9ec206c8360bc.png
[20210405200847911.png]: /images/20221022/250c99a0f2ad4aa8b1e0e062e93c84da.png
[https_blog.csdn.net_weixin_42647783_article_details_89054641]: https://blog.csdn.net/weixin_42647783/article/details/89054641
[https_blog.csdn.net_gangdanerya_article_details_105105611]: https://blog.csdn.net/gangdanerya/article/details/105105611
[20210407200546953.png]: /images/20221022/7fd56516bfbb4730a2ad625bb153ed28.png
[20210407200756687.png]: /images/20221022/c3b7ee47b9ee45e4800845429c1f9490.png
[https_blog.csdn.net_buaazyp_article_details_79814195]: https://blog.csdn.net/buaazyp/article/details/79814195
[https_blog.csdn.net_O_MMMM_O_article_details_106078679_utm_medium_distribute.pc_relevant.none-task-blog-baidujs_title-2_spm_1001.2101.3001.4242]: https://blog.csdn.net/O_MMMM_O/article/details/106078679?utm_medium=distribute.pc_relevant.none-task-blog-baidujs_title-2&spm=1001.2101.3001.4242
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMwODE1MjM3_size_16_color_FFFFFF_t_70 2]: /images/20221022/e6e4e5af2e694c438257e78f97597e9a.png
[https_blog.csdn.net_u011598824_article_details_56282618]: https://blog.csdn.net/u011598824/article/details/56282618