回复
#先锋团#卡尔曼滤波及其变种有哪些？究竟滤了个啥？ 原创 精华
从基础卡尔曼滤波到互补卡尔曼滤波
卡尔曼滤波自从1960被Kalman发明并应用到阿波罗登月计划之后一直经久不衰，直到现在也被机器人、自动驾驶、飞行控制等领域应用。基础卡尔曼滤波只能对线性系统建模；扩展卡尔曼滤波对非线性方程做线性近似以便将卡尔曼滤波应用到非线性系统。后来研究者发现将系统状态分成主要成分和误差，并将卡尔曼滤波用来预测误差，会使得系统的近似程度更高，效果更好。在姿态解算任务中，研究者们用辅助传感器如加速度计和磁力计来修正角速度计的积分结果，得到互补卡尔曼滤波的形式。
卡尔曼滤波是一种工具，对实际问题的不同建模方式会得到不同形式的卡尔曼滤波器。这导致了对于初学者不知道从何看起是好。另外也似乎很少有文章对基础卡尔曼滤波到各种形式的滤波形式做总结说明，于是便有了这篇文章。本文会从以下几个方面分析和讲解多种卡尔曼滤波器形式：
- 基础卡尔曼滤波——对线性系统的预测
- 扩展卡尔曼滤波——基础卡尔曼滤波在非线性系统的扩展
- 基于四元素的卡尔曼滤波器——基于实际问题的讲解
- 状态误差卡尔曼滤波——将状态误差引入状态向量
- 互补卡尔曼滤波——一种只使用角度误差和角速度误差作为状态向量和测量向量的滤波器形式
欢迎关注国内首个以自动驾驶技术栈为主线的交流学习社区！
原文出处：微信公众号【自动驾驶之心】
符号定义
小写字母为变量；加粗小写字母为向量；大写和加粗大写为矩阵
基础卡尔曼滤波器
宏观认识
卡尔曼滤波包含两个步骤
- 预测（prediction）—— Dynamic model
- 更新（correction/measurment update）—— Observation model
所谓预测，就是用一个数学模型，根据当前的传感器输入，直接计算此时系统的状态。可以理解为一个方程的计算就行。
所谓的更新，就是在某些时刻或者每一时刻，获取一些系统的其他状态输入（我们将这个值叫做测量值），比较此刻预测的系统状态和测量的系统状态，对预测出的系统状态进行修正，因此也叫测量更新（measurment update）。
整体框架如下图所示
状态方程及测量方程
$$
\mathbf{x}t = A_t\mathbf{x}{t-1}+B_t\mathbf{u}_t+\mathbf{w}_t\tag{1-1}
$$
$$
\mathbf{z}_t = H\mathbf{x}_t+\mathbf{v}_t\tag{1-2}
$$
其中$\mathbf{x}_t \in\mathbb{R}^n$是系统状态向量，$\mathbf{z}_t\in\mathbb{R}^m$是测量向量。$\mathbf{w}_t,\mathbf{v}_t$分别是过程噪声和观测噪声，且满足高斯分布
$$
\mathbf{w}_t\sim N(0,Q_t)
$$
$$
\mathbf{v}_t\sim N(0,R_t)
$$
预测过程
$$
\hat{\mathbf{x}}t^-=A_t\hat{\mathbf{x}}{t-1}^++B_t\mathbf{u}_t\tag{1-3}
$$
$$
P_t^-=A_tP_{t-1}^+A_t^T+Q_t\tag{1-4}
$$
其中，$P_t^-$是先验状态的误差协方差矩阵
$$
P_t^-\triangleq \mathbb{E}[(\mathbf{x}_t-\hat{\mathbf{x}}_t^-)(\mathbf{x}_t-\hat{\mathbf{x}}_t^-)^T]
$$
更新过程
$$
\tilde{\mathbf{y}}_t=\mathbf{z}_t-H_t\hat{\mathbf{x}}_t^-
$$
$$
S_t=H_tP_t^-H_t^T+R_t
$$
$$
K_t=P_t^-H_t^TS_t^{-1}\tag{1-5}
$$
$$
\mathbf{x}_t^+=\mathbf{x}_t^-+K_t\tilde{\mathbf{y}}_t\tag{1-6}
$$
$$
P_t^+=(I-K_tH_t)P_t^-\tag{1-7}
$$
详细公式推导
本文作为一篇概述性文章，为了不使篇幅过于冗长，不进行基础卡尔曼滤波器公式的推导。想完全理解基础卡尔曼滤波器可以参考下面这几篇资料：
- 卡尔曼滤波基础知识及公式推导——较为形象化地讲解预测和更新这两个过程之间地概率分布关系
- wiki Kalman Filter——准确的公式化推导
如何理解卡尔曼滤波器？
从概率分布的角度
卡尔曼滤波器将系统状态的变化中的过程噪声假设为均值为0的高斯噪声，使得状态向量也变为一个符合高斯分布的随机向量。同时对观测过程的噪声也假设为均值为0的高斯噪声。通过测量方程，即公式（1-2）得到将状态向量映射到测量向量的函数。于是，当得到测量值的时候，可以利用测量值与状态向量之间的关系得出另外一个对状态向量的估计。利用测量值得出的状态估计和状态方程计算的状态均符合高斯分布，两个高斯分布的联合概率分布依旧保持高斯特性。进一步推导可以得到公式（1-5）到公式（1-7）。关于这个角度的理解可以阅读上面推荐的第一篇文章。
从最小化误差的角度
卡尔曼滤波的最终输出是$\hat{\mathbf{x}}_t^+$，真实的状态为$\mathbf{x}_t$，令
$$
\mathbf{e}_t = \mathbf{x}_t - \hat{\mathbf{x}}_t
$$
对误差$\mathbf{e}_t$的平方求最小值，同样可以推导出公式（1-5）到公式（1-7）。因此卡尔曼滤波器也是系统状态的最优估计。关于这个角度的理解和推导可以参考上面推荐的第二篇文章。
扩展卡尔曼滤波
非线性方程的线性近似
卡尔曼滤波器建立在线性的状态方程和测量方程也就是公式（1-1）和公式（1-2）。但是在实际应用中，更多的关系是非线形关系，比如如何从连续的角速度计算出车辆当前的姿态角等。为了能够利用基本卡尔曼滤波器的预测和更新过程，对于非线性的状态方程和观测方程，我们利用一阶的泰勒展开，将非线性公式近似为线性公式。
状态方程及测量方程
$$
\mathbf{x}t=f(\mathbf{x}{t-1}, \mathbf{u}_t, \mathbf{w}_t)\tag{2-1}
$$
$$
\mathbf{z}_t=h(\mathbf{x}_t,\mathbf{v}_t)\tag{2-2}
$$
公式（2-1，2-2）可以类比基础卡尔曼滤波器中的公式（1-1，1-2）
一阶泰勒展开
我们先假设已知$t-1$时刻滤波器的输出，也就是$t-1$时刻的状态后验，以及对应的协方差矩阵为
$$
\hat{\mathbf{x}}{t-1}^+,P{t-1}^+
$$
同时，我们令$\mathbf{x}_t$的先验为
$$
\hat{\mathbf{x}}t^-=f(\hat{\mathbf{x}}{t-1}^+, \mathbf{u}_t, 0)\tag{2-3}
$$
对公式（2-1）在$\hat{x}_{t}^-$这一点做展开，并只保留一次项
$$
\mathbf{x}t\approx \hat{\mathbf{x}}t^-+A_t(\mathbf{x}{t-1}-\hat{\mathbf{x}}{t-1}^+)+W_t\mathbf{w}_t\tag{2-4}
$$
同时，对公式（2-2）在$\hat{\mathbf{z}}_t^-=h(\hat{\mathbf{x}}_t^-,0)$这一点做泰勒展开，并只保留一次项
$$
\mathbf{z}_t\approx \hat{\mathbf{z}}_t^-+H(\mathbf{x}_t-\hat{\mathbf{x}}_t^-)+V_t\mathbf{v}_t\tag{2-5}
$$
在公式（2-4）和公式（2-5）中：
- $A_{[i,j]}=\frac{\partial f_{i}}{\partial x_j}(\hat{\mathbf{x}}_{t-1}^+, \mathbf{u}_t, 0)$
- $W_{[i,j]}=\frac{\partial f_i}{\partial w_j}(\hat{\mathbf{x}}_{t-1}^+, \mathbf{u}_t, 0)$
- $H_{[i,j]}=\frac{\partial h_i}{\partial x_j}(\hat{\mathbf{x}}_t^-,0)$
- $V_{[i,j]}=\frac{\partial h_i}{\partial v_j}(\hat{\mathbf{x}}_t^-,0)$
预测方程及状态协方差矩阵
$$
\hat{\mathbf{x}}t^-=f(\hat{\mathbf{x}}{t-1}^+,\mathbf{u}t,0)\tag{2-6}
$$
$$
P_t^-=A_tP{t-1}^-A_t^T+W_tQ_tW_t^T\tag{2-7}
$$
其中，公式（2-7）中的第二项，因为在线性近似方程（2-4）中，噪声项满足分布
$$
W_t\mathbf{w}_t\sim N(0, W_tQW_t^T)
$$
更新方程及卡尔曼增益
$$
\tilde{\mathbf{y}}_t=\mathbf{z}_t-h(\hat{\mathbf{x}}_t^-,0)
$$
$$
S_t=H_tP_t^-H_t^T+V_tR_tV_t^T
$$
$$
K_t=P_t^-H_t^TS_t^{-1}\tag{2-8}
$$
$$
\hat{x}_t^+=\hat{\mathbf{x}}_t^-+K_t\tilde{\mathbf{y}}_t\tag{2-9}
$$
$$
P_t^+=(I-K_tH_t)P_t^-\tag{2-10}
$$
基础卡尔曼滤波器 && 扩展卡尔曼滤波器总结
基于四元数的拓展卡尔曼滤波器
从实际问题讲起
在运动物体的姿态估计，比如车辆的姿态计算中，常常利用IMU(Inertial Meseasurment Unit)惯性测量单元计算物体的姿态。
为了方便叙述，这里的姿态估计意味着我们希望解算车辆在每一时刻与起始坐标系之间三个轴的偏转角，通常用roll、pitch、yaw表示。
IMU(惯性测量单元)
现在的IMU一般是六轴或者九轴。六轴IMU可以输出$x,y,z$三个轴的加速度（acc）和角速度（gyro）；九轴则在六轴的基础上增加了磁力计（Magnetometer），测量三轴的磁场方向。为了简化问题，我们用六轴IMU作为示例。
相关定义
在姿态估计的各个领域中，通常使用四元数来表示一个旋转。四元数比欧拉角表达拥有更好的特性，同时相比于旋转矩阵又更加紧凑。定义四元数如下
$$
\mathbf{q}=[q_w\ q_x\ q_y\ q_z]
$$
为了估计系统的姿态，通常的做法是使用惯性测量单元IMU跟踪角速度，我们另每一时刻的角速度为
$$
\boldsymbol{\omega}_t=[\omega_x\ \omega_y\ \omega_z]
$$
另外，我们需要知道IMU的输出频率。假设IMU的输出时间间隔为$\Delta t$
四元素乘积的导数
这部分是为了后面推导扩展卡尔曼滤波的状态方程中的雅各比矩阵准备
我们令$t+\Delta t$时刻的旋转为$\mathbf{q}(t+\Delta t)$.$t$时刻的旋转为$\mathbf{q}(t)$。则$t+\Delta t$的旋转是$t$时刻的旋转经过$\Delta \mathbf{q}$的变化得到的，即
$$
\mathbf{q}(t+\Delta t)=\Delta \mathbf{q}\mathbf{q}(t)\tag{3-1}
$$
其中$\Delta \mathbf{q}$等于在时间间隔$\Delta t$内的角度变化量。在极短的时间间隔内，我们认为角度的变化是匀速的，也就是$\mathbf{\omega}_t$。我们令在这个时间间隔内，角度的旋转轴为$\mathbf{u}$，旋转角度为$\theta$,则
$$
\mathbf{u}=\frac{\mathbf{\omega}}{||\mathbf{\omega}||}
$$
$$
\theta=||\mathbf{\omega}||\Delta t
$$
根据四元数的基础知识，我们可以得到
$$
\begin{split}
\Delta \mathbf{q}&=cos\frac{\theta}{2}+\mathbf{u}sin\frac{\theta}{2}\
&=cos\frac{||\omega||\Delta t}{2}+\frac{\omega}{||\omega||}sin\frac{||\omega||\Delta t}{2}
\end{split}
$$
扩展卡尔曼滤波的重点之一在于求状态方程相对于状态的偏导；我们虽然可以从三轴角速度的输出得出角度积分的离散形式公式（1），但是我们其实不能对其直接对$\mathbf{q}(t)$求偏导。至于为什么不能直接求导，下面说得挺好:
求导的定义是函数值的微增量关于自变量的微增量的极限。表示旋转的单位四元数作差后，其不再是单位四元数，也就不是旋转四元数了
为了能够对公式（1）求出偏导数，我们先求旋转相对时间的导数$\dot{\mathbf{q}}$。然后利用泰勒展开，将公式（1）展开为线性方程，并只取一次项，这样就可以得到
$$
\begin{split}\begin{array}{rcl}
\mathbf{q}(t+\Delta t)-\mathbf{q}(t)
&=& \Big(\cos\frac{|\boldsymbol\omega|\Delta t}{2} + \frac{\boldsymbol\omega}{|\boldsymbol\omega|}\sin\frac{|\boldsymbol\omega|\Delta t}{2}\Big)\mathbf{q} - \mathbf{q} \
&=& \Big(-2\sin^2\frac{|\boldsymbol\omega|\Delta t}{4} + \frac{\boldsymbol\omega}{|\boldsymbol\omega|}\sin\frac{|\boldsymbol\omega|\Delta t}{2}\Big)\mathbf{q}
\end{array}\end{split}
$$
$$
\begin{split}\begin{array}{rcl}
\dot{\mathbf{q}}
&=& \underset{\Delta t\to 0}{\mathrm{lim}} \frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t} \
&=& \underset{\Delta t\to 0}{\mathrm{lim}} \frac{1}{\Delta t}\Big(-2\sin^2\frac{|\boldsymbol\omega|\Delta t}{4} + \frac{\boldsymbol\omega}{|\boldsymbol\omega|}\sin\frac{|\boldsymbol\omega|\Delta t}{2}\Big)\mathbf{q} \
&=& \frac{\boldsymbol\omega}{|\boldsymbol\omega|} \underset{\Delta t\to 0}{\mathrm{lim}} \frac{1}{\Delta t}\sin\big(\frac{|\boldsymbol\omega|\Delta t}{2}\big) \mathbf{q} \
&=& \frac{\boldsymbol\omega}{|\boldsymbol\omega|} \frac{d}{dt} \sin\big(\frac{|\boldsymbol\omega|}{2}t\big) \Big |_{t=0} \mathbf{q} \
&=& \frac{1}{2}\boldsymbol\omega \mathbf{q} \
&=& \frac{1}{2}
\begin{bmatrix}
-\omega_x q_x -\omega_y q_y - \omega_z q_z\
\omega_x q_w + \omega_z q_y - \omega_y q_z\
\omega_y q_w - \omega_z q_x + \omega_x q_z\
\omega_z q_w + \omega_y q_x - \omega_x q_y
\end{bmatrix}
\end{array}\end{split}
$$
我们令
$$
\begin{split}\boldsymbol\Omega(\boldsymbol\omega) =
\begin{bmatrix}
0 & -\omega_x & -\omega_y & -\omega_z \
\omega_x & 0 & \omega_z & -\omega_y \
\omega_y & -\omega_z & 0 & \omega_x \
\omega_z & \omega_y & -\omega_x & 0
\end{bmatrix}\end{split}
$$
则
$$
\dot{\mathbf{q}} = \frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\mathbf{q}\tag{3-2}
$$
有了角度相对于时间的导数之后，我们可以将公式（1）用泰勒展开
$$
\mathbf{q}_{t+1} = \mathbf{q}_t + \dot{\mathbf{q}}_t\Delta t +
\frac{1}{2!}\ddot{\mathbf{q}}t\Delta t^2 + \cdots
$$
只保留到一次项，则我们可以得到
$$
\mathbf{q}{t+1} = \Bigg[\mathbf{I}_4 + \frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t\Bigg]\mathbf{q}_t\tag{3-3}
$$
状态向量及控制输入
我们直接将车辆的姿态角以四元数形式表达作为系统状态向量
$$
\mathbf{x}_t=\begin{bmatrix}
q_w\
q_x\
q_y\
q_z
\end{bmatrix}
$$
同时，将每一时刻IMU的三轴角速度作为控制输入
$$
\mathbf{u}_t=\begin{bmatrix}
\omega_x\
\omega_y\
\omega_z
\end{bmatrix}
$$
状态方程及其雅各比矩阵
有了上一部分关于四元素导数的推导，我们可以直接写出状态方程如下
$$
\begin{split}
\mathbf{x}t&=\mathbf{f}(\mathbf{x}{t-1}, \mathbf{u}t) + \mathbf{w}=\Delta \mathbf{q}\mathbf{x}{t-1}+\mathbf{w}
\end{split}
$$
$$
\begin{split}
\mathbf{f}(\mathbf{x}_{t-1},\mathbf{u}t)&\approx
\Bigg[\mathbf{I}4 + \frac{1}{2}\boldsymbol\Omega(\mathbf{u})\Delta t\Bigg]\mathbf{x}{t-1}\
&=\begin{bmatrix}
q_w - \frac{\Delta t}{2} \omega_x q_x - \frac{\Delta t}{2} \omega_y q_y - \frac{\Delta t}{2} \omega_z q_z\
q_x + \frac{\Delta t}{2} \omega_x q_w - \frac{\Delta t}{2} \omega_y q_z + \frac{\Delta t}{2} \omega_z q_y\
q_y + \frac{\Delta t}{2} \omega_x q_z + \frac{\Delta t}{2} \omega_y q_w - \frac{\Delta t}{2} \omega_z q_x\
q_z - \frac{\Delta t}{2} \omega_x q_y + \frac{\Delta t}{2} \omega_y q_x + \frac{\Delta t}{2} \omega_z q_w
\end{bmatrix}\
&\approx \mathbf{x}{t-1}+\mathbf{A}_t(\mathbf{x}t-\mathbf{x}{t-1})
\end{split}
$$
$$
\mathbf{A}t=\frac{\partial f}{\partial \mathbf{x}{t-1}}=\begin{bmatrix}
1 & - \frac{\Delta t}{2} \omega_x & - \frac{\Delta t}{2} \omega_y & - \frac{\Delta t}{2} \omega_z\
\frac{\Delta t}{2} \omega_x & 1 & \frac{\Delta t}{2} \omega_z & - \frac{\Delta t}{2} \omega_y\
\frac{\Delta t}{2} \omega_y & - \frac{\Delta t}{2} \omega_z & 1 & \frac{\Delta t}{2} \omega_x\
\frac{\Delta t}{2} \omega_z & \frac{\Delta t}{2} \omega_y & - \frac{\Delta t}{2} \omega_x & 1
\end{bmatrix}
$$
其中$\mathbf{w}$是随机噪声，其协方差矩阵为$\mathbf{Q}_t$
预测方程
由于在上面的推导中已经求出雅各比矩阵，所以预测方程可以直接根据拓展卡尔曼滤波公式写出来
$$
\hat{\mathbf{x}}_t^-=\Bigg[\mathbf{I}4 + \frac{1}{2}\boldsymbol\Omega(\mathbf{u})\Delta t\Bigg]\hat{\mathbf{x}}{t-1}^+\tag{3-4}
$$
$$
\mathbf{P}_t^- = \mathbf{A}t\mathbf{P}{t-1}^+\mathbf{A}_t^T + \mathbf{Q}_t\tag{3-5}
$$
$$
\mathbf{A}_t=\begin{bmatrix}
1 & - \frac{\Delta t}{2} \omega_x & - \frac{\Delta t}{2} \omega_y & - \frac{\Delta t}{2} \omega_z\
\frac{\Delta t}{2} \omega_x & 1 & \frac{\Delta t}{2} \omega_z & - \frac{\Delta t}{2} \omega_y\
\frac{\Delta t}{2} \omega_y & - \frac{\Delta t}{2} \omega_z & 1 & \frac{\Delta t}{2} \omega_x\
\frac{\Delta t}{2} \omega_z & \frac{\Delta t}{2} \omega_y & - \frac{\Delta t}{2} \omega_x & 1
\end{bmatrix}\tag{3-6}
$$
测量更新
前文我们使用了角速度计的输出作为控制输入，并构建了状态方程和预测方程。IMU通常都会有加速度计输出，这部分输出可以用来作为测量量，并对预测的状态进行测量更新。我们先回顾测量更新中状态向量的更新过程。
$$
\hat{\mathbf{x}}_t^+=\hat{\mathbf{x}}_t^-+K_t\Big(\mathbf{z}_t-\mathbf{h}(\hat{\mathbf{x}}_t^-)\Big)
$$
令加速度计的输出为测量向量：
$$
\mathbf{a}_t=\begin{bmatrix}a_x \ a_y \ a_z\end{bmatrix}
$$
测量模型
现在我们已经确定好了状态向量为姿态角的四元数表达，测量向量为加速度计三个轴的输出，那么函数$\mathbf{h}(\mathbf{x})$要采用什么形式可以将姿态角转成三轴加速度呢？
其中的关键联系就是，当车辆静止时，加速度的合向量是重力加速度，垂直向下！
上图中，假设$XYZ$坐标系是起始坐标系，$X^\prime Y^\prime Z^\prime$是小车移动后的坐标系。在起始时，小车静止，小车的重力加速度的输出是垂直向下的，即
$$
\mathbf{g}=\mathbf{g}_{\mathrm{XYZ}}=[0\ 0\ 1]^T
$$
我们采用归一化的形式，也就是将重力加速度$9.81m/s^2$当作一个单位。
则当小车运动后，重力加速度在$X^\prime Y^\prime Z^\prime$坐标系下的表达为
$$
\mathbf{g}^{\prime}=\mathbf{g}_{X^\prime Y^\prime Z^\prime}=\frac{\mathbf{a}_t}{||\mathbf{a}_t||}
$$
$\mathbf{g}$和$\mathbf{g}^{\prime}$是重力加速度在两个不同坐标系下的表达，同时，这两个坐标系之间的旋转是此时的状态向量，因此
$$
\mathbf{z}_t=\mathbf{h}(\mathbf{x}_t)=\mathbf{R}(\mathbf{x}_t)\mathbf{g}+\mathbf{v}_t\tag{3-7}
$$
其中，
$$
\mathbf{z}_t=\frac{\mathbf{a}_t}{||\mathbf{a}_t||}
$$
$$
Var(\mathbf{v}_t)=\mathbf{R}_t
$$
$\mathbf{R}(\mathbf{q})$是将四元数转成旋转矩阵的表达，旋转矩阵左乘一个三位的列向量表示将该向量进行三维旋转。即下面的形式（$\mathbf{x}_t=[q_w\ q_x\ q_y\ q_z]$是我们之前定义过的）
$$
\mathbf{R}(\mathbf{x}_t)=\begin{bmatrix}
1-2(q_y^2+q_z^2) & 2(q_xq_y-q_wq_z) & 2(q_xq_z+q_wq_y) \
2(q_xq_y+q_wq_z) & 1-2(q_x^2+q_z^2) & 2(q_yq_z-q_wq_x) \
2(q_xq_z-q_wq_y) & 2(q_wq_x+q_yq_z) & 1-2(q_x^2+q_y^2)
\end{bmatrix}
$$
雅各比矩阵
从公式（3-7）我们可以得到测量模型中的转换函数$\mathbf{h}(\mathbf{x})$的雅各比矩阵
$$
\mathbf{h}(\mathbf{x}_t)=2
\begin{bmatrix}
g_x (\frac{1}{2} - q_y^2 - q_z^2) + g_y (q_xq_y - q_wq_z) + g_z (q_xq_z + q_wq_y) \
g_x (q_xq_y + q_wq_z) + g_y (\frac{1}{2} - q_x^2 - q_z^2) + g_z (q_yq_z - q_wq_x) \
g_x (q_xq_z - q_wq_y) + g_y (q_wq_x + q_yq_z) + g_z (\frac{1}{2} - q_x^2 - q_y^2)
\end{bmatrix}
$$
$$
\begin{split}
\mathbf{H}(\mathbf{x}_t) &= \frac{\partial\mathbf{h}(\mathbf{x}_t)}{\partial\mathbf{x}_t} \
&=\begin{bmatrix}
\frac{\partial\mathbf{h}(\mathbf{x}_t)}{\partial q_w} &
\frac{\partial\mathbf{h}(\mathbf{x}_t)}{\partial q_x} &
\frac{\partial\mathbf{h}(\mathbf{x}_t)}{\partial q_y} &
\frac{\partial\mathbf{h}(\mathbf{x}_t)}{\partial q_z}
\end{bmatrix} \
&=
2
\begin{bmatrix}
-g_yq_z + g_zq_y & g_yq_y + g_zq_z & - 2g_xq_y + g_yq_x + g_zq_w & - 2g_xq_z - g_yq_w + g_zq_x \
g_xq_z - g_zq_x & g_xq_y - 2g_yq_x - g_zq_w & g_xq_x + g_zq_z & g_xq_w - 2g_yq_z + g_zq_y \
-g_xq_y + g_yq_x & g_xq_z + g_yq_w - 2g_zq_x & -g_xq_w + g_yq_z - 2g_zq_y & g_xq_x + g_yq_y
\end{bmatrix}\end{split}
$$
更新方程
$$
\hat{\mathbf{x}}_t^+=\hat{\mathbf{x}}_t^-+\mathbf{K}_t\Big(\mathbf{z}_t-\mathbf{h}(\hat{\mathbf{x}}_t^-)\Big)\tag{3-8}
$$
$$
\mathbf{K}_t=\mathbf{P}_t^-\mathbf{H}^T(\hat{\mathbf{x}}_t^-)\mathbf{S}_t^{-1}\tag{3-9}
$$
$$
\mathbf{S}_t=\mathbf{H}(\hat{\mathbf{x}}_t^-)\mathbf{P}_t^-\mathbf{H}^T(\hat{\mathbf{x}}_t^-)+\mathbf{R}_t\tag{3-10}
$$
状态误差卡尔曼滤波器（ErKF : Error-state Kalman Filter)
概述
在使用卡尔曼滤波器做姿态估计（Attitude Estimation）中，很大一部分都采用不是直接将系统姿态角作为卡尔曼滤波的状态，而是将姿态角的积分误差和角速度计的误差作为系统状态。将角速度计的输出弥补上估计出的角速度计误差，然后对其积分，得到姿态角的估计，再弥补上姿态角的误差估计。整个的流程图大概如下面的图，引用自Intertial Head-Tracker Sensor Fusion by a Complementary Separate-Bias Kalman Filter
PS: 要强调的是，各种卡尔曼滤波的形式多种多样，同时各种符号的定义也都并不完全一致，这也是入门卡尔曼滤波比较难的地方，有时候找资料都不知道怎么找。这也是写这篇文章的目的，提供一个基础的脉络给卡尔曼滤波的初学者。因此这里给出的ErKF只是形式之一，主要是引用自论文Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation
状态误差的递推公式
首先，我们令$\mathbf{x}(t)$表示时间连续形式的状态向量。其相对于时间的导数为
$$
\dot{\mathbf{x}}(t)=\mathbf{f}\big(\mathbf{x}(t)\big)
$$
在等号的右边输入里加入微小扰动$\delta x(t)$，同时，根据泰勒展开将函数$\mathbf{f}(\cdot)$展开
$$
\begin{split}
\dot{\mathbf{x}}(t)+\delta \dot{\mathbf{x}}(t)&=\mathbf{f}\big(\mathbf{x}(t)+\delta \mathbf{x}(t)\big)\
&=\mathbf{f}(\mathbf{x}(t)) + \frac{\partial\mathbf{f}}{\partial \mathbf{x}(t)}\Big(\mathbf{x}(t)+\delta \mathbf{x}(t)-\mathbf{x}(t)\Big) + \boldsymbol{O}\
&\approx \mathbf{f}(\mathbf{x}(t)) + \frac{\partial\mathbf{f}}{\partial \mathbf{x}(t)}\delta \mathbf{x(t)}
\end{split}
$$
则
$$
\delta \dot{\mathbf{x}}(t) = \frac{\partial\mathbf{f}}{\partial \mathbf{x}(t)}\delta \mathbf{x(t)}
$$
将上式使用离散形式表达
$$
\frac{\delta \mathbf{x}_{t+\Delta t} - \delta \mathbf{x}_t}{\Delta t} = \frac{\partial \mathbf{f}}{\partial \mathbf{x}_t}\delta \mathbf{x}t
$$
即
$$
\begin{split}
\delta \mathbf{x}{t+1} &= \delta \mathbf{x}_t+\mathbf{A}_t\delta \mathbf{x}_t\Delta t\
&=\Big(\mathbf{I}+\mathbf{A}_t\Delta t\Big)\delta \mathbf{x}_t\
&=\Phi\delta \mathbf{x}_t
\end{split}\tag{4-1}
$$
于是，有了状态误差的递推公式，我们就可以像卡尔曼滤波一样推导预测和更新过程
预测过程
与直接对系统状态做卡尔曼滤波稍有不同，使用误差状态的卡尔曼滤波在计算姿态角的时候可以看成三步：
- 在卡尔曼滤波系统外使用积分算出此时的系统状态
- 使用卡尔曼滤波算出此时系统状态的误差
- 将积分出来的系统状态弥补上卡尔曼滤波计算出误差
系统状态计算
$$
\hat{\mathbf{x}}{t}^-=\hat{\mathbf{x}}{t-1}^++\int^t_{t-\Delta t}\mathbf{f}\big(\mathbf{x}(t)\big)
$$
上式只是一个公式化表达，其实就是将上一时刻的状态，在加上这一时间段状态的变化量。姿态估计中，状态的变化量通常是角速度计的输出乘以时间间隔。
状态误差方程及预测方程
状态误差方程
由公式（4-1）可以得到状态误差的方程为
$$
\delta \mathbf{x}_t=\Big(\mathbf{I}+\mathbf{A}t\Delta t\Big)\delta \mathbf{x}{t-1}+\mathbf{w}_t\tag{4-2}
$$
其中，$\mathbf{w}_t$是过程噪声，协方差矩阵为$Q_t$
预测方程
类似于普通卡尔曼滤波，预测方程为
$$
\delta \hat{\mathbf{x}}_t^-=\Big(\mathbf{I}+\mathbf{A}t\Delta t\Big)\delta \hat{\mathbf{x}}^+{t-1}\tag{4-3}
$$
$$
P_t^-=\Phi_tP_{t-1}^+\Phi_t^T+Q_t\tag{4-4}
$$
$$
\Phi_t = \mathbf{I}+\mathbf{A}_t\Delta t
$$
$$
\mathbf{A}t = \frac{\partial f}{\partial \mathbf{x}}(\hat{\mathbf{x}}{t-1}^+)
$$
测量方程及更新方程
这里我们直接将使用其余传感器如加速度计、磁力计计算出的姿态作为系统的测量量。在这里先记得，加速度计根据输出的夹角可以计算出$pitch、roll$角，由磁力计可以计算出$yaw$角即可。当然，也可以采用其他能够直接输出系统姿态角的传感器作为测量值。
测量方程
$$
\mathbf{z}_t=\mathbf{h}(\mathbf{x}_t)+\mathbf{v}_t\tag{4-5}
$$
其中，$\mathbf{v}_t$是测量噪声，协方差矩阵为$R_t$
更新方程
这里要执行两步更新
- 先更新对状态误差的估计
- 更新状态的估计（即把状态误差弥补到$\hat{\mathbf{x}}_t^-$)
$$
\delta \hat{\mathbf{x}}_t^+=K_t\big(\mathbf{z}_t-\mathbf{h}(\hat{\mathbf{x}}_t^-)\big)\tag{4-6}
$$
$$
K_t=P_t^-H_t^TS_t^{-1}\tag{4-7}
$$
$$
S_t=H_tP_t^-H_t^T+R_t\tag{4-8}
$$
$$
H_t = \frac{\partial \mathbf{h}}{\partial \mathbf{x}}(\hat{\mathbf{x}}_t^-)\tag{4-9}
$$
$$
\hat{\mathbf{x}}_t^+=\hat{\mathbf{x}}_t^-+\delta \hat{\mathbf{x}}_t^+\tag{4-10}
$$
补充
在上面的推导中，将我们的目标变量，即系统的姿态角作为外部一个单独的积分计算，但是实际上更多的做法是将姿态角和角速度的偏差直接放在状态向量中进行计算。然后对每一时刻的角速度偏差应用到角速度计的输出，再将其作为系统输入应用到状态方程。也就是像概述中的图示那样。但是其实各种卡尔曼滤波的建模方式都不一样，Error-state Kalman Filter和Complimentarty Kalman Filter也没有严格的定义。所以索性这一章节当作对状态误差的理解和推导，在下面的互补卡尔曼滤波给出一种似乎是应用更广泛的卡尔曼滤波器。
互补卡尔曼滤波
前言
正如前文所说，卡尔曼滤波器的建模形式多种多样，而且很多研究也是在上世纪，对于误差状态卡尔曼滤波（Error-state Kalman Filter)和互补卡尔曼滤波（Comlimentary Kalman Filter)其实没有严格的定义。这里的互补卡尔曼滤波其实也可以看成上文ErKF的另一种形式。主要采用自论文Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter。卡尔曼滤波的工作太多，博客和论文也五花八门，看起来十分不易。这篇论文从引用、论文叙述、符号标识看起来都很不错，很适合想要将卡尔曼滤波应用到姿态估计的工程师阅读。甚至有一些工作，直接使用普通卡尔曼滤波输出，然后利用互补滤波器的概念，在多种姿态输出之间做加权平均，也叫互补卡尔曼滤波器，比如这篇专利：一种基于互补卡尔曼滤波算法计算融合姿态角度的方法
互补的概念
其实只要有了角速度计（Gyroscope）我们就可以根据其输出，并在时间上做积分解算出车辆的姿态角$roll、pitch、yaw$。但是由于任何传感器都是带噪声的，同时，直接用积分解算，误差会随机时间推移而累积，最终的姿态解算精度就非常差。除了使用角速度计进行积分可以得到姿态角之外，用加速度计（Accelerometer）和磁力计（Magnatometer）也可以计算出动态系统的姿态。其中，加速度计使用重力加速度作为锚定量，测量静止状态下三轴加速度之间的夹角，可以计算出$roll、pitch$角；磁力计（或叫地磁计）利用地球磁场北极作为锚定量，可以计算出$yaw$角。使用这两者对角速度计进行补充，可以得出更加准确的姿态估计。
从加速度计计算姿态角roll、pitch
加速度计（Accelerometer）可以输出三个轴的加速度，在静止的情况下，三个轴的合向量就是重力加速度。因此，我们可以利用三个轴加速度之间的关系计算静止状态下的俯仰角（pitch）和翻滚角（roll）
关于如何推导从三个轴的加速度计算roll和pitch，可以看这篇文章
最后得出的形式也非常简单：
$$
Pitch=\theta = arcsin\frac{-a_x}{\sqrt{a_x^2+a_y^2+a_z^2}}=arcsin\frac{-a_x}{g}=arctan\frac{-a_x}{\sqrt{a_y^2+a_z^2}}
$$
$$
Roll=\phi=arctan\frac{a_y}{a_z}
$$
$$
g=9.81m/s^2
$$
从磁力计计算姿态角yaw
磁力计的三轴合向量会指向地磁北极，利用这一特性，可以从磁力计的输出获得与地磁北极的偏转角。利用这一点，可以计算出相对于起始位置的$yaw$角
具体的计算公式可以看这篇博客
从加速度计算的姿态弥补
从加速度计可以计算出roll角和pitch角，因此，可以将这个结果和角速度的积分结果结合起来，得到一个更好的估计姿态。不过要注意的是，从加速度计算的姿态弥补有两个局限：
- 加速度计只能计算出Roll角和Pitch角，因此yaw角无法得到互补信息
- 当车辆处于较大的加速度运动状态时，三轴加速度的合向量跟重力加速度会有偏差，因此互补结果应该根据这个偏差的大小做改变。
互补滤波器
互补滤波器使用角速度的积分结果和加速度与磁力计的计算结果进行插值，得到更好的结果
$$
\mathbf{q}=(1-\alpha)\mathbf{q}{\omega}+\alpha\mathbf{q}{am}
$$
其中，$\mathbf{q}{\omega}$是对角速度积分得出的姿态（用四元素表达）；$\mathbf{q}{am}$是使用加速度计和磁力计计算出的姿态。
互补卡尔曼滤波器
互补卡尔曼滤波器将姿态角的误差、角速度的误差当作状态向量；并利用其余传感器如加速度计和磁力计计算出的姿态角与估计的姿态角之间的差作为测量量。通过以下步骤得到系统的姿态角：
- 卡尔曼滤波器输出姿态角的误差和角速度的误差
- 将当前时刻角速度的输出加上角速度的误差，并利用积分公式算出姿态角
- 将步骤2算出的姿态角加上步骤1输出的姿态角误差
状态向量和测量向量
状态向量
$$
\delta \mathbf{x}_t=
\begin{bmatrix}
\delta \boldsymbol{\Theta}\
\delta \boldsymbol{\Omega}
\end{bmatrix}
=\begin{bmatrix}
\delta \psi & \delta\theta & \delta\phi & \delta \omega_x & \delta \omega_y & \delta \omega_z
\end{bmatrix}^T
$$
其中$\psi, \theta, \phi$是系统的$roll、pitch、yaw$角；$\omega_x,\omega_y,\omega_z$是三轴角速度。
测量向量
$$
\mathbf{z}_t=
\begin{bmatrix}
\psi_m-\hat{\psi} & \theta_m - \hat{\theta} & \phi_m - \hat{\phi}
\end{bmatrix}^T
$$
其中，$\psi_m,\theta_m,\phi_m$是使用加速度计和磁力计计算出的系统$roll、pitch、yaw$角
状态方程
我们可以推导出姿态角对于时间的导数，通常这个导数是姿态角和角速度的函数，即
$$
\dot{\boldsymbol{\Theta}}(t)=\mathbf{f}\big(\mathbf{\Theta}(t),\mathbf{\Omega}(t)\big)
$$
从公式（4-1）的推导过程，以及泰勒展开，我们可以得到$\delta\Theta$的递推公式
$$
\delta \mathbf{\Theta}t=\delta \mathbf{\Theta}{t-1}+
\frac{\partial f}{\partial \mathbf{\Theta}}\delta \mathbf{\Theta}{t-1}+
\frac{\partial f}{\partial \mathbf{\Omega}}\delta \mathbf{\Omega}{t-1}
$$
我们直接假设角速度的误差是一个常量误差，即
$$
\delta \mathbf{\Omega}t = \delta \mathbf{\Omega}{t-1}
$$
则我们可以将上面两式写成矩阵形式
$$
\delta \mathbf{x}_t=
\begin{bmatrix}
\delta \boldsymbol{\Theta}t\
\delta \boldsymbol{\Omega}t
\end{bmatrix}=
\begin{bmatrix}
A_t& B_t\
\mathbf{0} & \mathbf{I}
\end{bmatrix}
\begin{bmatrix}
\delta \boldsymbol{\Theta}{t-1}\
\delta \boldsymbol{\Omega}{t-1}
\end{bmatrix}+\mathbf{w}_t\tag{5-1}
$$
测量方程
$$
\mathbf{z}t=
\begin{bmatrix}
\mathbf{I}{3x3} & \mathbf{0}\
\mathbf{0} & \mathbf{0}_{3x3}
\end{bmatrix}
\delta \mathbf{x}_t + \mathbf{v}_t\tag{5-2}
$$
预测&&更新过程
有了上面状态方程和测量方程的推导，剩下的就是按照公式（4-3）到公式（4-10）的过程代入。这里唯一不同的就是卡尔曼滤波输出的向量是角速度的误差和姿态的误差。在计算姿态的时候先将角速度的误差应用到角速度计的数据，对角度积分，将角度的误差应用到角度积分结果，得到最终的角度输出。整个框架如下图
后话
- 卡尔曼滤波是一个很古老的算法，但同时又是被广泛应用的算法。即使在今天姿态解算中很多用了因子图，但是对IMU的预积分依旧要使用卡尔曼滤波。但是卡尔曼滤波算法只是一个工具，不同的系统建模方式会产生不同形式的卡尔曼滤波器，这也导致了初学者不知道从哪里入手。
- 在查资料的过程中发现，卡尔曼滤波的一些变种如Error-State Kalman Filter和Complimentary Kalman Filter其实并不是严格定义的。
- 笔者对卡尔曼滤波并没有很丰富的应用经验，本身也不是专门做运动控制和姿态解算的。本文的叙述在追求通俗易懂之外尽力保证公式和符号定义的准确。但无法保证没有错误。
- 对于卡尔曼滤波器中各个变量的初始值设置是门学问，论文中都会有独立的章节讲述初始值如何设置。这方面可能得结合实际应用和效果得出最优的方案。
Reference
[1] Roll and Pitch Angles From Accelerometer Sensors
[2] 四元数、欧拉角、旋转矩阵转换
[3] 四元素乘积求导
[5] Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter
[6] Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation
[8] 卡尔曼滤波基础知识及公式推导
©著作权归作者所有，如需转载，请注明出处，否则将追究法律责任
赞
收藏
回复
相关推荐
