Udacity机器人软件工程师课程笔记（三十二) - 卡尔曼滤波器 - 一维卡尔曼滤波器 - 多维卡尔曼滤波器 - 拓展卡尔曼滤波器（EKF）

一、概述

如wiki百科中介绍的那样，卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。

卡尔曼滤波在技术领域有许多的应用。常见的有飞机及太空船的导引、导航及控制。卡尔曼滤波也广为使用在时间序列的分析中，例如信号处理及计量经济学中。卡尔曼滤波也是机器人运动规划及控制的重要主题之一，有时也包括在轨迹最佳化。卡尔曼滤波也用在中轴神经系统运动控制的建模中。因为从给与运动命令到收到感觉神经的回授之间有时间差，使用卡尔曼滤波有助于建立符合实际的系统，估计运动系统的目前状态，并且更新命令。

二、一维高斯分布

高斯分布是概率分布，是一个连续函数。随机变量x取值介于$X_1$ 和$X_2$。
由以下函数的积分给出在$X_1$ 至 $X_2$中的x的概率:
$$p（X_1<X<X_2）={\int }_{X_1}^{X_2}{F}_X(x)dx$$
例如下图中，$x$位于8.7m和9m之间的概率为7％。

均值和方差

高斯的特征在于两个参数-平均值（$\mu$）和方差（$\sigma ²$）。平均值是最可能出现的值，位于函数的中心，并且方差与曲线的宽度有关。术语“单峰”表示分布中存在单个峰。

高斯分布通常缩写为$N（x：\mu ，\sigma ²）$，并将在以后以这种方式引用。

高斯分布的公式如下所示。 请注意，该公式包含二次函数的指数。 二次将x的值与μ进行比较，在x =μ的情况下，指数等于1（$e^0=1$）。

$p(x)=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$

所有选项的概率之和必须是1。因此，函数下面的面积总和总是1。

$\int p(x)dx=1$

接下来使用c++编写一维高斯函数，来计算一个值在给定平均值和方差的情况下出现的概率。

#include <iostream>
#include <math.h>

using namespace std;

double f(double mu, double sigma2, double x)
{
	double prob = 1.0 / sqrt(2.0 * 3.14159 * sigma2) * exp(-0.5 * pow((x - mu), 2.0) / sigma2);
	return prob;
}

int main()
{
	cout << f(10.0, 4.0, 8.0) << endl;
	return 0;
}

注：从此部分以后大部分程序将使用c++来编写

三、一维卡尔曼滤波器

变量命名约定

$x_t$:位置（state）
$z_t$:测量值(measurement)
$u_t$:控制作用(control action)

卡尔曼滤波循环

卡尔曼滤波的作用方式可以用以下的图表示

1.测量值更新

（1）平均值计算

假设由上述概率分布，有先验概率有$\mu =20，{\sigma }^2=9$的高斯分布，现有一个测量值，有$v=30，r^2=9$的高斯分布，则其平均值${\mu }^{\prime },{\sigma }^{2^{\prime }}$,可以以如下方式进行计算：
$${\mu }^{\prime }=\frac{r^2\mu +{\sigma }^{2}v}{r^2+{\sigma }^2}$$
先验的不确定性乘以测量的平均值，以赋予其更大的权重，同样，测量的不确定性乘以先验的平均值。
$${\sigma }^{2^{\prime }}=\frac{1}{\frac{1}{r^2}+\frac{1}{{\sigma }^2}}$$

（2）程序实现

接下来以先验概率$N(x:{\mu }_1=10,{\sigma }^2=8)$, 和测量值$N(x:{\mu }_1=13,{\sigma }^2=2)$为例，计算其平均值：

#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> measurement_update(double mean1, double var1, double mean2, double var2)
{
	new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2);
	new_var = 1 / (1 / var1 + 1 / var2);
	return make_tuple(new_mean, new_var);
}

int main()
{

	tie(new_mean, new_var) = measurement_update(10, 8, 13, 2);
	printf("[%f, %f]", new_mean, new_var);
	return 0;
}

输出为

[12.400000, 1.600000]

2.位置预测

位置预测公式

后验均值：${\mu }^{\prime }={\mu }_1+{\mu }_2$

后验方差：${\sigma }^{2^{\prime }}={\sigma }_1^2+{\sigma }_2^2$

#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> state_prediction(double mean1, double var1, double mean2, double var2)
{
    new_mean = mean1 + mean2;
    new_var =  var1 + var2;
    return make_tuple(new_mean, new_var);
}

int main()
{

    tie(new_mean, new_var) = state_prediction(10, 4, 12, 4);
    printf("[%f, %f]", new_mean, new_var);
    return 0;
}

3.一维卡尔曼滤波器

该代码将遍历可用的测量和运动 并对每个测量和运动应用测量校正或状态预测

#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> measurement_update(double mean1, double var1, double mean2, double var2)
{
	new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2);
	new_var = 1 / (1 / var1 + 1 / var2);
	return make_tuple(new_mean, new_var);
}

tuple<double, double> state_prediction(double mean1, double var1, double mean2, double var2)
{
	new_mean = mean1 + mean2;
	new_var = var1 + var2;
	return make_tuple(new_mean, new_var);
}

int main()
{
	//Measurements and measurement variance
	double measurements[5] = { 5, 6, 7, 9, 10 };
	double measurement_sig = 4;

	//Motions and motion variance
	double motion[5] = { 1, 1, 2, 1, 1 };
	double motion_sig = 2;

	//Initial state
	double mu = 0;
	double sig = 1000;

	for (int i = 0; i < sizeof(measurements) / sizeof(measurements[0]); i++) {
		tie(mu, sig) = measurement_update(mu, sig, measurements[i], measurement_sig);
		printf("update:  [%f, %f]\n", mu, sig);
		tie(mu, sig) = state_prediction(mu, sig, motion[i], motion_sig);
		printf("predict: [%f, %f]\n", mu, sig);
	}

	return 0;
}

输出如下：

update:  [4.980080, 3.984064]
predict: [5.980080, 5.984064]
update:  [5.992019, 2.397446]
predict: [6.992019, 4.397446]
update:  [6.996198, 2.094659]
predict: [8.996198, 4.094659]
update:  [8.998121, 2.023388]
predict: [9.998121, 4.023388]
update:  [9.999063, 2.005830]
predict: [10.999063, 4.005830]

四、多维高斯分布

我们不能使用多个一维高斯来表示多维系统，因为维度之间可能存在相关性，我们无法使用独立的一维高斯模型来建模。

来看下图的二维高斯分布

其中，

均值以向量表示
$\mu =\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right]$

方差的多维等效项是协方差矩阵，
$\Sigma =\left[\begin{array}{cc}{\sigma }_x^2& {\sigma }_x{\sigma }_y\\ {\sigma }_y{\sigma }_x& {\sigma }_y^2\end{array}\right]$

${\sigma }_x^2$和${\sigma }_y^2$代表方差，${\sigma }_x{\sigma }_y$和${\sigma }_y{\sigma }_x$是相关性。如果一个维度的方差与另一维度的方差之间存在相关性，则这些项为非零。在这种情况下，高斯函数看起来“偏斜”。

多元高斯公式

以下是多元高斯公式。注意$x$和$\mu$是向量，并且$\Sigma$是一个矩阵。

$p(x)=\frac{1}{2{\pi }^{\frac{D}{2}}|\Sigma {|}^{\frac{1}{2}}}{e}^{-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )}$

如果D = 1，则该公式将简化为一维高斯公式。

五、多维卡尔曼滤波器

1.多维KF简介

以下是到目前为止我们所使用到的卡尔曼滤波方程：
$x^{\prime }=Fx$
$P=FP{F}^T+Q$
$y=z-H{x}^{\prime }$
$S=H{P}^{\prime }{H}^T+R$
$K=P^{\prime }{H}^T{S}^{-1}$
$x=x^{\prime }+Ky$

我们将使用它们来研究卡尔曼滤波器的两个极端情况:在传感器测量完美的情况下，以及传感器产生完全不确定数据的情况。

传感器测量完美的情况

如果测量的传感器非常精确，那么测量噪声R就是一个零矩阵。
$R=[0]$

S的方程将简化为以下公式
$S=H{P}^{\prime }{H}^T$

代入卡尔曼增益公式
$K=P^{\prime }{H}^T(H{P}^{\prime }{H}^T{)}^{-1}$

重写如下
$K=P^{\prime }{H}^T(H^{T-1}{P}^{{}^{\prime }-1}{H}^{-1})$

利用矩阵性质$A{A}^{-1}=I$可得，
$K=H^{-1}$

使用卡尔曼增益来更新平均值
$x=x^{\prime }+Ky$

代入残差$y$的公式$y=z-H{x}^{\prime }$
$x=x^{\prime }+K(z-H{x}^{\prime })$

将之前得到的$K=H^{-1}$代入公式
$x=x^{\prime }+H^{-1}(z-H{x}^{\prime })$

简化如下
$x=x^{\prime }+H^{-1}z-x^{\prime }$
$x=H^{-1}z$

H是从状态映射到测量的函数，所以它的逆函数将从测量值映射到状态。新的平均数完全由测量值构成。

传感器产生完全不确定数据

测量噪声为$R=[\infty ]$

$S=H{P}^{\prime }{H}^T+R$

则$S=[\infty ]$

则卡尔曼增益为
$K=P^{\prime }{H}^T{S}^{-1}$
$K=P^{\prime }{H}^T{\infty }^{-1}$
$K=0$

则由$x=x^{\prime }+Ky$得
$x=x^{\prime }$

在这种情况下，新的平均值等于状态预测，测量校正根本不包括在内。

---

卡尔曼滤波器的两个极端是完美传感器和可忽略传感器 。卡尔曼增益介于两者之间。

完美传感器	可忽略传感器
$R=[0]$	$R=[\infty ]$
$x=H^{-1}z$	$x=x^{\prime }$

通过这两个例子，我们就可以更好的了解卡尔曼滤波方程了。

2.多维KF设计

在这部分将使用线性代数，因为它允许我们轻松地处理多维问题。首先，我们把状态预测写成线性代数的形式。

（1）状态转换

下面的公式是状态转换函数，它将状态从时间 t 推进到时间 t +1。在这里只是机器人位置$x$和速度$\dot{x}$之间的关系。我们假设机器人的速度没有变化。
$x^{\prime }=x+\Delta t\dot{x}$
${\dot{x}}^{\prime }=\dot{x}$

我们可以用矩阵形式表示相同的关系，如下所示。左侧是后状态，右边是状态转移函数和先前状态。这个方程式显示了状态在一段时间$\Delta t$内的变化。注意，我们仅在此处使用平均值。

${\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]}^{\prime }=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]$

状态转移函数记为$F$，公式为:
$x^{\prime }=Fx$

在现实中，该方程还应考虑过程噪声，因为它自己的术语在方程中。然而，过程噪声是一个均值为0的高斯函数，所以均值的更新方程不需要包含它。
$x^{\prime }=Fx+noise$
$noise=N(0,Q)$

虽然很常用$\Sigma$表示数学中高斯分布的协方差，使用字母更常见$P$表示局部状态的状态协方差。

如果用$F$乘以状态$x$，那么协方差就会受到$F$平方的影响。在矩阵形式，这将是这样的:
$P^{\prime }=FP{F}^T$

为了计算后验协方差，将先验协方差乘以状态转移函数的平方，加上$Q$作为过程噪声增加的不确定性。$Q$可以解释为什么机器人会意外减速，或者受到外部影响而偏离轨道。
$P^{\prime }=FP{F}^T+Q$

现在我们更新了平均值和协方差作为状态预测的一部分。

（2）测量校正

接下来，我们进入度量更新步骤。如果我们回到最初的例子，我们在x维中跟踪一个机器人的位置和速度，机器人只测量位置(速度是一个隐藏的状态变量)。因此，测量函数非常简单——一个包含1和0的矩阵。这个矩阵演示了如何将状态映射到观察，$z$。

$z=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]$

这个矩阵称为测量函数，并用$H$表示。

对于度量更新步骤，有几个公式。首先计算测量残差$y$。测量残差是测量值与基于预测的期望测量值之间的差值。测量残差将在以后的公式中使用。
$y=z-H{x}^{\prime }$

接下来，考虑测量噪声，即$R$。该公式将状态预测协方差映射到测量空间，并加入测量噪声。结果为$S$,将被用在后续的方程中来计算卡尔曼增益。
$S=H{P}^{\prime }{H}^T+R$

（3）卡尔曼增益

接下来，我们计算卡尔曼增益，$k$，卡尔曼增益决定了应该在状态预测上放多少权重，以及在测量校正上放多少权重。它是根据状态预测和测量校正的不确定性而变化的平均因子。
$K=P^{\prime }{H}^T{S}^{-1}$
$x=x^{\prime }+Ky$

（4）卡尔曼滤波方程

卡尔曼滤波器的最后一步是使用卡尔曼增益更新新状态的协方差。
$P=(I-KH)P^{\prime }$

这些是在多维空间中实现卡尔曼滤波的方程。

状态预测:
$x^{\prime }=Fx$
$P=FP{F}^T+Q$

测量校正:
$y=z-H{x}^{\prime }$
$S=H{P}^{\prime }{H}^T+R$

卡尔曼增益的计算:
$K=P^{\prime }{H}^T{S}^{-1}$

后验状态和协方差的计算:
$x=x^{\prime }+Ky$
$P=(I-KH)P^{\prime }$

卡尔曼滤波器可以成功地从不准确的初始估计中恢复，但是尽可能准确地估计噪声参数$Q$和$R$是非常重要的，因为它们被用来确定哪个估计或测量更可信。

（5）相关程序

#include <iostream>
#include <math.h>
#include <tuple>
#include "Core" // Eigen Library
#include "LU"   // Eigen Library

using namespace std;
using namespace Eigen;

float measurements[3] = { 1, 2, 3 };

tuple<MatrixXf, MatrixXf> kalman_filter(MatrixXf x, MatrixXf P, MatrixXf u, MatrixXf F, MatrixXf H, MatrixXf R, MatrixXf I)
{
    for (int n = 0; n < sizeof(measurements) / sizeof(measurements[0]); n++) {

        // Measurement Update
        MatrixXf Z(1, 1);
        Z << measurements[n];

        MatrixXf y(1, 1);
        y << Z - (H * x);

        MatrixXf S(1, 1);
        S << H * P * H.transpose() + R;

        MatrixXf K(2, 1);
        K << P * H.transpose() * S.inverse();

        x << x + (K * y);

        P << (I - (K * H)) * P;

        // Prediction
        x << (F * x) + u;
        P << F * P * F.transpose();
    }

    return make_tuple(x, P);
}

int main()
{

    MatrixXf x(2, 1);// Initial state (location and velocity) 
    x << 0,
    	 0; 
    MatrixXf P(2, 2);//Initial Uncertainty
    P << 100, 0, 
    	 0, 100; 
    MatrixXf u(2, 1);// External Motion
    u << 0,
    	 0; 
    MatrixXf F(2, 2);//Next State Function
    F << 1, 1,
    	 0, 1; 
    MatrixXf H(1, 2);//Measurement Function
    H << 1,
    	 0; 
    MatrixXf R(1, 1); //Measurement Uncertainty
    R << 1;
    MatrixXf I(2, 2);// Identity Matrix
    I << 1, 0,
    	 0, 1; 

    tie(x, P) = kalman_filter(x, P, u, F, H, R, I);
    cout << "x= " << x << endl;
    cout << "P= " << P << endl;

    return 0;
}

六、扩展卡尔曼滤波器

基本卡尔曼滤波器（The basic Kalman filter）是限制在线性的假设之下。然而，大部分非平凡的（non-trivial）的系统都是非线性系统。其中的“非线性性质”（non-linearity）可能是伴随存在过程模型（process model）中或观测模型（observation model）中，或者两者兼有之。 除了需要线性化非线性运动或测量函数以能够更新方差外，其机制与卡尔曼滤波器并没有太大不同。

我们已经了解了如何对一维状态预测或测量函数进行此操作，但是现在该探讨如何对多维的函数进行线性化了。为此，我们将使用多维泰勒级数。

1.多维线性化

若系统为一个非线性系统，那么如果像之前计算线性系统一样来计算非线性系统就会导致如下的结果，

可以看到输出已经不是一个正态分布了，那么如何解决这个问题？

平均值mean可以通过一个非线性函数$f(x)$更新，但是协方差必须通过非线性函数函数$f(x)$的线性化来更新。

为了计算非线性函数局部的近似值，我们引入多维泰勒级数概念。使用泰勒级数来线性化函数。

多维泰勒级数的方程式如下所示。
$T(x)=f(a)+(x-a{)}^{T}Df(a)+\frac{1}{2!}(x-a{)}^T{D}^{2}f(a)(x-a)+...$

多维泰勒级数和一维泰勒级数很相似。和之前一样，为了计算线性近似，我们只需要前两项。
$T(x)=f(a)+(x-a{)}^{T}Df(a)$

$Df(a)$为雅可比矩阵，它包含多维方程的偏导项。
$Df(a)=\frac{\delta f(a)}{\delta x}$

在它的展开形式中，雅可比矩阵是一个偏导矩阵。它告诉我们f的每一个分量是如何随着状态向量的每一个分量的改变而改变的。

$Df(a)=\left[\begin{array}{cccc}\frac{\delta f_1}{\delta x_1}& \frac{\delta f_1}{\delta x_2}& ...& \frac{\delta f_1}{\delta x_n}\\ \frac{\delta f_2}{\delta x_1}& \frac{\delta f_2}{\delta x_2}& ...& \frac{\delta f_2}{\delta x_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\delta f_m}{\delta x_1}& \frac{\delta f_m}{\delta x_2}& ...& \frac{\delta f_m}{\delta x_n}\end{array}\right]$

行对应函数$f$的维数，列对应$x$的维数(状态变量)。矩阵的第一个元素是关于$x$的第一个维数的函数。
雅可比矩阵是一维情况的推广。在一维情况下，雅可比矩阵只有$\frac{df}{dx}$项。

实例应用

让我们看一个具体的例子。假设我们在追踪一个物体的x-y坐标。这就是说，我们的状态是一个向量x，有状态变量x和y。
$x=\left[\begin{array}{c}x\\ y\end{array}\right]$

然而，我们的传感器不允许我们直接测量物体的x和y坐标。相反,我们的传感器测量距离机器人对象,$r$, $r$之间的角度和x轴,$\theta$。
$z=\left[\begin{array}{c}r\\ \theta \end{array}\right]$

需要注意的是,物体的状态使用的是笛卡尔坐标系，而度量是在极坐标表示法中进行的。

我们的测量函数将状态映射到观测值，
$\left[\begin{array}{c}x\\ y\end{array}\right]\underrightarrow{meas.function}\left[\begin{array}{c}r\\ \theta \end{array}\right]$

因此，我们的测量函数必须从笛卡尔坐标映射到极坐标。但是没有矩阵$H$能成功地进行这种转换，因为笛卡尔坐标和极坐标之间的关系是非线性的。
$r=\sqrt{x^2+y^2}$
$\theta =ta{n}^{-1}(\frac{y}{x})$

因此，不采用测量残差方程$y=z-H{x}^{\prime }$,而是使用专用函数$h(x^{\prime })$进行映射
$h(x^{\prime })=\left[\begin{array}{c}\sqrt{x^2+y^2}\\ ta{n}^{-1}(\frac{y}{x})\end{array}\right]$

此时测量残差方程为$y=z-h(x^{\prime })$.我们的测量协方差矩阵不能以同样的方式更新，因为它会变成一个非高斯分布。我们来计算一个线性化的$H$，然后用它来代替。

函数的泰勒级数$h(x)$,其均值为μ,定义如下，
$h(x)\simeq h(\mu )+(x-\mu {)}^{T}Df(\mu )$

雅可比矩阵,$Df(\mu )$,如下定义。我们称它为$H$,因为它是测量函数H (x)的线性化，
$H=\left[\begin{array}{cc}\frac{\partial r}{\partial x}& \frac{\partial r}{\partial y}\\ \frac{\partial \theta }{\partial x}& \frac{\partial \theta }{\partial y}\end{array}\right]$

如果要计算每一个偏导，矩阵就会简化成这样，
$H=\left[\begin{array}{cc}\frac{x}{\sqrt{x^2+y^2}}& \frac{y}{\sqrt{x^2+y^2}}\\ -\frac{y}{x^2+y^2}& \frac{x}{x^2+y^2}\end{array}\right]$

这个矩阵$H$，可以用来更新状态的协方差。

---

总结一下扩展卡尔曼滤波器的计算流程，值得重新审视这些方程式，以了解发生了什么变化以及什么保持不变。

状态预测:
$\cancel{x^{\prime }=Fx}$ $\to x^{\prime }=f(x)$
$P=F$$P{F}^T$$+Q$

测量校正:
$\cancel{y=z-H{x}^{\prime }}$$\to y=z-h(x^{\prime })$
$S=H$$P^{\prime }$$H^T$$+R$

卡尔曼增益的计算:
$K=P^{\prime }$$H^T$$S^{-1}$

后验状态和协方差的计算:
$x=x^{\prime }+Ky$
$P=(I-KH$$)P^{\prime }$

用蓝色突出显示的是替换了度量和状态转换函数的雅可比矩阵。

扩展的卡尔曼滤波要求我们在每一次迭代中计算非线性函数的雅可比矩阵，因为均值(也就是我们线性化的点)是更新的。

2.总结

以下是关于扩展卡尔曼滤波器的关键信息:

• 当测量和/或状态转换函数为非线性时，不能使用卡尔曼滤波器，因为这会导致非高斯分布。
• 相反，我们取一个局部线性近似，并使用这个近似来更新估计的协方差。线性逼近是使用泰勒级数的第一项来做的，它包括函数的一阶导数。
• 在多维情况下，求一阶导数并不像有多个状态变量和多个维度那么简单。这里我们用了一个雅可比矩阵，它是一个偏导矩阵，包含了每个维数对每个状态函数的偏导。

虽然使用卡尔曼滤波对理解基础数学很重要，但不要觉得有必要记住这些方程。可能的情况是，无论使用什么软件包或编程语言，它们都有允许应用卡尔曼滤波的库，或者至少执行线性代数计算(如矩阵乘法和计算雅可比矩阵)。

3.EKF 示例

让我们来看另一个车辆测量的例子——这次是四旋翼。这个四旋翼稍微简化了一点——它的运动被限制在y轴上。因此，它的状态可以由以下向量定义，
$x=\left[\begin{array}{c}\varphi \\ \dot{y}\\ y\end{array}\right]$

也就是它的侧倾角，速度和位置。

有一个四旋翼，例如下图所示。 这个四旋翼飞机想知道它与墙壁之间的距离。 如果四旋翼飞机想要穿越房间的内部或建筑物的外部，同时又要与墙壁保持安全距离，这是一项重要的测量方法。

为了估计这个距离，四旋翼配备了一个测距传感器。

则四旋翼到墙的距离为：
$h(x)=wall-y$

现在，如果四旋翼旋转到某个角度$\varphi$,

应用一些基本的三角学知识，可以确定这个四旋翼测距仪的测量模型。
$h(x)=\frac{wall-y}{cos\varphi }$

这个函数的分母上有一个余弦，使得这个函数是非线性的。这意味着我们需要使用扩展的卡尔曼滤波器进行估计，并在此过程中，使函数线性化。

计算H

为了应用扩展的卡尔曼滤波，我们需要计算H，即我们在上面定义的测量模型的雅可比矩阵。计算这三个偏导数的结果如下:

$\frac{\delta h}{\delta \varphi }=\frac{sin\varphi }{co{s}^2\varphi }(wall-y)$
$\frac{\delta h}{\delta \dot{y}}=0$
$\frac{\delta h}{\delta y}=\frac{-1}{cos\varphi }$

在代码中实现扩展的卡尔曼滤波时，有一些软件库可以对函数求偏导数，从而简化了实现。当然，也有现成的EKF实现。然而，理解算法的内部工作方式并将其智能地应用于手头的问题总是很有帮助的。

$H=\left[\begin{array}{ccc}\frac{sin\varphi }{co{s}^2\varphi }(wall-y)& 0& \frac{-1}{cos\varphi }\end{array}\right]$

它可以用于EKF方程更新状态的协方差。

---

扩展卡尔曼滤波方程

如果状态转换或测量函数是非线性的，则蓝色的矩阵显示使用了雅可比矩阵。

$x^{\prime }=f(x)$
$P=F$$P{F}^T$$+Q$

测量校正:
$y=z-h(x^{\prime })$
$S=H$$P^{\prime }$$H^T$$+R$

卡尔曼增益的计算:
$K=P^{\prime }$$H^T$$S^{-1}$

后验状态和协方差的计算:
$x=x^{\prime }+Ky$
$P=(I-KH$$)P^{\prime }$