KF filter

Tutorial - The Kalman Filter

1. Introduction

   长期以来，卡尔曼滤波器[1]一直被认为是许多跟踪和数据预测任务的最优解，即[2]。它在视觉运动分析中的应用经常被记录下来。这里给出了标准卡尔曼滤波推导，作为实际使用前面几节介绍的一些统计技术的教程练习。该滤波器被构造为一个均方误差最小化器，但也提供了该滤波器的另一种推导方法，展示了该滤波器与最大似然统计量之间的关系。记录这个推导过程可以让读者进一步了解滤波器中的统计结构。 滤波的目的是从一个信号中提取所需的信息，而不考虑其他任何因素。滤波器执行这项任务的好坏可以用成本或损失函数来衡量。事实上，我们可以将滤波器的目标定义为使损失函数最小化。

2. Mean squared error

   由于有必要考虑滤波器在一段时间内预测许多数据的能力，一个更有意义的度量是误差函数的期望值。这个结果就是均方误差(MSE)函数。

3. Maximum likelihood

   上面推导的均方误差，虽然直观，但有点启发。可以使用极大似然统计量进行更严格的推导。这是通过重新定义滤波器的目标来实现的，即找到最大化$y$的概率或可能性的$\hat{x}$。

   假设加性随机噪声为高斯分布，标准差为$\sigma_{k}$，式11.9的驱动函数为MSE，MSE可以通过$\hat{x}{k}$的变化达到最大值。因此，当$y{k}$的期望变化被最好地建模为高斯分布时，均方误差函数是适用的。在这种情况下，MSE提供$\hat{x}{k}$的值，使信号$y{k}$的可能性最大化。

   在接下来的推导中，最优滤波器被定义为该滤波器，它来自于所有可能的滤波器的集合，这些滤波器使均方误差最小。

4. Kalman Filter Derivation

   在继续讨论卡尔曼滤波之前，首先应该承认诺伯特·维纳·[4]的工作。维纳在均方误差意义上描述了一个最优有限脉冲响应(FIR)滤波器。这里不讨论他的解决方案，尽管它与卡尔曼滤波有很多共同之处。只需说，他的解决方案同时使用了接收信号与原始数据的自相关和互相关联，从而推导出滤波器的脉冲响应。 卡尔曼也提出了一种最优MSE滤波器的设计方法。然而，卡尔曼的处方比韦纳的有一些优势；它避开了确定滤波器脉冲响应的需要，而这一点不太适合数值计算。卡尔曼使用状态空间技术描述了他的滤波器，这与维纳的分类不同，它使滤波器既可以作为一个平滑的过滤器，也可以作为一个预测器。后三者中，卡尔曼滤波用于预测数据的能力已被证明是一个非常有用的函数。这导致卡尔曼滤波被应用于广泛的跟踪和导航问题。用状态空间方法定义滤波器还简化了滤波器在离散域中的实现，这也是它具有广泛吸引力的另一个原因。

5. State space derivation

   假设我们想要知道以下形式的过程中变量的值；$x_{k}$是$k$时刻过程的状态向量， $(n \times 1)$；$\Phi$为过程从$k$点状态到$k+1$点状态的状态转换矩阵，假设随时间平稳，$(n \times m)$；$w_{k}$是与已知协方差相关的白噪声过程，$(n \times 1)$。 对该变量的观察可以用以下形式建模；$z_{k}$是$x$在$k$时刻的实际测量值，$(m \times 1)$；$H$为状态向量与测量向量之间的无噪声连接，假设随时间保持平稳，$(m \times n)$；$v_{k}$是相关的测量误差。再次假设这是一个具有已知协方差的白噪声过程，并且与过程噪声的互相关性为零，$(m \times 1)$。 如第2节所示，为了使最小MSE产生最优滤波器，必须能够使用高斯分布对系统误差进行正确的建模。假设两种噪声模型的协方差随时间平稳；

   $P_{k}$是$k$时刻过程的误差协方差矩阵；

   假设$\hat{x}{k}$的先验估计数为$\hat{x}{k}^{'}$，由系统知识得到。将旧估计值与实测数据相结合，可以写出新估计值的更新方程；$K_{k}$是卡尔曼增益，很快就会推导出来。$z_{k}-H\hat{x}{k}^{'}$称为创新或计量残差； $$ \begin{align} \hat{x}{k} & = \hat{x}{k}^{'}+K{k}(Hx_{k}+v_{k}-H\hat{x}{k}^{'}) \tag{11.18}\ x{k}-\hat{x}{k} & = x{k}-[\hat{x}{k}^{'}+K{k}(Hx_{k}+v_{k}-H\hat{x}{k}^{'})]\ & = (x{k}-\hat{x}{k}^{'})-K{k}[H(x_{k}-\hat{x}{k}^{'})+v{k}] \ & = (I-K_{k}H)(x_{k}-\hat{x}{k}^{'})-K{k}v_{k}\ P_{k} & = E[e_{k}e_{k}^{T}]=E[(x_{k}-\hat{x}{k})(x{k}-\hat{x}{k})^{T}]\ & = E[[(I-K{k}H)(x_{k}-\hat{x}{k}^{'})-K{k}v_{k}][(I-K_{k}H)(x_{k}-\hat{x}{k}^{'})-K{k}v_{k}]^{T}] \tag{11.19} \end{align} $$ 注意到，$x_{k}-\hat{x}{k}^{''}$是先验估计的误差。很明显，这与测量噪声无关，因此期望可以改写为； $$ \begin{align} P{k} & = E[e_{k}e_{k}^{T}]=E[(x_{k}-\hat{x}{k})(x{k}-\hat{x}{k})^{T}]\ & = E[[(I-K{k}H)(x_{k}-\hat{x}{k}^{'})-K{k}v_{k}][(I-K_{k}H)(x_{k}-\hat{x}{k}^{'})-K{k}v_{k}]^{T}] \tag{11.19} \ & = E[[(I-K_{k}H)(x_{k}-\hat{x}{k}^{'})-K{k}v_{k}][(I-K_{k}H)(x_{k}-\hat{x}{k}^{'})]^{T}-(I-K_{k}H)(x_{k}-\hat{x}{k}^{'})-K{k}v_{k}^{T}]\ & = E[(I-K{k}H)(x_{k}-\hat{x}{k}^{'})[(I-K{k}H)(x_{k}-\hat{x}{k}^{'})]^{T}-K{k}v_{k}[(I-K_{k}H)(x_{k}-\hat{x}{k}^{'})]^{T}-(I-K{k}H)(x_{k}-\hat{x}{k}^{'})(K{k}v_{k})^{T}+(K_{k}v_{k})(K_{k}v_{k})^{T}]\ & = E[(I-K_{k}H)(x_{k}-\hat{x}{k}^{'})(x{k}-\hat{x}{k}^{'})^{T}(I-K{k}H)^{T}-0-0+K_{k}v_{k}v_{k}^{T}K_{k}^{T}]\ & = E[(I-K_{k}H)(x_{k}-\hat{x}{k}^{'})(x{k}-\hat{x}{k}^{'})^{T}(I-K{k}H)^{T}]+E[K_{k}v_{k}v_{k}^{T}K_{k}^{T}]\ & = (I-K_{k}H)E(x_{k}-\hat{x}{k}^{'})(x{k}-\hat{x}_{k}^{'})^{T}^{T}+K_{k}E[v_{k}v_{k}^{T}]K_{k}^{T} \tag{11.20} \ & = (I-K_{k}H)P_{k}^{'}(I-K_{k}H)^{T}+K_{k}RK_{k}^{T} \tag{11.21} \end{align} $$ $P_{k}^{''}$是$P_{k}$的先验估计。式11.21为误差协方差更新方程。协方差矩阵的对角线包含均方误差，如式所示；

   矩阵对角元素的和就是矩阵的迹。在误差协方差矩阵的情况下，迹是误差的平方和。因此，可以通过最小化$P_{k}$的迹来最小化均方误差，从而最小化$P_{kk}$的迹。 首先对$P_{k}$的迹对$K_{k}$求导，然后将结果集设为零，从而得到这个最小值的条件。

   注意一个矩阵的迹等于它的转置的迹，因此它可以写成； $$ \begin{align} P_{k} & = (I-K_{k}H)P_{k}^{'}(I-K_{k}H)^{T}+K_{k}RK_{k}^{T} \ & = P_{k}^{'}-K_{k}HP_{k}^{'}-P_{k}^{'}H^{T}K_{k}^{T}+K_{k}HP_{k}^{'}H^{T}K_{k}^{T}+K_{k}RK_{k}^{T}\ & = P_{k}^{'}-K_{k}HP_{k}^{'}-P_{k}^{'}H^{T}K_{k}^{T}+K_{k}(HP_{k}^{'}H^{T}+R)K_{k}^{T} \tag{11.23} \ T[P_{k}] & = T[P_{k}^{'}] -T[K_{k}HP_{k}^{'}]-T[P_{k}^{'}H^{T}K_{k}^{T}]+T[K_{k}(HP_{k}^{'}H^{T}+R)K_{k}^{T}]\ & = T[P_{k}^{'}] -T[K_{k}HP_{k}^{'}]-T[(K_{k}HP_{k}^{'})^{T}]+T[K_{k}(HP_{k}^{'}H^{T}+R)K_{k}^{T}]\ & = T[P_{k}^{'}] -2T[K_{k}HP_{k}^{'}]+T[K_{k}(HP_{k}^{'}H^{T}+R)K_{k}^{T}] \tag{11.24} \ \frac{dT[P_{k}]}{dK_{k}} & = 0-2(HP_{k}^{'})^{T}+K_{k}(HP_{k}^{'}H^{T}+R)^{T}+K_{k}(HP_{k}^{'}H^{T}+R) \ \frac{dT[P_{k}]}{dK_{k}} & = -2(HP_{k}^{'})^{T}+K_{k}(HP_{k}^{'T}H^{T}+R^{T})+K_{k}(HP_{k}^{'}H^{T}+R) \ \frac{dT[P_{k}]}{dK_{k}} & = -2(HP_{k}^{'})^{T}+2K_{k}(HP_{k}^{'}H^{T}+R) \tag{11.25} \end{align} $$ 公式11.27是卡尔曼增益方程。计量残差具有相关的测量预测协方差。这被定义为；

   式11.29为具有最优增益的误差协方差矩阵的更新方程。三个方程11.16、11.27和11.29给出了变量$x_{k}$的估计值。使用实现状态投影；

   为了完成递归，需要找到一个方程，该方程将误差协方差矩阵投影到下一个时间区间k + 1。这是通过首先形成一个表达式的先验误差；

   这就完成了递归过滤器。算法循环如图11.5所示。

6. The Kalman Filter as a chi-square merit function

   卡尔曼滤波的目的是使实际数据和估计数据之间的均方误差最小化。因此，它在均方误差意义上提供了数据的最佳估计值。在这种情况下，应该有可能表明卡尔曼滤波与卡方有很多共同之处。卡方优值函数是一个极大似然函数，前面推导过，公式11.9。它通常被用作一个标准，将一组模型参数拟合到一个称为最小二乘拟合的模型中。卡尔曼滤波器通常被称为递归最小二乘(RLS)拟合器。与卡方价值函数的相似之处将使我们对卡尔曼滤波有一个不同的认识。

   然后可以将最优参数集定义为使上述函数最小化的参数集。展开方差得到；

7. Model covariance update

   将模型参数协方差考虑为逆形式，称为信息矩阵。利用标准误差代入法，可以建立协方差矩阵的另一种更新方程;

Kalman Filter Derivation

目的是证明卡尔曼滤波方程在所有无偏估计量的基础上提供了最小方差估计量。

没有对过程或测量噪声的特定分布作任何假设。

Kalman Filter参数调整

问题描述

定义一个随机离散时间过程的状态向量 ，该过程用一个离散随机差分方程描述： $$ x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1} $$

• 其中n维向量 $x_k$为k时刻的系统状态变量，n维向量$x_{k-1}$是k-1时刻的系统状态变量。

• A是状态转移矩阵或者过程增益矩阵，是$n \times n$ 阶方阵，它将k-1时刻状态和当前的k时刻状态联系起来。

• B是可选的控制输入$u \in R^l$的增益，在大多数实际情况下并没有控制增益，所以$Bu_{k-1}$这一项很愉快的变成零了。

• $w_{k-1}$是n维向量，代表过程激励噪声，它对应了$x_k$中每个分量的噪声，是期望为0，协方差为Q的高斯白噪声，$w_{k}$~$N(0,Q)$。

再定义一个观测变量 ，得到观测方程： $$ z_k=Hx_{k}+v_{k} $$

• 其中观测值$z_k$是m阶向量，状态变量$x_k$是n阶向量。
• H是$m \times n$阶矩阵，代表状态变量$x_k$对测量变量$z_k$的增益。
• 观测噪声$v_k$是期望为0，协方差为R的高斯白噪声，$v_k$~$N(0,R)$。

矩阵确定-举例1

在目标跟踪的应用中，假设质点坐标为$(x,y)$是直接观测得到的，质点在x、y轴方向速度分别为$v_x$，$v_y$，那么系统状态变量$x_k=(x,y,v_x,v_y)^T$，系统观测变量$z_k=(\overline{x},\overline{y})^T$，系统没有控制输入，所以状态方程就成了： $$ x_k=Ax_{k-1}+w_{k-1} $$ 它的状态转移矩阵A根据运动学公式确定： $$ A= \begin{bmatrix} 1 & 0 & \Delta t & 0\ 0 & 1 & 0 & \Delta t\ 0 & 0 & 1 & 0\ 0 & 0 & 0 & 1 \end{bmatrix} $$ 其实用矩阵的形式写开上面的方程就很好理解了： $$ x_k=Ax_{k-1}+w_{k-1} \rightarrow \begin{bmatrix} x \ y \ v_x \ v_y \end{bmatrix}_k

\begin{bmatrix} 1 & 0 & \Delta t & 0\ 0 & 1 & 0 & \Delta t\ 0 & 0 & 1 & 0\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \ y \ v_x \ v_y \end{bmatrix}{k-1} +w{k-1}= \begin{bmatrix} x+\Delta t \cdot v_x \ y+\Delta t \cdot v_y \ v_x \ v_y \end{bmatrix}{k-1} +w{k-1} $$ 该过程的观测方程为： $$ z_k=Hx_{k}+v_{k} $$ 它的观测矩阵H也是要指定的，它的目的是将m维的测量值转换到n维与状态变量相对应，由于直接观测的量是位置$z_k=(\overline{x},\overline{y})^T$，我们只需要取状态变量的前两个元素就够了，所以H设计成如下： $$ H= \begin{bmatrix} 1 & 0 & 0 & 0\ 0 & 1 & 0 & 0 \end{bmatrix} $$ 用矩阵形式写开就是： $$ z_k=Hx_{k}+v_{k} \rightarrow \begin{bmatrix} \overline{x} \ \overline{y} \end{bmatrix}_k = \begin{bmatrix} 1 & 0 & 0 & 0\ 0 & 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} x \ y \ v_x \ v_y \end{bmatrix} +v_k= \begin{bmatrix} x \ y \end{bmatrix}_k+v_k $$

• 这里简要说一下状态转移矩阵A的确定：

  (1) 在标量卡尔曼滤波中，比如测量值是温度、湿度，一般认为下一个时刻该温度或者湿度维持不变，这种情况下状态转移矩阵通常就是标量1。

  (2) 在导航和目标跟踪中卡尔曼滤波常被用来做位置估计，比如匀速直线运动（CV）和匀加速直线运动（CA）： $$ A_{CV}= \begin{bmatrix} 1 & \Delta t & 0 \ 0 & 1 & 0 \ 0 & 0 & 1 \end{bmatrix} $$

  $$ x_k=Ax_{k-1}+w_{k-1} \rightarrow \begin{bmatrix} x \ v_x \ a_x \end{bmatrix}_k

  \begin{bmatrix} 1 & \Delta t & 0 \ 0 & 1 & 0 \ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \ v_x \ a_x \end{bmatrix}{k-1} +w{k-1}= \begin{bmatrix} x+\Delta t \cdot v_x \ v_x \ a_x \end{bmatrix}{k-1} +w{k-1} $$

  $$ A_{CA}= \begin{bmatrix} 1 & \Delta t & \frac{\Delta t^2}{2} \ 0 & 1 & \Delta t \ 0 & 0 & 1 \end{bmatrix} $$

  $$ x_k=Ax_{k-1}+w_{k-1} \rightarrow \begin{bmatrix} x \ v_x \ a_x \end{bmatrix}_k

  \begin{bmatrix} 1 & \Delta t & \frac{\Delta t^2}{2} \ 0 & 1 & \Delta t \ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \ v_x \ a_x \end{bmatrix}{k-1} +w{k-1}= \begin{bmatrix} x+\Delta t \cdot v_x +\frac{\Delta t^2}{2} \cdot a_x\ v_x + \Delta t \cdot a_x \ a_x \end{bmatrix}{k-1} +w{k-1} $$

  在前面的举例中x，y方向上的运动就被微分近似为匀速直线运动。

  (3) 若被估计的过程或观测变量与过程的关系是非线性的，此时不能够直接应用卡尔曼滤波（因为不满足上一节提到的卡尔曼滤波适用的前提必须是线性系统），这个时候扩展卡尔曼滤波（EKF）应运而生，它用雅克比矩阵将期望和方差线性化，从而将卡尔曼滤波扩展到非线性系统，但是EKF由于考虑了泰勒级数的展开，运算量大大增加。

矩阵确定-举例2

一个运动目标的状态变量$x_k=(x(k), \dot{x}(k), \ddot{x}(k))^T$，其中第一项是目标在k时刻的位置，这里假设为一维坐标，第二项为k时刻的速度，第三项为k时刻的加速度，雷达仅能观测到目标的位置即$x(k)$，那么它的状态转移矩阵就该设计成： $$ A_{CA}= \begin{bmatrix} 1 & \Delta t & \frac{\Delta t^2}{2} \ 0 & 1 & \Delta t \ 0 & 0 & 1 \end{bmatrix} $$ 它的观测矩阵就该设计成： $$ H= \begin{bmatrix} 1 & 0 & 0 \end{bmatrix} $$ 到了这里怎么设计矩阵A和H应该有思路了吧。

参数调整

$\hat{x}_{\overline{k}}$：表示k时刻先验状态估计值，这是算法根据前次迭代结果（就是上一次循环的后验估计值）做出的不可靠估计。

$\hat{x}k$、$\hat{x}{k-1}$：分别表示k时刻、k-1时刻后验状态估计值，也就是要输出的该时刻最优估计值，这个值是卡尔曼滤波的结果。

A：表示状态转移矩阵，是$n \times n$阶方阵，它是算法对状态变量进行预测的依据，状态转移矩阵如果不符合目标模型有可能导致滤波发散，它的确定请参看第二节中的举例。

B：表示可选的控制输入$u \in R^l$的增益，在大多数实际情况下并没有控制增益。

$u_{k-1}$：表示k-1时刻的控制增益，一般没有这个变量，可以设为0。

$\hat{P}{\overline{k}}$：表示k时刻的先验估计协方差，这个协方差矩阵只要确定了一开始的$\hat{P}{0}$ ，后面都可以递推出来，而且初始协方差矩阵$\hat{P}_{0}$只要不是为0，它的取值对滤波效果影响很小，都能很快收敛。

$\hat{P}{k}$、$\hat{P}{k-1}$：分别表示k时刻、k-1时刻的后验估计协方差，是滤波结果之一。

Q：表示过程激励噪声的协方差，它是状态转移矩阵与实际过程之间的误差。这个矩阵是卡尔曼滤波中比较难确定的一个量，一般有两种思路：一是在某些稳定的过程可以假定它是固定的矩阵，通过寻找最优的Q值使滤波器获得更好的性能，这是调整滤波器参数的主要手段，Q一般是对角阵，且对角线上的值很小，便于快速收敛；二是在**自适应卡尔曼滤波（AKF）**中Q矩阵是随时间变化的。

$K_k$：表示卡尔曼增益，是滤波的中间结果。

$z_k$：表示测量值，是m阶向量。

H：表示量测矩阵，是$m \times n$阶矩阵，它把m维测量值转换到n维与状态变量相对应。

R：表示测量噪声协方差，它是一个数值，这是和仪器相关的一个特性，作为已知条件输入滤波器。需要注意的是这个值过大过小都会使滤波效果变差，且R取值越小收敛越快，所以可以通过实验手段寻找合适的R值再利用它进行真实的滤波。

KF例子

单观测单状态单次测量(温度测量)

假设要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度$(\hat{x}k=A\hat{x}{k-1}+Bu_{k-1}=\hat{x}{k-1}=23^{\circ})$。同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5，卡尔曼滤波里面用的是方差=偏差的平方$P_k=AP{k-1}A^{T}+Q=P_{k-1}+Q=3^2+4^2=5^2$）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为$K_{k}=P_{k}H^{T}(HP_{k}H^{T}+R)^{-1}=P_{k}(P_{k}+R)^{-1}=\frac{5^2}{5^2+4^2}=0.609$，我们可以估算出k时刻的实际温度值是：$x_{k}=x_{k}+K_{k}(z_{k}-Hx_{k})=x_{k}+K_{k}(z_{k}-x_{k})=23+0.609 \times (25-23)=24.218$度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.218度）的偏差。算法如下：$P_{k}=(I-K_{k}H)P_{k}=(I-K_{k})P_{k}=(1-0.609) \times 5^2=3.126^2$。这里的5就是上面的k时刻预测的那个23度温度值的偏差，得出的3.126就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！

单观测单状态连续测量(温度测量)

假设现在对一个房间内的温度进行测试，房间内温度初值为25°c，每过一个时间周期，温度$x_{k}$都将在上一个周期温度的基础上变动$\omega_k$，于是可以建立状态差分方程$x_k=x_{k-1}+\omega_k$

再假设有一个误差$v_k$比较大的温度计（如果误差不大也不需要卡尔曼滤波了），通过温度计可以得到测量值$z_k$，于是建立观测方程$z_k=x_k+v_k$

每当经过一个时间周期，用上一周期得到的后验估计量来估计下一周期的先验估计量，由后验协方差估计先验协方差。由先验协方差计算卡尔曼增益，由卡尔曼增益计算下一周期的后验估计量和后验协方差。

matlab程序仿真：

function main
close all
N=120;%迭代次数
CON=25;%期望值
Xexpect=CON*ones(1,N);%期望值拷贝
X=zeros(1,N);%真值
Z=zeros(1,N);%观测值
Xkf=zeros(1,N);%估计值
P=zeros(1,N);%估计协方差

%赋初值
Xkf(1)=24.9;
P(1)=0.01;

%噪声
Q=0.001;
R=0.25;
W=sqrt(Q)*randn(1,N);   %方差决定过程噪声大小
V=sqrt(R)*randn(1,N);   %方差决定观测噪声大小

%系统矩阵
F=1;
G=1;
H=1;
I=eye(1);   %本系统状态为一维
    
%第一步，随时间推移，房间真实温度波动变化
%k时刻的真实温度，温度计是不知道的，但是它又是真实存在的，因此我们要过滤杂波，尽量收敛到真实温度
X=F*Xexpect+G*W;

%第二步，随着时间推移，获取实时数据
%温度计对k时刻房间温度的测量，Kalman滤波是站在温度计角度进行的。
%他不知道此刻真实温度，只能站在本次测量值和上一次估计值来做处理。其目的是最大限度降低测量噪声R的影响。尽可能的逼近X（k);
Z=H*X+V;
    
%模拟房间温度及测量过程，并滤波
for k=2:N 
    %第三步，卡尔曼滤波。
    %有了k时刻的观测和k-1时刻的状态就可以滤波了。
    X_pre=F*Xkf(k-1);      %状态预测
    P_pre=F*P(k-1)*F'+Q;    %协方差预测
    Kg=P_pre*inv(H*P_pre*H'+R); %计算卡尔曼增益
    e=Z(k)-H*X_pre;     %新息
    Xkf(k)=X_pre+Kg*e;  %状态更新
    P(k)=(I-Kg*H)*P_pre;    %协方差更新
end

%计算误差
Err_Messure=zeros(1,N); %测量值与期望值之间的偏差
Err_Kalman=zeros(1,N);  %估计值与期望值之间的偏差
for k=1:N
    Err_Messure(k)=Z(k)-Xexpect(k);
    Err_Kalman(k)=Xkf(k)-Xexpect(k);
end

t=1:N;
figure; %画图显示
%依次输出理论值，叠加过程噪声的真实值
%温度计测量值，卡尔曼估计值
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-ko',t,Xkf,'-g*');
legend('期望值','真实值','观测值','卡尔曼滤波值');
xlabel('采样时间/s');
ylabel('温度值/c');

%误差分析图
figure %画图显示
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k*');
legend('测量偏差','卡尔曼滤波偏差');
xlabel('采样时间/s');
ylabel('温度值/c');

单观测多状态连续测量(温度测量)

如下是根据以上温度预测范例修改后的结果，主要改进是：原程序中温度一直保持24度不变，这种情况下预测量是1维（温度）；本程序修改温度为时变值，这种情况下要求预测量成为2维（温度和变化间隔），状态转移矩阵A也成为矩阵，而不是简单的1。

function main
close all

% 初始化参数
n_iter = 300;       % 计算连续n_iter个时刻
n = 2;              % [温度，变化间隔]
m = 1;              % 温度

x = zeros(n, n_iter);           % 状态量，每个时刻包含两个值：温度，变化间隔
P = zeros(n, n, n_iter);        % 温度及变化间隔的方差
z = zeros(m, n_iter);           % 观测量，每个时刻包含一个值：温度

xrate  = 0.01;                     % 温度变化率
xreal = 24+(0:n_iter-1)*xrate;  % 真值，温度变化间隔为1
z = xreal + randn(m,n_iter);    % 观测量，误差方差为1

kf_para.A = [1,xrate;0,1];        % 状态转移矩阵，根据上一时刻预测当前时刻值
kf_para.H = zeros(m,n);         % 观测矩阵，把m维观测量转换成n维状态量
kf_para.H(1)=1;					% [温度] = [1, 0][温度, 变化间隔]^T = [温度]
kf_para.Q = [4e-4, 0; 0, 1e-4]; % 预测误差的方差
kf_para.R = 0.25;               % 观察误差的方差

x(:,1) = [23.5; 0.9];             % 初始化，第一个时刻的温度及变化间隔
P(:,:,1) = diag(rand(n,1));

% 卡尔曼滤波迭代
for ii = 2:n_iter
    [x(:,ii), P(:,:,ii)] = kf_iter(kf_para, x(:,ii-1), P(:,:,ii-1), z(ii));
end

% 结果打印
figure;
plot(x(1,:),'r*'); hold; plot(z,'bo'); plot(xreal, 'k.');
legend('卡尔曼滤波后的温度', '观测温度', '真实温度');
title('温度对比');
ylabel('温度');

figure;
plot(x(2,:));
title('滤波后的温度变化间隔（真实值为1）');

fprintf('观测误差均值为：%5.3f；均方差为：%5.3f\n', mean(abs(z-xreal)), std(z-xreal));
fprintf('滤波误差均值为：%5.3f；均方差为：%5.3f\n', mean(abs(x(1,:)-xreal)), std(x(1,:)-xreal));

卡尔曼滤波函数：

function [ x, P ] = kf_iter( kf_para, x_k_1, P_k_1, z )
% kf_para：卡尔曼滤波器的参数，A、H、Q、R
% x_k_1，P_k_1：上一观测时刻的最优预测值及方差
% z：当前时刻的观测值
% x, P：当前时刻的最优预测值及方差

n = size(x_k_1,1);

% 根据前值预测当前值
xbar = kf_para.A*x_k_1;
Pbar = kf_para.A*P_k_1*kf_para.A' + kf_para.Q;

%计算卡尔曼增益
K = Pbar*kf_para.H'*pinv(kf_para.H*Pbar*kf_para.H'+kf_para.R); 

% 根据卡尔曼增益和当前观测值修订预测值，获得当前最优预测值
x = xbar+K*(z-kf_para.H*xbar);
P = (diag(ones(n,1))-K*kf_para.H)*Pbar;

end

单观测多状态连续测量(位移测量)

假设对车子运动状态进行测试，状态量为$x_k=(x,v_x)^T$，控制量为$u_k=g$，观测量为小车的位移。每过一个时间周期，小车的位移都在上一个时间周期的基础上变动，建立状态差分方程： $$ \begin{align} x_k &= Ax_{k-1}+Bu_{k}+\omega_{k}\ \begin{bmatrix} x \ v_x \end{bmatrix}k &=\begin{bmatrix} 1 & \Delta t \ 0 & 1 \end{bmatrix} \begin{bmatrix} x \ v_x \end{bmatrix}{k-1}+ \begin{bmatrix} \frac{\Delta t^2}{2} \ \Delta t \end{bmatrix}_k \cdot g + \omega_k \ &=\begin{bmatrix} x+v_x \Delta t+\frac{g \Delta t^2}{2} \ v_x+g \Delta t \end{bmatrix}+\omega_k \end{align} $$ 通过观察可以得到小车的位移，建立观测方程： $$ \begin{align} z_k&=Cx_k+v_k \ &=\begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} x \ v_x \end{bmatrix}_k +v_k \ &=x_k+v_k \end{align} $$ 假设车子已经运动9个时间周期后，才开始估计。初始估计量和协方差矩阵均为0.

matlab程序仿真：

function main
close all
% 初始化参数 
delta_t = 0.1; % 采样时间 
t = 0:delta_t:5; 
N = length(t); % 序列的长度 

n = 2; % [位移，速度]
m = 1; % 位移

Q =[0 0;0 9e-1]; % 系统噪声，噪声方差叠加在速度上，大小为n*n方阵，n=状态向量的维数 
R = 10; % 位置测量方差估计，可以改变它来看不同效果，大小为m*m仿真，m=观测量的维数 

g = 10; % 加速度值 
x = 1/2*g*t.^2; % 实际真实位置 
z = x + sqrt(R).*randn(1,N); % 测量时加入测量白噪声 

A = [1 delta_t; 0 1]; % 状态转移矩阵，n*n 
B = [1/2*delta_t^2; delta_t]; % 控制矩阵，n*m 
H = [1,0]; % 观测矩阵，m*n

xhatminus = zeros(n, N); % x的先验估计 
Pminus = zeros(n, n); % 先验方差估计 n*n 
xhat = zeros(n, N); % x的后验估计 
P = zeros(n, n); % 后验方差估计 n*n 
K = zeros(n, m); % Kalman增益 n*m 
I = eye(n, n);

% 估计的初始值都为默认的0，即P=[0 0;0 0],xhat=0 
for k = 9:N %假设车子已经运动9个delta_T才开始估计 
    % 时间更新过程 
    xhatminus(:,k) = A*xhat(:,k-1)+B*g; 
    Pminus = A*P*A'+Q; % 测量更新过程 
    K = Pminus*H'*inv( H*Pminus*H'+R ); 
    xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k)); 
    P = (I-K*H)*Pminus; 
end 

figure 
plot(t,z); 
hold on 
plot(t,xhat(1,:),'r-') 
plot(t,x(1,:),'g-'); 
legend('含有噪声的测量', '后验估计', '真值'); 
xlabel('Iteration'); 

多观测多状态连续测量(分段)

假设一平面目标状态向量为$X=(x,y)$，初始位置为$(1000m,8000m)$，初始加速度为零，采样时间间隔$T=2s$，测量噪声为100m。前400s为匀速运动，沿y轴负方向，$V=\dot{X}=(0,-12m/s)^T$；401s到600s目标向x轴方向90度慢转弯，x轴方向加速度$0.075m/s^2$；601s到610s目标恢复匀速运动，加速度为零；611s到660s目标进行90度快转弯，x轴方向加速度$-0.3m/s^2$；然后目标匀速运动至观测结束。

从题目中可得目标点在二维平面内移动，x轴方向状态变量为$(s_x,v_x,a_x)$，y轴方向状态变量为$(s_y,v_y)$，所以整个目标状态可以用$x=(s_x,v_x,a_x,s_y,v_y)$五维向量表示。

初始状态为： $$ x=\begin{bmatrix} 1000 \ 0 \ 0 \ 800 \ -12 \end{bmatrix} $$ 状态转移矩阵： $$ F_x=\begin{bmatrix} 1 & T & \frac{T^2}{2} & 0 & 0 \ 0 & 1 & T & 0 & 0 \ 0 & 0 & 1 & 0 & 0 \ 0 & 0 & 0 & 1 & T \ 0 & 0 & 0 & 0 & 1 \end{bmatrix} $$ 测量转移矩阵： $$ H_k=\begin{bmatrix} 1 & 0 & 0 & 0 & 0 \ 0 & 0 & 0 & 1 & 0 \end{bmatrix} $$ 不考虑飞行器内部扰动，所以$\Gamma_k=\begin{bmatrix}0 & 0 & 0 & 0 & 0 \end{bmatrix}$。先验状态初始协方差： $$ P_{0|0}=diag(\sigma_x^2,\sigma_x^2/T,\sigma_x^2/T^2,\sigma_y^2,\sigma_y^2/T) $$ 以上部分为自己设计，影响的是收敛速度。

测量噪声协方差： $$ Q_k^v=diag(\sigma_x^2,\sigma_y^2) $$ matlab代码：

function main
close all
sigma = 10000; %测量误差 
T = 2; %间隔时间步长 
t1 = 400; %沿y轴匀速运动 
t2 = 600; %向x轴方向转弯 a = 0.075 
t3 = 610; %匀速运动 
t4 = 660; %90°快转弯 a = -0.3 
totaltime = 700; %总的观测时间为700秒 350步 
V_y = -12; 
ax = 0; 
V_x = 0; 
X_add = 0;

%初始值 
f_k1 = [1 T T^2/2;0 1 T;0 0 1]; 
f_k2 = [1 T;0 1]; 
F_k = blkdiag(f_k1,f_k2); %预测状态转移矩阵 
X_k1 = [1000 0 ax 800 -12]'; %状态初始值 

Z_k = [1000;800]; 
H_k = [1 0 0 0 0;0 0 0 1 0]; %观测矩阵为2x5维 
P_k1 = diag([sigma sigma/T sigma/T^2 sigma sigma/T]); %初始协方差矩阵 
Qvk = sigma*eye(2); %观测噪声协方差 

for i=1:450 
    if i>200 && i<=300 
        ax = 0.075; 
        X_k1(3,1) = ax; 
    end 
    if i>300 && i<=305 
        ax = 0; 
        X_k1(3,1) = ax; 
    end 
    if i>305 && i<=330 
        ax = -0.3; 
        X_k1(3,1) = ax; 
    end 
    if i>330 
        ax = 0; 
        X_k1(3,1) = ax; 
    end 
    X_add = X_add + V_x *T + 0.5*(ax*T^2); %x轴方向距离改变量 
    V_x = V_x + ax*T;
    Z_k = [1000+X_add+wgn(1,1,40);800+V_y*T*(i - 1)+wgn(1,1,40)]; 
    
    X_k2 = F_k*X_k1; %先验状态值 
    P_k2 = F_k*P_k1*F_k'; %先验协方差矩阵 
    
    Gk = P_k2*H_k'*inv(H_k*P_k2*H_k' + Qvk); %计算卡尔曼增益 
    P_k3 = (eye(5)-Gk*H_k)*P_k2; %后验协方差矩阵  
    X_k3 = X_k2 + Gk*(Z_k - H_k*X_k2); 
    
    P_k1 = P_k3; 
    X_k1 = X_k3; 
    X_plot(:,i) = X_k3; 
    Z_plot(:,i) = Z_k; 
end 

%画图
plot(Z_plot(1,:),Z_plot(2,:),X_plot(1,:),X_plot(4,:),'black'); 
axis([0 6000 -10000 1000]) 
title('目标轨迹'); 
xlabel('X(米)'); 
ylabel('Y(米)'); 
legend('目标测量轨迹','目标真实轨迹');

单观测单状态多传感器

方法和以下KF和EKF并存的例子一样。对于多个传感器测量同一个观测值，对每个传感器的观测值构建一个KF滤波器。滤波器的预测过程是一样的，估计过程依赖不同传感器的观测方差和观测矩阵而变化。将上一个KF滤波器的输出作为下一个KF滤波器的预测值，依次经过多个KF滤波器，最终输出估计结果。

EKF例子

匀速圆周运动

设一平面目标状态向量为$X=(x,y)$，以坐标原点为圆心，半径为500米进行匀速圆周运动，速度为$10m/s$；目标运动的过程噪声方差为$2m^2$ 的白噪声。现有一个位于$（750，750）$的雷达覆盖该目标，进行监视，观测量为径向距离d，噪声为$3m^2$，监视时间间隔为0.5秒。 针对以上场景进行目标跟踪滤波设计和仿真？

解：已知$v=10m/s$可以算出角速度$w=v/r=10/500(rad/s)$；根据题意及以上参数可得出雷达所测的径向距离： $$ d_t=f_t(\theta_t)=((X_r-r\cos\theta_t)^2+(Y_r-r\sin\theta_t)^2)^{1/2} $$ 其中$\dot{\theta}=wt$，$X_r=750$，$Y_r=750$。定义状态向量： $$ X_t= \begin{bmatrix} d_t \ \theta_t \end{bmatrix} $$ 状态向量中两个分量是相关的，第一个分量是第二个分量的函数。将状态向量以差分方式表示，因为径向距离是直接关于$theta$的函数，因此可对其直接用一阶泰勒展开： $$ d_{t+1} \approx d_t+\frac{\partial f}{\partial \theta}\Delta \theta =d_t+f_t^{\theta}wT $$ 其中$T$为监视时间间隔为0.5。

因而状态向量的差分方程组形式为： $$ \begin{cases} \hat{d}{t+1}=\hat{d}t+f_t^{\theta}wT \ \hat{\theta}{t+1}=\hat{\theta}t+wT \end{cases} $$ 状态误差： $$ \begin{cases} \tilde{d}{t+1}=d{t+1}-\hat{d}{t+1} \ \tilde{\theta}{t+1}=\theta_{t+1}-\hat{\theta}{t+1} \end{cases} $$ 状态误差转移方程为： $$ \begin{bmatrix} \tilde{d}{t+1} \ \tilde{\theta}_{t+1} \end{bmatrix}= \begin{bmatrix} 1 & \frac{\partial f_t^{\theta}}{\partial \theta} \ 0 & 1 \end{bmatrix} \begin{bmatrix} \tilde{d}_t \ \tilde{\theta}_t \end{bmatrix} $$ 这里暂时不考虑噪声转移问题，将噪声视为加性噪声。 可得状态转移矩阵为： $$ F_t=\begin{bmatrix} 1 & \frac{\partial f_t^{\theta}}{\partial \theta} \ 0 & 1 \end{bmatrix} $$ 注意到里面对角线上面的是二阶导。测量转移矩阵： $$ H_t= \begin{bmatrix} 1 \ 0 \end{bmatrix} $$ 过程噪声方差： $$ Q_t^w= \begin{bmatrix} 2 & 0 \ 0 & 0.2 \end{bmatrix} $$ 0.2是假设为$\theta$的过程噪声方差。测量噪声方差： $$ Q_t^v= \begin{bmatrix} 3 & 0 \ 0 & 0.3 \end{bmatrix} $$ 0.3是假设为$\theta$的测量噪声方差。假设初始状态值为： $$ X_0= \begin{bmatrix} f_0(0) \ 0 \end{bmatrix} $$ 误差协方差矩阵设定为： $$ P_0= \begin{bmatrix} 2 & 0 \ 0 & 1 \end{bmatrix} $$ matlab代码：

function main
close all
N = 2000; % 测量次数
phi = 0; % 初始角度
y0 = 750; % 雷达坐标
x0 = 750; % 雷达坐标
T = 0.5; % 测量时间间隔
v = 10; % 匀速圆周线速度
r = 500; % 匀速圆周运动半径
w = v/r; % 匀速圆周运动角速度
%f(phi) = sqrt((x0 - r*cos(phi))^2 + (y0 - r*sin(phi))^2) % 真值 
f =@(phi)((x0 - r*cos(phi))^2 + (y0 - r*sin(phi))^2)^(1/2); 
%f_2_order_diff(phi) = w*T*diff(f(phi),2) % 状态转移矩阵，f的二阶导数 
f_2_order_diff =@(phi)(((r*cos(phi)*x0+r*sin(phi)*y0)/((x0-r*cos(phi))^2+(y0-r*sin(phi))^2)^(3/2))-(((r*sin(phi)*x0-r*cos(phi)*y0))^2/(((x0-r*cos(phi))^2+(y0-r*sin(phi))^2)^(5/2))));

%过程噪声方差 
Qw = diag([2 0.2]); 
%测量噪声 
Qv = diag([3 0.3]); 
%状态值初始化 
x_k1 = [f(phi) 0]'; 
%初始协方差 
p_k1 = diag([2,1]); 
%状态转移矩阵 
F = [1 f_2_order_diff(phi);0 1]; 
%测量转移矩阵 
H = [1 0;0 1]; 
%过程噪声 
W_noise = diag([normrnd(0,2);normrnd(0,0.2)])*randn(2,4000); 
%测量噪声 
V_noise = diag([normrnd(0,3);normrnd(0,0.1)])*randn(2,4000); 

Z_K = zeros(2, N);
X_K = zeros(2, N);

for i = 1:N
    phi = phi + w*T; 
    x_k2 = F*x_k1 + W_noise(:,i); %下一时刻的先验状态预测   
    p_k2 = F*p_k1*F' + Qw; 
    k = p_k2*H'*inv(H*p_k2*H' + Qv); 
    
%    z_k1 =[f(phi)+wgn(1,1,10) 0]'; 
	z_k1 = [f(phi)+V_noise(1,i) 0]';
    z_k = H*z_k1; 
    Z_K(:,i) = z_k; 
    
    x_k1 = x_k2 + k*(z_k - H*x_k2); 
    X_K(:,i) = x_k1; 
    p_k1 = (eye(2) - k*H)*p_k2; 
end 

t = linspace(1,2000,2000); 
subplot(2,1,1)
plot(t,X_K(1,:),t,Z_K(1,:),'black'); 
legend('预测轨迹','真值轨迹');

subplot(2,1,2)
plot(t,(X_K(1,:)-Z_K(1,:)),'-g');
legend('预测轨迹与真实轨迹之间的误差');

抛物运动

从空中水平抛射出的物体，初始水平速度$v_x(0)$，初始位置坐标$(x(0),y(0))$，受重力$g$和阻尼力影响，阻尼力与速度平方成正比，水平和垂直阻尼系数分别为$k_x,k_y$，还存在不确定的零均值白噪声干扰力$\delta a_x,\delta a_y$。在坐标原点处有一观测设备（不妨想象成雷达），可测得距离r（零均值白噪声误差$\delta r$）、与Y轴角度$\alpha$（零均值白噪声误差$\delta \alpha$）。

根据图中情景，选取横向位置、速度，纵向位置、速度为状态量，列出非线性状态方程及观测方程： $$ \begin{cases} X(k+1)=X(k)+V_x(k) \cdot T+q_1 \ V_x(k+1)=V_x(k)-(k_xV_x^2(k)) \cdot T+q_2 \ Y(k+1)=Y(k)+V_y(k) \cdot T+q_3 \ V_y(k+1)=V_y(k)+(k_yV_y^2(k)-g) \cdot T+q_4 \end{cases} $$

$$ \begin{cases} r=\sqrt{x^2+y^2}+v_1 \ \alpha=\arctan \frac{x}{y}+v_2 \end{cases} $$

根据状态方程和观测方程，计算雅可比矩阵： $$ \Phi(k)=\begin{bmatrix} 1 & T & 0 & 0 \ 0 & 1-2k_xV_x(k) \cdot T & 0 & 0 \ 0 & 0 & 1 & T \ 0 & 0 & 0 & 1+2k_yV_y \cdot T \end{bmatrix} $$

$$ H(k)=\begin{bmatrix} \frac{x}{r} & 0 & \frac{y}{r} & 0 \ \frac{1/y}{1+\frac{x}{y}^2} & 0 & \frac{x/y^2}{1+\frac{x}{y}^2} & 0 \end{bmatrix} $$

matlab代码：

function main
close all

%%  真实轨迹模拟
kx = .01;   ky = .05;       % 阻尼系数
g = 9.8;                    % 重力
t = 15;                     % 仿真时间
Ts = 0.1;                   % 采样周期 
len = fix(t/Ts);            % 仿真步数
dax = 3; day = 3;           % 系统噪声
X = zeros(len,4); 
X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值
for k=2:len
    x = X(k-1,1); 
    vx = X(k-1,2); 
    y = X(k-1,3); 
    vy = X(k-1,4); 
    x = x + vx*Ts;
    vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;
    y = y + vy*Ts;
    vy = vy + (ky*vy^2-g+day*randn(1))*Ts;
    X(k,:) = [x, vx, y, vy];
end

%%  构造量测量
dr = 8; 
dafa = 0.1;        % 量测噪声
for k=1:len
    r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
    a = atan(X(k,1)/X(k,3))*57.3 + dafa*randn(1,1);
    Z(k,:) = [r, a];
end

%% ekf 滤波
Qk = diag([0; dax/10; 0; day/10])^2;
Rk = diag([dr; dafa])^2;
Pk = 10*eye(4);
Pkk_1 = 10*eye(4);
x_hat = [0,40,400,0]';
X_est = zeros(len,4);
x_forecast = zeros(4,1);
z = zeros(4,1);
for k=1:len
    % 1 状态预测    
    x1 = x_hat(1) + x_hat(2)*Ts;
    vx1 = x_hat(2) + (-kx*x_hat(2)^2)*Ts;
    y1 = x_hat(3) + x_hat(4)*Ts;
    vy1 = x_hat(4) + (ky*x_hat(4)^2-g)*Ts;
    x_forecast = [x1; vx1; y1; vy1];        %预测值
    % 2  观测预测
    r = sqrt(x1*x1+y1*y1);
    alpha = atan(x1/y1)*57.3;
    y_yuce = [r,alpha]';
    %  状态矩阵
    vx = x_forecast(2);  vy = x_forecast(4);
    F = zeros(4,4);
    F(1,1) = 1;  F(1,2) = Ts;
    F(2,2) = 1-2*kx*vx*Ts;
    F(3,3) = 1;  F(3,4) = Ts;
    F(4,4) = 1+2*ky*vy*Ts;
    Pkk_1 = F*Pk*F'+Qk;
    % 观测矩阵
    x = x_forecast(1); y = x_forecast(3);
    H = zeros(2,4);
    r = sqrt(x^2+y^2);  xy2 = 1+(x/y)^2;
    H(1,1) = x/r;  H(1,3) = y/r;
    H(2,1) = (1/y)/xy2;  H(2,3) = (-x/y^2)/xy2;
    
    Kk = Pkk_1*H'*(H*Pkk_1*H'+Rk)^-1;       %计算增益
    x_hat = x_forecast+Kk*(Z(k,:)'-y_yuce);      %校正
    Pk = (eye(4)-Kk*H)*Pkk_1;
    X_est(k,:) = x_hat;
end
%% 
figure, hold on, grid on;
plot(X(:,1),X(:,3),'-b');
plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180));
plot(X_est(:,1),X_est(:,3), 'r');
xlabel('X'); 
ylabel('Y'); 
title('EKF simulation');
legend('real', 'measurement', 'ekf estimated');
axis([-5,230,290,530]);

KF和EKF并存例子

激光雷达和雷达融合(多传感器融合)

传感器融合实例

以汽车跟踪为例，目标是知道汽车时刻的状态$x=(p_x,p_y,v_x,v_y)$。

已知的传感器有lidar、radar。

• lidar：笛卡尔坐标系。可检测到位置，没有速度信息。其测量值$z=(p_x,p_y)$

• radar：极坐标系。可检测到距离，角度，速度信息，但是精度较低。其测量值$z=(\rho,\phi,\dot{\rho})$

传感器融合步骤：

1. 第一组测量值，用于对状态$x$进行初始化；
2. 相同的预测过程；
3. 若当前仅有lidar观测，则构建lidar观测矩阵，在预测的基础上进行估计；若当前仅有radar观测，则构建radar观测矩阵，在预测的基础上进行估计；若当前同时有lidar观测和radar观测，则先在预测的基础上用lidar数据进行估计，再在lidar数据估计的基础上用radar数据进行估计。

• 初始化

  初始化，指在收到第一个测量值后，对状态$x$进行初始化。初始化如下，同时加上对时间的更新。

  对于lidar来说， $$ \begin{bmatrix} p_x \ p_y \ v_x \ v_y \end{bmatrix}= \begin{bmatrix} 1 & 0 \ 0 & 1 \ 0 & 0 \ 0 & 0 \end{bmatrix} \begin{bmatrix} p_x \ p_y \end{bmatrix} $$ 对于radar来说， $$ \begin{bmatrix} p_x \ p_y \ v_x \ v_y \end{bmatrix}= \begin{bmatrix} \rho\cos\phi \ \rho\sin\phi \ \dot{\rho}\cos\phi \ \dot{\rho}\sin\phi \end{bmatrix} $$

• 预测未来

  预测主要涉及的公式是： $$ \begin{align} x' &=Fx \ P' &=FPF^T+Q \end{align} $$ 需要求解的有三个变量：F、P、Q。

  F表明了系统的状态如何改变，这里仅考虑线性系统，F易得： $$ Fx= \begin{bmatrix} 1 & 0 & dt & 0 \ 0 & 1 & 0 & dt \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} p_x \ p_y \ v_x \ v_y \end{bmatrix}= \begin{bmatrix} p_x+v_x \cdot dt \ p_y+v_y \cdot dt \ v_x \ v_y \end{bmatrix} $$ P表明了系统状态的不确定性程度，用x的协方差表示，这里自己指定为： $$ P= \begin{bmatrix} 1 & 0 & 0 & 0 \ 0 & 1 & 0 & 0 \ 0 & 0 & 1000 & 0 \ 0 & 0 & 0 & 1000 \end{bmatrix} $$ Q表明了$x′=Fx$未能刻画的其他外界干扰。本例子使用线性模型，因此加速度变成了干扰项。$x′=Fx$中未衡量的额外项目v为： $$ v= \begin{bmatrix} \frac{a_x(dt)^2}{2} \ \frac{a_y(dt)^2}{2} \ a_xdt \ a_ydt \end{bmatrix}= \begin{bmatrix} \frac{(dt)^2}{2} & 0 \ 0 & \frac{(dt)^2}{2} \ dt & 0 \ 0 & dt \end{bmatrix} \begin{bmatrix} a_x \ a_y \end{bmatrix}=Ga $$ v服从高斯分布N(0,Q)。 $$ \begin{align} Q &= E[vv^T]=E[Gaa^TG^T]=GE[aa^T]G^T \ &= G \begin{bmatrix} \sigma_{ax}^2 & 0 \ 0 & \sigma_{ay}^2 \end{bmatrix}G^T \ &= \begin{bmatrix} \frac{(dt)^4}{4}\sigma_{ax}^2 & 0 & \frac{(dt)^3}{2}\sigma_{ax}^2 & 0 \ 0 & \frac{(dt)^4}{4}\sigma_{ay}^2 & 0 & \frac{(dt)^3}{2}\sigma_{ay}^2 \ \frac{(dt)^3}{2}\sigma_{ax}^2 & 0 & (dt)^2\sigma_{ax}^2 & 0 \ 0 & \frac{(dt)^3}{2}\sigma_{ay}^2 & 0 & (dt)^2\sigma_{ay}^2 \end{bmatrix} \end{align} $$

• 修正当下(lidar)

  lidar使用了KF。这里牵涉到的公式主要是： $$ \begin{align} y &= z-Hx \ S &= HPH^T+R \ K &= PH^TS^{-1} \ x' &= x+Ky \ P' &= (I-KH)P \end{align} $$ 需要求解的有两个变量：H、R。

  H表示了状态空间到测量空间的映射。 $$ Hx=\begin{bmatrix} 1 & 0 & 0 & 0 \ 0 & 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} p_x \ p_y \ v_x \ v_y \end{bmatrix}= \begin{bmatrix} p_x \ p_y \end{bmatrix} $$ R表示了测量值的不确定度，一般由传感器的厂家提供，这里lidar参考如下： $$ R_{lidar}=\begin{bmatrix} 0.0225 & 0 \ 0 & 0.0225 \end{bmatrix} $$

• 修正当下(radar)

  radar使用了EKF。这里牵涉到的公式主要是： $$ \begin{align} y &= z-f(x) \ S &= H_jPH_j^T+R \ K &= PH_j^TS^{-1} \ x' &= x+Ky \ P' &= (I-KH_j)P \end{align} $$ 区别与上面lidar的主要有：

  1. 状态空间到测量空间的非线性映射h(x)
  2. 非线性映射线性化后的Jacob矩阵
  3. radar的观测方差为$R_{radar}$

  状态空间到测量空间的非线性映射f(x)如下 $$ h(x)=\begin{bmatrix} \rho \ \phi \ \dot{\rho} \end{bmatrix}= \begin{bmatrix} \sqrt{p_x^2+p_y^2} \ \arctan\frac{p_y}{p_x} \ \frac{p_xv_x+p_yv_y}{\sqrt{p_x^2+p_y^2}} \end{bmatrix} $$ 非线性映射线性化后的Jacob矩阵$H_j$ $$ H_j=\frac{\partial h(x)}{\partial x}= \begin{bmatrix} \frac{\partial \rho}{\partial p_x} & \frac{\partial \rho}{\partial p_y} & \frac{\partial \rho}{\partial v_x} & \frac{\partial \rho}{\partial v_y} \ \frac{\partial \phi}{\partial p_x} & \frac{\partial \phi}{\partial p_y} & \frac{\partial \phi}{\partial v_x} & \frac{\partial \phi}{\partial v_y} \ \frac{\partial \dot{\rho}}{\partial p_x} & \frac{\partial \dot{\rho}}{\partial p_y} & \frac{\partial \dot{\rho}}{\partial v_x} & \frac{\partial \dot{\rho}}{\partial v_y} \end{bmatrix} $$ R表示了测量值的不确定度，一般由传感器的厂家提供，这里radar参考如下： $$ R_{radar}=\begin{bmatrix} 0.09 & 0 & 0 \ 0 & 0.0009 & 0 \ 0 & 0 & 0.09 \end{bmatrix} $$

多传感器融合的示例如下，需要注意的有：

1. lidar和radar的预测部分是完全相同的
2. lidar和radar的参数更新部分是不同的，不同的原因是不同传感器收到的测量值是不同的
3. 当收到lidar或radar的测量值，依次执行预测、更新步骤
4. 当同时收到lidar和radar的测量值，依次执行预测、更新1、更新2步骤