卡尔曼滤波笔记

---

前言

最近项目开发上需要用到卡尔曼滤波对GPS、IMU、里程计进行融合定位，本文主要记录对卡尔曼滤波的学习与理解。

---

1.公式

卡尔曼滤波主要由下列五个公式组成。分为predict和update，其中公式1、2为predict，3、4、5为update。

$x_t^{-}$ —— t时刻状态先验估计值，即根据t-1时刻最优估计值预测出来的k时刻结果，属于预测结果；
$p_t^{-}$ ——t时刻先验估计协方差
$x_t$ —— t时刻状态后验估计值（最优估计值），即融合后的结果
$p_t$——t时刻后验估计协方差，衡量最优估计值$x_t$的不确定度
$z_t$——测量值，通常指传感器观测
$k_t$——卡尔曼增益系数
$H$——状态变量$x_t$到观测变量$z_t$的转换矩阵，表示将状态和观测连接起来的关系
$Q$——系统误差协方差，表示状态转移矩阵与实际过程之间的误差
$R$——传感器测量噪声协方差

注：右上角带“-”号的变量表示预测值
通过引入一个例子来理解上述公式中变量的含义。假设一辆车行驶在道路上，我们想得到它的定位信息，此时有卫星作为传感测量。当前时刻汽车的自有属性（状态）可以用位置p和速度v来表示，这就是上式4中的当前时刻状态$x_t$，也是我们想用卡尔曼滤波手段得到的结果，这个值越准确越好；而其中位置p可通过卫星来测量，此为上式中的观测值$z_t$，当然传感器是存在缺陷的，因此测得的值并不准确，存在测量噪声，定义测量噪声的协方差为R。另外卡尔曼滤波的精髓在于对当前时刻$x_t$的状态确定来源于两部分，第一来源传感器的直接观测$z_t$，第二则来源于通过运动模型对上一时刻状态$x_{t-1}$来估计出当前时刻状态$x_t^{-}$，将这两者进行加权估计，得到最终认为准确的状态$x_t$（公式4）。

在使用上一时刻$x_{t-1}$来估计出当前时刻状态$x_t^{-}$时（公式1），我们通常会假设在这相邻很小的一段时间内，汽车做匀速直线运动，因此状态转移矩阵F设置为匀速运动矩阵，可实际上汽车并非十分精确的保持匀速直线运动状态，故我们所建立的系统也会存在误差，即过程噪声，定义过程噪声的协方差为Q。此外，我们通过脚踩刹车或油门改变汽车状态，这为外部输入量，也就是公式1中的控制量$u_{t-1}$。

公式4中对估计与观测进行加权时，可以看出当卡尔曼增益$k_t$越大，则更加倾向于信任观测$z_t$，反之则倾向于估计值$x_t^{-}$。而观测矩阵H是对$x_t^{-}$进行维度变换，使之能与$z_t$进行矩阵运算。

公式2是通过上一时刻协方差$p_{t-1}$来中预测当前时刻协方差$p_t^{-}$，该公式是通过协方差的性质推导出来的，具体如下
$$cov(Ax,Ax)=Acov(x,x)A^T$$
而本样例中$p_t$取值如下
$$p_t=\left[\begin{array}{cc}D(p)& cov(p,v)\\ cov(p,v)& D(v)\end{array}\right]$$
一般来说观测矩阵H是恒定的，若系统更新的时间间隔也恒定，即状态转移矩阵F也不变，此时系统中卡尔曼增益$k_t$和系统协方差$p_t^{-}$只与Q、R有关。

再回顾一下Q作为系统误差噪声，我们是假定车辆为匀速直线运动，若Q值越大，说明我们假定匀速直线运动模型得到的$x_t^{-}$的置信度越低，也就是说车辆此时可能越不按照匀速直线来运动，所以最终$x_t$取值更倾向于观测$z_t$。
举例说明，假设状态值和观测值都是一维的，则F=H=1，公式3可简化为
$$k_t=\frac{p_{t-1}+Q}{p_{t-1}+Q+R}$$
上式中，若Q增大，R减小，则$k_t$增大，再看公式4，$x_t$取值更倾向于观测$z_t$，符合上述分析。

2.实验

下面结合代码来讲卡尔曼的概念
首先假设我们采用GPS和IMU来进行卡尔曼融合滤波。IMU的频率为100Hz，作为卡尔曼中传播来更新每个时刻的预测状态，GPS的频率为5Hz，作为观测来更新估计状态。当没有GPS数据时，IMU一直做预测，当来一帧GPS数据后，计算卡尔曼增益，再进行校正。
对于初始的系统误差协方差Q和测量噪声协方差R的设置，需要理解在此例中，IMU作为过程传播，GPS作为状态校正，因此Q设置为IMU的acc_noise和gyr_noise，表示过程传播中的协方差，因为IMU噪声的存在，状态转移矩阵不一定很好的描述这个假定的匀加速直线运动模型。

Eigen::Matrix<double, 12, 12> Qi = Eigen::Matrix<double, 12, 12>::Zero();
Qi.block<3, 3>(0, 0) = delta_t2 * acc_noise_ * Eigen::Matrix3d::Identity();
Qi.block<3, 3>(3, 3) = delta_t2 * gyro_noise_ * Eigen::Matrix3d::Identity();
Qi.block<3, 3>(6, 6) = delta_t * acc_bias_noise_ * Eigen::Matrix3d::Identity();
Qi.block<3, 3>(9, 9) = delta_t * gyro_bias_noise_ * Eigen::Matrix3d::Identity();

R设置为GPS的pose的方差，由于我们的GPS精度较高，因此这个协方差初始化可以设置的非常小，通过config文件设置初值。

measurement:
    posi_noise: 10

R矩阵设置

void ESKF::SetCovarianceR(double posi_noise) {
    R_.setZero();
    R_ = Eigen::Matrix3d::Identity() * posi_noise * posi_noise;
}

预测过程

X_ = Fk * X_;
P_ = Fk * P_ * Fk.transpose() + Bk * Q_ * Bk.transpose();

校正过程

//计算卡尔曼增益
K_ = P_ * G_.transpose() * (G_ * P_ * G_.transpose() + C_ * R_ * C_.transpose()).inverse();
//校正cov
P_ = (TypeMatrixP::Identity() - K_ * G_) * P_;
//校正状态变量
X_ = X_ + K_ * (Y_ - G_ * X_);

---

总结

卡尔曼滤波的作用其实可以简单地看作为：根据系统噪声Q和测量噪声的协方差R设定权重来纠正状态值，且使状态值更倾向于协方差较小的一方。