|
|
卡尔曼滤波中的噪声协方差矩阵（R和Q）应该怎么取值,和噪声分布之间的关系是什么？
关注者
413
被浏览
358,922
9 个回答
我的理解是：理论+试错调参
一、理论：
首先，PQR都是矩阵，看到矩阵，不能只是看到一个方块，要知道causality，也就是里面对应元素的意思。协方差矩阵，每个元素都是某两个值的协方差，协方差公式为
那么，是谁和谁的协方差呢？p_{ij}，就是第i个状态量与第j个状态量的协方差。那么你可以看出来，协方差矩阵P应该是对称的。如果某两个变量是不相关的，那么协方差为0。一般只需要设置P0，剩下的都是自动算的。在设置噪声矩阵的时候，我们通常假设数据的噪声相互之间是独立的，那就只设置对角线上的元素，自己和自己的协方差（也就是“方差”），这时候就只需要考虑数据自己本身的特性了。
P0 = Var⟨X0⟩ P的初值就是代表你对你估计的X0有多不确定。P0为0的时候就是说你对你给初值有着坚不可摧的信心。一般来说宁可设成单位阵都不会设成0的。
卡尔马滤波算法假设所有噪声都是零均值的白噪声，那么Q和R中的元素就是白噪声的方差。即：
Xk+1 = f(X^ k) + wk， wk ∼ N(0; Qk)
Zk+1 = h(X^ k+1/k) + vk， vk ∼ N(0; Rk)
Q代表建模误差，跟你计算机舍入误差（比如说用了浮点，那么会引入1e-7的误差项）、模型的线性化程度、离散化引入误差有关系；如果你还有输入u的话，输入的误差也会考虑到这里。
R的选取与传感器的特性有关，具体到应用中，比如导航系统，我的GPS传感器传下来的位置p会在真值附近有个方差为±3m的噪声，那么R中对应p的那个元素就该是3。
如果P0、Q、R无法精确获得，只知道可能的取值范围，则采用可能的较大值（保守）。如果不确切知道Q、R、P0的准确先验信息，应适当增大Q的取值，以增大对实时量测值的利用权重，俗称调谐。但是调谐存在盲目性，无法知道Q要调到多大才行。
——《卡尔曼滤波与组合导航原理》秦永元
二、调参
关于PQR的关系，看公式就一目了然：
这俩公式本质上就是K= （Pk+Q） / （Pk+Q+R）
Q和R分别代表对预测值和测量值的置信度（反比），通过影响卡尔曼增益K的值，影响预测值和测量值的权重。越大的R代表越不相信测量值。
而参数的选择其实和需求有关系，基于不同的准确度、动态性能、平滑度等会有不同的取舍。
《Fundamentals Of Kalman Filtering：A Practical Approach》Paul Zarchan 是一本注重实践操作，饱含干货的好书，可以借鉴。
调参手段，就是固定其中一个调另一个。
那么，怎么判断你调的效果变好了呢？
1、如果你在做仿真，知道真值是多少，那么只要看估计出来的值是不是更精确了就好；如果你加的是随机噪声，可能还要用到Monte Carlo判据。协方差矩阵对角线元素开根号其实就是标准差“1σ”， 若KF正确工作，则66%的误差点应落在±1σ界限以内。 就像下面这个图。Theory那俩条线就是σ在逐渐收敛，然后虚线那些是误差，误差的跳动范围大概就在两条边界线之间。（图来自Paul Zarchan的书）
2、再荐一本书：《Optimal State Estimation Kalman-H-and Nonlinear Approaches》与上面提到的Paul的书不同，这本书注重理论，推导清晰严谨，也有很多直觉解释，对新手友好。此书中提到还有一个判断方法： 如果KF正常工作，残差（innovation）应该是一个均值为0、方差为（HPH+R）、白色的变量。 当KF工作的时候，记录innovation的值，验证一下是否符合这些性质，如果不如预期，就说明有问题了，可以据此调参。
请仔细读下下面这篇文章：卡尔曼滤波及其变种有哪些？究竟滤了个啥？
- 基础卡尔曼滤波 ——对线性系统的预测
- 扩展卡尔曼滤波 ——基础卡尔曼滤波在非线性系统的扩展
- 基于四元素的 卡尔曼滤波器 ——基于实际问题的讲解
- 状态误差卡尔曼滤波 ——将状态误差引入状态向量
- 互补卡尔曼滤波 ——一种只使用角度误差和角速度误差作为状态向量和测量向量的滤波器形式
1从基础卡尔曼滤波到互补卡尔曼滤波
卡尔曼滤波 自从1960被Kalman发明并应用到阿波罗登月计划之后一直经久不衰，直到现在也被机器人、自动驾驶、飞行控制等领域应用。基础卡尔曼滤波只能对线性系统建模；扩展卡尔曼滤波对非线性方程做线性近似以便将卡尔曼滤波应用到非线性系统。后来研究者发现将系统状态分成主要成分和误差，并将卡尔曼滤波用来预测误差，会使得系统的近似程度更高，效果更好。在姿态解算任务中，研究者们用辅助传感器如加速度计和磁力计来修正角速度计的积分结果，得到互补卡尔曼滤波的形式。
卡尔曼滤波是一种工具，对实际问题的不同建模方式会得到不同形式的卡尔曼滤波器。这导致了对于初学者不知道从何看起是好。另外也似乎很少有文章对基础卡尔曼滤波到各种形式的滤波形式做总结说明，于是便有了这篇文章。本文会从以下几个方面分析和讲解多种卡尔曼滤波器形式：
- 基础卡尔曼滤波 ——对线性系统的预测
- 扩展卡尔曼滤波 ——基础卡尔曼滤波在非线性系统的扩展
- 基于四元素的卡尔曼滤波器 ——基于实际问题的讲解
- 状态误差卡尔曼滤波 ——将状态误差引入状态向量
- 互补卡尔曼滤波 ——一种只使用角度误差和角速度误差作为状态向量和测量向量的滤波器形式
作者：汽车人 | 原文出处： 公众号【自动驾驶之心】 自动驾驶之心->： 【全栈算法】技术交流群
点击关注 @自动驾驶之心 ，第一时间看到最前沿与价值的CV/自动驾驶/AI类工作~
强烈推荐！自动驾驶与AI学习社区： 欢迎加入国内首个自动驾驶开发者社区！这里有最全面有效的自动驾驶与AI学习路线（感知/定位/融合）和自动驾驶与AI公司内推机会！
2符号定义
小写字母为变量；加粗小写字母为向量；大写和加粗大写为矩阵
3基础卡尔曼滤波器
宏观认识
卡尔曼滤波包含两个步骤
- 预测（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）。因此卡尔曼滤波器也是系统状态的最优估计。关于这个角度的理解和推导可以参考上面推荐的第二篇文章。
4扩展卡尔曼滤波
非线性方程的线性近似
卡尔曼滤波器建立在线性的状态方程和测量方程也就是公式（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}
5基础卡尔曼滤波器 && 扩展卡尔曼滤波器总结
6基于四元数的拓展卡尔曼滤波器
从实际问题讲起
在运动物体的姿态估计，比如车辆的姿态计算中，常常利用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}
7状态误差卡尔曼滤波器（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 也没有严格的定义。所以索性这一章节当作对状态误差的理解和推导，在下面的互补卡尔曼滤波给出一种 似乎 是应用更广泛的卡尔曼滤波器。
8互补卡尔曼滤波
前言
正如前文所说，卡尔曼滤波器的建模形式多种多样，而且很多研究也是在上世纪，对于误差状态卡尔曼滤波（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）的过程代入。这里唯一不同的就是卡尔曼滤波输出的向量是角速度的误差和姿态的误差。在计算姿态的时候先将角速度的误差应用到角速度计的数据，对角度积分，将角度的误差应用到角度积分结果，得到最终的角度输出。整个框架如下图
9后话
- 卡尔曼滤波是一个很古老的算法，但同时又是被广泛应用的算法。即使在今天姿态解算中很多用了因子图，但是对IMU的预积分依旧要使用卡尔曼滤波。但是卡尔曼滤波算法只是一个工具，不同的系统建模方式会产生不同形式的卡尔曼滤波器，这也导致了初学者不知道从哪里入手。
- 在查资料的过程中发现，卡尔曼滤波的一些变种如Error-State Kalman Filter和Complimentary Kalman Filter其实并不是严格定义的。
- 笔者对卡尔曼滤波并没有很丰富的应用经验，本身也不是专门做运动控制和姿态解算的。本文的叙述在追求通俗易懂之外尽力保证公式和符号定义的准确。但无法保证没有错误。
- 对于卡尔曼滤波器中各个变量的初始值设置是门学问，论文中都会有独立的章节讲述初始值如何设置。这方面可能得结合实际应用和效果得出最优的方案。
10Reference
[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] 卡尔曼滤波基础知识及公式推导
[9] AHRS: Attitude and Heading Reference Systems
自动驾驶与AI学习社区
欢迎加入国内首个自动驾驶开发者社区！这里有最全面有效的自动驾驶与AI学习路线（感知/定位/融合）和自动驾驶与AI公司内推机会！
自动驾驶之心—SLAM交流群
建了自动驾驶之心 SLAM 交流群！想要进交流群的同学，可以直接加微信号： AIDriver001 。加的时候备注一下： SLAM+学校/公司+昵称 ，即可。然后就可以拉你进群了。
往期回顾
自动驾驶之心 | Feat-Calibr | 一款实用的开源激光雷达到车体外参的标定工具
自动驾驶之心 | 首篇！BEV-Locator：多目端到端视觉语义定位网络（清华&轻舟智航）
自动驾驶之心 | NIPS2022 | 港大最新Sparse2Dense：通用点云3D检测稠密模块！
自动驾驶之心 | Make RepVGG Greater Again！揭示重参化量化崩溃根因并解决（美团）
自动驾驶之心 | 基于跨窗口一致性的半监督语义分割的渐进学习算法
自动驾驶之心 | 目标检测的结构化知识蒸馏（奔驰&TU Delft）
自动驾驶之心 | 90+目标跟踪算法&9大benchmark！基于判别滤波器和Siamese 网络的视觉目标跟踪：综述与展望