最近正好准备想试试利用EKF实现多传感器的融合。但没想到本身ROS里面就已经有EKF的功能包了。这个包用于评估机器人的3D位姿，使用了来自不同源的位姿测量信息，它使用带有6D（3D position and 3D orientation）模型信息的扩展卡尔曼滤波器来整合来自轮子里程计，IMU传感器和视觉里程计的数据信息。基本思路就是用松耦合方式融合不同传感器信息实现位姿估计。下面来学习一下

节点订阅的topic

odom（nav_msgs/Odometry.msg）

imu_data(sensor_msgs/Imu.msg)

VO/视觉里程计（nav_msgs/Odometry.msg）

发布的topics

robot_pose_ekf/odom_combined（geometry_msgs/PoseWithCovarianceStamped.msg）

tf变换

robot_pose_ekf节点的工作原理

姿势解释Pose interpretation

协方差解释Covariance interpretation

robot_pose_ekf节点

概要

robot_pose_ekf 是 ROS Navigation stack 中的一个包,通过扩展卡尔曼滤波器对 imu、里程计 odom、视觉里程计 vo 的数据进行融合,来估计平面移动机器人的真实位置姿态,输出 odom_combined 消息。

Robot Pose EKF 包用于评估机器人的3D位姿，基于来自不同来源的位姿测量信息。
它使用带有6D（3D position and 3D orientation）模型信息的扩展卡尔曼滤波器来整合来自轮式里程计，IMU传感器和视觉里程计的数据信息。换言之，如果要改造这个包，则需要其他传感器输出的位姿也按照它的结构。
基本思路就是用松耦合方式融合不同传感器信息实现位姿估计。

配置

该包的源码： https://github.com/ros-planning/robot_pose_ekf

先将这个包下载下来,然后放置于工作空间。EKF节点的默认启动文件位于robot_pose_ekf包目录中

先打开启动文件robot_pose_ekf.launch，里面有些参数是可以被修改的

<launch>

<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
  <param name="output_frame" value="odom_combined"/>
  <param name="base_footprint_frame" value="base_footprint"/>

  <param name="freq" value="30.0"/>
  <!-- freq: 滤波器更新和发布频率，注意：频率高仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不表示可以提高每次位姿评估的精度 -->
  <param name="sensor_timeout" value="1.0"/>  
  <!-- sensor_timeout: 当某传感器停止给滤波器发送信息时，滤波器应该等多长时间方才可以在没有该传感器信息状况下继续工作 -->

  <!-- 下面定义输入的源，如果需要输入其他传感器的，则对应的地方修改即可 -->
  <param name="odom_used" value="true"/>
  <param name="imu_used" value="true"/>
  <param name="vo_used" value="true"/>

  <remap from="odom" to="pr2_base_odometry/odom" />
</node>

</launch>

可以使用 remap 将其映射到新名称的 topic上。重映射是基于替换的思想,每个重映射包含一个原始名称和一个新名称。每当节点使用重映射中的原始名称时,ROS 客户端库就会将它默默地替换成其对应的新名称。

cm一下即可编辑该功能包，但是报错了

原来是需要先安装

rosdep install robot_pose_ekf

然后在cm则成功了哈～

节点订阅的topic

odom（nav_msgs/Odometry.msg）

2D pose (used by wheel odometry): 该2D pose包含了机器人在地面的位置（position）和方位（orientation）信息以及该位姿的协方差（covariance）。
用来发送该2D位姿的消息实际上表示了一个3D位姿，只不过把z，pitch和roll分量简单忽略了。

原始消息定义如下所示

# This represents an estimate of a position and velocity (速度) in free space.  
# The pose in this message should be specified in the coordinate frame given by header.frame_id.
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

缩消息定义

std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

imu_data(sensor_msgs/Imu.msg)

3D orientation (used by the IMU): 3D方位提供机器人底座相对于世界坐标系的Roll, Pitch and Yaw偏角。 Roll and Pitch角是绝对角度（因为IMU使用了重力参考），而YAW角是相对角度。协方差矩阵用来指定方位测量的不确定度。当仅仅收到这个主题消息时，机器人位姿ekf还不会启动，因为它还需要来自主题’vo’或者’odom’的消息。

关于欧拉角的求解，可以参考博客《ROS学习笔记之——机器人航向角的求解》和《 ROS学习笔记之——机器人初始姿态角的确定》

原始消息定义如下所示

# This is a message to hold data from an IMU (Inertial Measurement Unit)
#
# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the 
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
# data a covariance will have to be assumed or gotten from some other source
#
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 
# estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each 
# covariance matrix, and disregard the associated estimate.

Header header

geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z

缩消息定义

std_msgs/Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance

VO/视觉里程计（nav_msgs/Odometry.msg）

3D pose (used by Visual Odometry): 3D位姿可以完整表示机器人的位置和方位并给出位姿协方差。当用传感器只测量部分3D位姿(e.g. the wheel odometry only measures a 2D pose)时候, 可以给还未真正开始测量的部分3D位姿先简单指定一个大的协方差。

原始消息定义如下所示

# This represents an estimate of a position and velocity in free space.  
# The pose in this message should be specified in the coordinate frame given by header.frame_id.
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

缩消息定义

std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

robot_pose_ekf node并不需要所有3个传感器源一直同时可用。每个源都能提供位姿和协方差，且这些源以不同速率和延时工作。随着时间推移某个源可能出现和消失（比如某个传感器突然不工作了），该node可以自动探测当前可用的源。如果要把自己想使用的传感器加入到输入源中，请参考指南（http://wiki.ros.org/robot_pose_ekf/Tutorials/AddingGpsSensor）

发布的topics

robot_pose_ekf/odom_combined（geometry_msgs/PoseWithCovarianceStamped.msg）

滤波器输出 (估计的3D机器人位姿)

原始消息定义如下所示

# This expresses an estimated pose with a reference coordinate frame and timestamp

Header header
PoseWithCovariance pose

缩消息定义

std_msgs/Header header
geometry_msgs/PoseWithCovariance pose

tf变换

odom_combined → base_footprint

robot_pose_ekf节点的工作原理

姿势解释Pose interpretation

给滤波器node提供信息的所有传感器源都有自己的参考坐标系，并且随着时间推移都可能出现漂移现象。因此，每个传感器发出来的绝对位姿不能直接对比。因此该node使用每个传感器的相对位姿差异来更新扩展卡尔曼滤波器。

协方差解释Covariance interpretation

当机器人在四周移动时候，随着时间推移位姿不确定性会变得越来越大，协方差将无限增长。这样一来发布位姿自身协方差没有意义，因此传感器源公布协方差如何随时间变化（例如，速度的协方差）。请注意，使用世界观测（例如测量到已知墙的距离）将减少机器人姿势的不确定性; 然而，这是定位，而不是里程计

Timing

假定机器人上次更新位姿滤波器在t_0时刻，该node只有在收到每个传感器测量值（时间戳>t_0）之后才会进行下一次的滤波器更新。例如，在odom topic收到一条消息时间戳（t_1 > t_0）, 且在imu_data topic上也收到一条消息（其时间戳t_2 > t_1 > t_0）, 滤波器将被更新到所有传感器信息可用的最新时刻，这个时刻是t_1。在t_1时刻odom位姿直接给出了，但是imu位姿需要通过在t_0和t_2两时刻之间进行线性插值求得。在t_0到 t_1时间段，机器人位姿滤波器使用odom和IMU相对位姿进行更新。

下图是PR2机器人机器人的实验结果显示，从绿色初始位置开始移动最后回到出发位置。完美的odometry x-y曲线图应该是一个精确的闭环曲线图。上图蓝色线是轮式里程计的输入，蓝色点是其估计的结束位置。红色线是robot_pose_ekf的输出, robot_pose_ekf整合了轮式里程计和IMU的信息，给出了红色的结束位置点。

扩展卡尔曼滤波的原理介绍

这部分是数学部分，其实之前博客《学习笔记之——基于粒子滤波器的目标跟踪算法（内含卡尔曼滤波的学习笔记）》就学习过卡尔曼滤波了，来看看二者的区别吧～

对于kalman滤波，从一开始就是为线性系统设计的算法，不能用于非线性系统。在kalman滤波中，对于小车当前的状态（x，v），预测其下一秒的位置为这个就是一个线性系统的预测。而实际中往往都是非线性系统。

Kalman滤波

（一个不错的视频https://www.bilibili.com/video/av4356232?from=search&seid=7045130318637455155）

再付上几个不错的参考资料

http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

https://blog.csdn.net/u010720661/article/details/63253509

https://en.wikipedia.org/wiki/Extended_Kalman_filter

线性化卡尔曼滤波

线性化卡尔曼滤波和线性卡尔曼滤波器在滤波器的算法方面有同样的算法结构。不一样的地方在于这两者的系统模型不同。线性卡尔曼滤波器的系统本身就是线性系统，而线性化卡尔曼滤波器的系统本身是非线性系统，但是机智的大神们将非线性的系统进行了线性化，于是卡尔曼滤波就可以用在非线性系统中了。

但是我们在对非线性系统进行线性化的过程中，只有被线性化的那个点附近的线性化模型和真实的模型相近，远的误差就大了，那么这个时候卡尔曼滤波器的效果就不好。估计线性化卡尔曼滤波器会发散。而线性化卡尔曼的表现取决于线性化做得好不好

泰勒级数展开

泰勒级数展开是将一个在 $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(x)=\frac{f({{x}_{0}})}{0!}+\frac{f'({{x}_{0}})}{1!}(x-{{x}_{0}})+...+\frac{{{f}^{(n)}}({{x}_{0}})}{n!}{{(x-{{x}_{0}})}^{n}}+{{R}_{n}}(x)$

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

（著名的欧拉公式 ${{e}^{ix}}=\cos x +i\sin x$ 就是利用 ${{e}^{ix}}$ ， $\cos x$ 和 $\sin x$ 的泰勒展开式得来的！）

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

$f(\mathbf{x})=f({{\mathbf{x}}_{k}})+{{[\nabla f({{\mathbf{x}}_{k}})]}^{T}}(\mathbf{x}-{{\mathbf{x}}_{k}})+\frac{1}{2!}{{(\mathbf{x}-{{\mathbf{x}}_{k}})}^{T}}H({{\mathbf{x}}_{k}})(\mathbf{x}-{{\mathbf{x}}_{k}})+{{o}^{n}}$

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

${[\nabla f({{\bf{x}}_k})]^T} = {{\bf{J}}_F} = \left[ {\begin{array}{*{20}{c}} {\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_1}}}{{\partial {x_n}}}}\\ {\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_2}}}{{\partial {x_n}}}}\\ \vdots & \vdots & \ddots & \vdots \\ {\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_1}}}}&{\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_2}}}}& \cdots &{\frac{{\partial f{{({{\bf{x}}_k})}_m}}}{{\partial {x_n}}}} \end{array}} \right]$

${\bf{H}}({{\bf{x}}_k}) = \left[ {\begin{array}{*{20}{c}} {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_1^2}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_1}\partial {x_2}}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_1}\partial {x_n}}}}\\ {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_2}\partial {x_1}}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_2^2}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_2}\partial {x_n}}}}\\ \vdots & \vdots & \ddots & \vdots \\ {\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_n}\partial {x_1}}}}&{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial {x_n}\partial {x_2}}}}& \cdots &{\frac{{{\partial ^2}f({{\bf{x}}_k})}}{{\partial x_n^2}}} \end{array}} \right]$

这里， $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}}$

真实的状态值 ${{\mathbf{\theta }}_{k}}$ ，预测的状态值 $\mathbf{\theta }_{k}^{'}$ ，估计的状态值 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ ，观测值 ${{\mathbf{z}}_{k}}$ ，观测值的预测 $\mathbf{\hat{z}}_{k}$ ，估计值与真实值之间的误差协方差矩阵 ${{\mathbf{\Sigma }}_{k}}$ ，求期望的符号 $\left\langle \cdot \right\rangle$ 。.扩展卡尔曼滤波EKF的状态转移方程和观测方程为:

${{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}$ （1）（状态值）状态转移方程

${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}$ （2）（测量值）观测方程

其中， ${{\mathbf{s}}_{k}}\sim \mathcal{N}(0,\mathbf{Q})$ 以及 ${{\mathbf{v}}_{k}}\sim \mathcal{N}(0,\mathbf{R})$ 为加性高斯噪声

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

${{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{F}}_{k-1}}\left( {{\mathbf{\theta }}_{k-1}}-\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle \right)+{{\mathbf{s}}_{k}}$ （3）

再利用泰勒展开式对（2）式在本轮的状态预测值 $\mathbf{\theta }_{k}^{'}$ 处展开得

${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}=h\left( \mathbf{\theta }_{{k}}^{\mathbf{'}} \right)+{{\mathbf{H}}_{k}}\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{{k}}^{\mathbf{'}} \right)+{{\mathbf{v}}_{k}}$ （4）

其中， ${\mathbf{F}}_{k-1}$ 和 ${\mathbf{H}}_{k}$ 分别表示函数 $f(\mathbf{\theta })$ 和 $h(\mathbf{\theta })$ 在 $\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle$ 和 $\mathbf{\theta }_{k}^{'}$ 处的雅克比矩阵。这里对泰勒展开式只保留到一阶导，二阶导数以上的都舍去，噪声假设均为加性高斯噪声

由公式3、4可以得到下面两个等式

$f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )={{\mathbf{F}}_{k-1}}\left( {{\mathbf{\theta }}_{k-1}}-\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle \right)$ （6）注意：为上一状态的估计值

$h({{\mathbf{\theta }}_{k}})-h\left( \mathbf{\theta }_{{k}}^{\mathbf{'}} \right)={{\mathbf{H}}_{k}}\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{{k}}^{{'}} \right)$ （7）注意：真实的状态值 ${{\mathbf{\theta }}_{k}}$ ，预测的状态值 $\mathbf{\theta }_{k}^{'}$

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

Propagation：

$\mathbf{\theta }_{k}^{'}=f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle)$ 上一个状态的估计值，通过f函数来预测当前的状态值

下面计算一下估计值 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ 与真实值 ${{\mathbf{\theta }}_{k}}$ 之间的误差协方差矩阵 ${{\mathbf{\Sigma }}_{k}}$

观测方程在本轮的状态预测值 $\mathbf{\theta }_{k}^{'}$ 处展开得 ${{\mathbf{z}}_{k}}=h({{\mathbf{\theta }}_{k}})+{{\mathbf{v}}_{k}}=h\left( \mathbf{\theta }_{{k}}^{\mathbf{'}} \right)+{{\mathbf{H}}_{k}}\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{{k}}^{\mathbf{'}} \right)+{{\mathbf{v}}_{k}}$
通过负反馈引入估计的状态值 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ 为： $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-{{{\mathbf{\hat{z}}}}_{k}} \right)=\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-h(\theta _{k}^{'} )\right)$
$h({{\mathbf{\theta }}_{k}})-h\left( \mathbf{\theta }_{{k}}^{\mathbf{'}} \right)={{\mathbf{H}}_{k}}\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{{k}}^{{'}} \right)$

有：

估计值 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ 与真实值 ${{\mathbf{\theta }}_{k}}$ 之间的误差协方差矩阵 ${{\mathbf{\Sigma }}_{k}}$ ${{\mathbf{\Sigma }}_{k}}=\left\langle {{\mathbf{e}}_{k}}\mathbf{e}_{k}^{T} \right\rangle =\left\langle \left( {{\mathbf{\theta }}_{k}}-\left\langle {{\mathbf{\theta }}_{k}} \right\rangle \right){{\left( {{\mathbf{\theta }}_{k}}-\left\langle {{\mathbf{\theta }}_{k}} \right\rangle \right)}^{T}} \right\rangle$

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

${{\mathbf{\Sigma }}_{k}}=\left\langle \left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( h\left( {{\mathbf{\theta }}_{k}} \right)-h\left( \mathbf{\theta }_{k}^{'} \right)+{{\mathbf{v}}_{k}} \right) \right]{{\left[ {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{{\mathbf{K}}_{k}}\left( h\left( {{\mathbf{\theta }}_{k}} \right)-h\left( \mathbf{\theta }_{k}^{'} \right)+{{\mathbf{v}}_{k}} \right) \right]}^{T}} \right\rangle$

${{\mathbf{\Sigma }}_{k}}=\left\langle \left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)+{{\mathbf{K}}_{k}}{\mathbf{v}}_{k}} \right]{{\left[ \left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)+{{\mathbf{K}}_{k}}{\mathbf{v}}_{k}} \right]}^{T}} \right\rangle$

${{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)\left\langle \left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right){{\left( {{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)}^{T}} \right\rangle {{\left( \mathbf{I}-{{\mathbf{K}}_{k}}{{\mathbf{H}}_{k}} \right)}^{T}}+{\mathbf{K}}_{k}}{\mathbf{R}}{\mathbf{K}}_{k}}^{T}$

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

其中 $\mathbf{\Sigma }_{k}^{'}$ 表示真实的状态值 ${{\mathbf{\theta }}_{k}}$ 与预测的状态值 $\mathbf{\theta }_{k}^{'}$ 之间的误差协方差矩阵。于是得到式

${{\mathbf{\Sigma }}_{k}}=\mathbf{\Sigma }_{k}^{'}-{{\mathbf{K}}_{k}}\mathbf{H}_{k}{\mathbf{\Sigma } }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}_{k}^{T}}}\mathbf{K}_{k}^{T}+{{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}\mathbf{\Sigma }_{k}^{'}{{\mathbf{H}_{k}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T}$ （8）

因为 ${{\mathbf{\Sigma }}_{k}}$ 的对角元即为真实值与估计值的误差的平方，矩阵的迹（用 $T[]$ 表示）即为总误差的平方和，即

$T\left[ {{\mathbf{\Sigma }}_{k}} \right]=T\left[ \mathbf{\Sigma }_{k}^{'} \right]+T\left[ {{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}{\mathbf{\Sigma } }_{k}^{'}{{\mathbf{H}_{k}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T} \right]-2T\left[ {{\mathbf{K}}_{k}}\mathbf{H}_{k}\mathbf{\Sigma }_{k}^{'} \right]$

利用以下矩阵迹的求导公式（其中 $\mathbf{A}$ 和 $\mathbf{B}$ 表示矩阵， $\mathbf{a}$ 表示列向量）：

$Tr(\mathbf{A}+\mathbf{B})=Tr(\mathbf{A})+Tr(\mathbf{B})$

$Tr(\mathbf{AB})=Tr(\mathbf{BA})$

$\mathbf{a}^{T} \mathbf{a}=Tr(\mathbf{a}\mathbf{a}^{T})$

$\frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XBX}^{T})=\mathbf{XB}^{T}+\mathbf{XB}$

$\frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{AX}^{T})=\mathbf{A}$

$\frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XA})=\mathbf{A}^{T}$

要让估计值更接近于真实值，就要使上面的迹尽可能的小，因此要取得合适的卡尔曼增益 ${{\mathbf{K}}_{k}}$ ，使得迹得到最小，言外之意就是使得迹对 ${{\mathbf{K}}_{k}}$ 的偏导为0，即

$\frac{dT\left[ {{\mathbf{\Sigma }}_{k}} \right]}{d{{\mathbf{K}}_{k}}}=2{{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}{\mathbf{\Sigma }}_{k}^{'}{{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)-2{{\left( \mathbf{H}_{k}{\mathbf{\Sigma }}_{k}^{'} \right)}^{T}}=0$

这样就能算出合适的卡尔曼增益了，即

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

代回式（8）得到

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

接下来就差真实值与预测值之间的协方差矩阵 $\mathbf{\Sigma }_{k}^{'}$ 的求值公式了

$\mathbf{\Sigma }_{_{k}}^{'}=\left\langle \mathbf{e}_{k}^{'}\mathbf{e}{{_{k}^{'}}^{T}} \right\rangle =\left\langle \left( {{\theta }_{k}}-\theta _{k}^{'} \right){{\left( {{\theta }_{k}}-\theta _{k}^{'} \right)}^{T}} \right\rangle$

将以下两个等式代入到上式，

${{\mathbf{\theta }}_{k}}=f({{\mathbf{\theta }}_{k-1}})+{{\mathbf{s}}_{k}}$ ， $\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}$

可以得到

$\mathbf{\Sigma }_{_{k}}^{'}=\left\langle \left[f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{s}}_{k}} \right]{{\left[ f({{\mathbf{\theta }}_{k-1}})-f(\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle )+{{\mathbf{s}}_{k}} \right]}^{T}} \right\rangle$

有 ${{\mathbf{\theta }}_{k}}$ 、 $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle$ 与观测噪声 ${{\mathbf{s}}_{k}}$ 是独立的，求期望等于零;； $\left\langle {{\mathbf{s}}_{k}}{{\mathbf{s}}_{k}}^{T} \right\rangle$ 表示观测噪声的协方差矩阵，用 ${\mathbf{Q}}$ 表示。于是得到

$\mathbf{\Sigma }_{_{k}}^{'}=\mathbf{F}_{k-1}\left\langle \left( {{\theta }_{k-1}}-\left\langle {{\theta }_{k-1}} \right\rangle \right){{\left( {{\theta }_{k-1}}-\left\langle {{\theta }_{k-1}} \right\rangle \right)}^{T}} \right\rangle {{\mathbf{F}}_{k-1}^{T}}+\left\langle {{\mathbf{s}}_{k}}\mathbf{s}_{k}^{T} \right\rangle \\ =\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$ 为： $\left\langle {{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-{{{\mathbf{\hat{z}}}}_{k}} \right)=\mathbf{\theta }_{k}^{'}+{{\mathbf{K}}_{k}}\left( {{\mathbf{z}}_{k}}-h(\theta _{k}^{'} )\right)$ （5）

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

其中的雅克比矩阵 ${\mathbf{F}}_{k-1}$ 和 ${\mathbf{H}}_{k}$ 分别为

${{\mathbf{F}}_{k-1}}={{\left. \frac{\partial f}{\partial \mathbf{\theta }} \right|}_{\left\langle {{\mathbf{\theta }}_{k-1}} \right\rangle }}$ ， ${{\mathbf{H}}_{k}}={{\left. \frac{\partial h}{\partial \mathbf{\theta }} \right|}_{\mathbf{\theta }_{k}^{'}}}$