1、重点看【SLAM中的EKF，UKF，PF原理简介 - 半闲居士 - 博客园】

3、重点实例【扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）】

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

一、泰勒级数展开

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

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

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

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

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

${[\nabla f({{\bf{x}}})]^T} = {{\bf{J}}({\bf x})} = \begin{bmatrix} \frac{\partial f({\bf x})_1}{\partial {\bf x}_1} & \hdots & \frac{\partial f({\bf x})_1}{\partial {\bf x}_n}\\ \vdots & \ddots & \vdots \\ \frac{\partial f({\bf x})_m}{\partial {\bf x}_1} & \hdots & \frac{\partial f({\bf x})_m}{\partial {\bf x}_n} \end{bmatrix}$

这里， $f({{\bf{x}}_k})$ 为 $m$ 维， ${{\bf{x}}_k}$ 状态向量为 $n$ 维， $\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_m}\partial {x_n}}} = \frac{{\partial f({{\bf{x}}_k})^T}}{{\partial {x_m}}}\frac{{\partial f({{\bf{x}}_k})}}{{\partial {x_n}}}$ 。

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

二、扩展卡尔曼滤波EKF

标准卡尔曼滤波KF的状态转移方程和观测方程为

${{\mathbf{\theta }}_{k}}=\mathbf{A}{{\mathbf{\theta }}_{k-1}}+\mathbf{B}{{\mathbf{u}}_{k-1}}+{{\mathbf{s}}_{k}}$

${{\mathbf{z}}_{k}}=\mathbf{C}{{\mathbf{\theta }}_{k}}+{{\mathbf{v}}_{k}}$

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

利用泰勒展开式对上式在上一次的估计值处 $\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle$ (估计值)展开得

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

其中， ${\mathbf{F}}_{k-1}$ 和 ${\mathbf{H}}_{k}$ 分别表示函数 $f(\mathbf{\theta })$ 和 $h(\mathbf{\theta })$ 在 $\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle$ 和 $\mathbf{\theta }_{k}^{'}$ 处的雅克比矩阵。

（注：这里对泰勒展开式只保留到一阶导，二阶导数以上的都舍去，噪声假设均为加性高斯噪声）

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

Propagation：

$\mathbf{\theta }_{k}^{'}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle)$

$\mathbf{\Sigma }_{k}^{'}=\mathbf{F}_{k-1}{{\mathbf{\Sigma }}_{k-1}}{{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}$

Update：

$\mathbf{S}_{k}^{'}={{\left( \mathbf{H_{k}\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}$

$\mathbf{K}_{k}^{'}=\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}}_{k}^{T}}\mathbf{S}_{k}^{'}$

$\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+\mathbf{K}_{k}^{'}\left( {{\mathbf{z}}_{k}}-{h(\theta }_{k}^{'}) \right)$

${{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-\mathbf{K}_{k}^{'}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}$

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

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

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

from :卡尔曼滤波系列——（二）扩展卡尔曼滤波_GHelpU的博客-CSDN博客_扩展卡尔曼滤波算法

from :扩展卡尔曼滤波（EKF）算法详细推导及仿真（Matlab）_钢蛋_小朋友的博客-CSDN博客_ekf算法

这是书上给出的一个例子，我希望从中能归纳一种套路可以用在大部分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步的预测，这里只是把预测结果转化到测量度量下。

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

接下来进入状态更新

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

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

（十）：协方差更新

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

from：卡尔曼滤波（5）：一种用EKF解决问题的思路_buaazyp的博客-CSDN博客

雅克比矩阵计算参考扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）（有空记得看这篇博文）

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 :扩展卡尔曼滤波器的原理及应用_hankecs的博客

扩展卡尔曼滤波【转】

一、泰勒级数展开

二、扩展卡尔曼滤波EKF

matlab示例：

猜你喜欢