卡尔曼滤波器与C++实现(kf, ekf)

卡尔曼滤波器讲解与C++实现(kf, ekf)

C++代码：如果有用，请点个star支持一下
卡尔曼C++代码

简介

​ 第一次接触卡尔曼滤波器(kalman filter，下面简称为KF)是在大三做比赛的时候，现在刚入研究生，中间虽然只有短短两年，但应用kf解决问题的次数以及调试KF的时间已经数不胜数；现在网上也有很多关于KF的介绍和推导，其中有很多讲解的非常不错，但我还是在此简单提一下我个人的看法与分析，这是我发的第一篇文章，原因其实很大程度是暑假未入学，课题组任务也不重，有些闲暇时间。

​ 我个人学习理论知识喜欢结合实际问题，所以本文也以机器人运动为例，分析讲解KF。

​ 首先，以一个正在运动的机器人为例，为了简化问题，我们假设这个机器人是一维的，也就是只有x维度的信息，同时它是匀速运动的；所以对于这个机器人来讲，如果我们想要尽可能准确的知道它当前时刻的状态信息(滤波)，或者我们想知道它下一时刻的状态信息(预测)，我们所知道或者想知道的它的信息有：x方向的坐标(posx)，x方向的速度(vx)，我们统一用状态量x向量来包含这些机器人信息：
$$x=\left[\begin{array}{c}pos\\ v\end{array}\right]\text{\hspace{1em}(1)}$$
​ 当然你可能会说实际情况还会有角速度，加速度等等运动信息，我们这里是作了极大的简化以方便理解，因为对于匀速运动来讲，你只需要知道当前时刻的位置和速度，就可以知道下一时刻的位置和速度，也就是卡尔曼的预测功能，所以我们只需要这四个量即可；这时候你可能就要问到卡尔曼的滤波功能，毕竟名字就是卡尔曼滤波。滤波是因为在实际问题中，假如我们想要获取一个机器人的位置，你也许会通过读各种传感器信息，但是这些传感器信息反馈的值绝对是有误差的，无非是大小的区别；这也就意味着我们是不可能在实际问题中拿到任何真值，但我们又希望我们拿到的值尽可能的接近真值，这时候就要用到卡尔曼的滤波作用，在这个问题中，我们所知道的机器人状态x向量中的值也均不是真值。

预测部分 predict

​ 好，现在我们知道了机器人的初始状态量，我们如何计算下一时刻的状态呢？上文已经提到在这个问题中机器人将一直进行匀速运动，对于匀速运动的公式相信大家都很熟悉，所以我们分别对xy方向计算下一时刻的坐标：
$$\begin{gathered} pos=1\times pos+vt \\ v=0\times pos+1\times v\text{\hspace{1em}(2)} \end{gathered}$$
​ 但我们计算的时候都是以机器人的状态量为一个整体计算的，所以将上述k-1到k时刻的状态变化用状态量向量表示：
$$\begin{gathered} \left[\begin{array}{c}po{s}_k\\ v_k\end{array}\right]=\left[\begin{array}{c}1t\\ 01\end{array}\right]\times \left[\begin{array}{c}po{s}_{k-1}\\ v_{k-1}\end{array}\right] \\ {x}_k=A{x}_{k-1}\text{\hspace{1em}(3)} \end{gathered}$$
​ A就称为状态转移矩阵，顾名思义从上一个状态转换到下一个状态。在匀速运动时，我们知道机器人下一时刻的位置取决于上一时刻的位置以及它本身的速度，所以对于机器人来讲，它的位置信息于速度信息不是相互独立的，而是有一定相关性，这种相关性对于我们预测下一时刻的信息有重要意义，所以为了捕获这种相关性，我们采用协方差矩阵，相信大家也都有了解，不了解的话一句话说就是协方差矩阵中每个元素值都是第i个变量和第j个变量的相关程度，用符号$\sum$表示，用P矩阵表示出来则是：
$$P_k=\left[\begin{array}{c}{\sum }_{pp}{\sum }_{pv}\\ {\sum }_{vp}{\sum }_{vv}\end{array}\right]\text{\hspace{1em}(4)}$$
​ 可以注意到这里对P矩阵的描述是带有时间信息k的，也就意味着类似于状态向量，P矩阵也是要进行不同时刻之间的状态转移，因为P反映的是状态向量x状态量之间的相关程度，所以P矩阵的转移矩阵其实就是状态向量x的转移矩阵，这里我们要利用协方差矩阵公式：
$$\begin{gathered} Cov(x)=P \\ Cov(Ax)=AP{A}^T\text{\hspace{1em}(5)} \end{gathered}$$
​ 所以$P_k$为：
$$P_k=A{P}_{k-1}{A}^T\text{\hspace{1em}(6)}$$
​ 这就是匀速运动时候的机器人状态转移与协方差转移公式，但假如我们在k时刻推了机器人一把，机器人受到外部影响，不再进行匀速运动，而是有了一定的加速度$a$，这时候机器人的位置和速度就会变化为：
$$\begin{gathered} po{s}_k=1\times po{s}_{k-1}+v_{k-1}t+\frac{1}{2}a{t}^2 \\ {v}_k=0\times po{s}_{k-1}+1\times v_{k-1}+at\text{\hspace{1em}(7)} \end{gathered}$$
​ 用矩阵表示为：
$$x_k=A{x}_{k-1}+\left[\begin{array}{c}\frac{1}{2}{t}^2\\ t\end{array}\right]a\text{\hspace{1em}(8)}$$
​ 我们用$u_k$表示控制向量，用$B$表示控制量矩阵：
$$x_k=A{x}_{k-1}+B{u}_k\text{\hspace{1em}(9)}$$
​ 当然如果你的机器人模型是一直保持匀速运动的，你可以认为控制量一直为0即可，增加这个公式项是为了更准确的拟合运动状态，使得kf对匀加速度运动模型也有适用性。

​ 还有一个非常重要的地方我们没考虑进去：外部因素干扰，机器人在运动时候可能轮胎打滑，或是遇到大风难以行进等等不可掌握的因素我们也需要考虑进去，我们用一个矩阵Q来统一表示这些所有的误差因素，Q也是卡尔曼滤波器调试时的主要改变参数矩阵：
$$\begin{gathered} x_{pre}=A{x}_{k-1}+B{u}_k \\ {P}_k=A{P}_{k-1}{A}^T+Q\text{\hspace{1em}(10)} \end{gathered}$$
​ 这样，卡尔曼滤波的预测部分就建模完成，$x_{pre}$是当前时刻的预测值，这个值是我们基于数学模型计算出来的，肯定与真实值有较大差距，所以我们还需要利用传感器的值或是测量的机器人值进一步更新以获取更好的结果。

更新部分 update

​ 从现代控制理论的角度来讲，卡尔曼滤波器是一种状态观测器，也就意味着如果你想要对机器人的某些状态向量运用卡尔曼滤波，你首先要确保这些状态量是直接可观测或者间接可观测的的，通俗点来说就是这些状态量你可以通过传感器或者计算得到；比如现在你有机器人的位置和速度，你可以对位置进行卡尔曼滤波处理，但你不能对机器人重量形状尺寸等等进行处理；从卡尔曼的原理来讲，即是更新部分受限制，需要有观测量$z_k$的参与。

​ 依旧以上述机器人为例，在某一时刻，机器人身上的传感器返回给我们机器人当前的位置信息$pos$，我们依旧将他化为向量形式：
$$z_k=[pos]\text{\hspace{1em}(11)}$$
​ 同时我们也提到过，传感器的测量值肯定不是完全准确的，会有各种因素引起的误差，我们将这些不确定性的协方差用矩阵$R$表示

​ 现在我们手里有两个高斯分布（卡尔曼假设状态量是随机的，符合高斯分布，即每一个状态量都有一个均值$\mu$和方差${\sigma }^2$，我们通过预测得出的状态量是一个高斯分布，传感器拿到的值也是一个高斯分布）。

​ 所以，如果我们想要获得尽可能准确的结果，就是希望得到两个高斯分布中间交叉的部分，数学方法来讲，就是两个高斯分布的乘积，乘积后的结果其实也是一个新的高斯分布，接下来我将简单推导一下结果。

​ 对于一个标准的高斯分布来讲，其数学表达式为：
$$N(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}\text{\hspace{1em}(12)}$$

$$N(x,{\mu }_1,{\sigma }_1)\times N(x,{\mu }_2,{\sigma }_2)=N(x,{\mu }_{new},{\sigma }_{new})\text{\hspace{1em}(13)}$$

$$\begin{gathered} {\mu }_{new}={\mu }_1+\frac{{\sigma }_1^2({\mu }_2-{\mu }_1)}{{\sigma }_1^2+{\sigma }_2^2} \\ {\sigma }_{new}={\sigma }_1^2-\frac{{\sigma }_1^4}{{\sigma }_1^2+{\sigma }_2^2}\text{\hspace{1em}(14)} \end{gathered}$$

​ 我们用K来简化一下表达式
$$\begin{gathered} k=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2} \\ {\mu }_{new}={\mu }_1+k({\mu }_2-{\mu }_1) \\ {\sigma }_{new}^2={\sigma }_1^2-k{\sigma }_1^2\text{\hspace{1em}(15)} \end{gathered}$$
​ 我们前面已经提到，传感器数据符合高斯分布：$(z_k,R)=({\mu }_2,{\sigma }_2^2)$，我们预测的结果也符合高斯分布$(x_{pre}，P_k)=(\mu ,\sigma )$，看起来好像可以带入到式子15中去，但还有一个关键的问题就是$x_{pre}$到$z_k$的转换问题，这里我用一个例子讲一下我个人的理解，如果有更好的想法可以提出来：假如我们想要获取机器人的位置信息，单位是m，我们利用公式算出来的预测值单位也是m，但是传感器是别人设计的，它返回的值单位是cm，这时候我们不能直接让预测值与传感器值结合计算，而是有一个单位的转换，就引出了H矩阵，当然如果你的单位是一致的，H矩阵对应的元素值就是1，当然这只是很简单的例子，实际问题远比现在复杂得多，尤其是用到ekf的时候。
$$z_{pre}=H{x}_{pre}\text{\hspace{1em}(16)}$$
​ 转换之后的预测量也是符合高斯分布的：$(H{x}_{pre},H{P}_k{H}^T)=({\mu }_1,{\sigma }_1)$，我们再代入到式子15中可以得到：
$$\begin{gathered} K=H{P}_k{H}^T(H{P}_k{H}^T+R_K{)}^{-1} \\ {x}_k=x_{pre}+K(z_k-H{x}_{pre}) \\ {P}_k=P_k-KH{P}_k\text{\hspace{1em}(17)} \end{gathered}$$
​ 式子17便是卡尔曼的更新公式，这里的K矩阵其实是方程的最优解，有兴趣的可以去详细看一下K的由来，上面12-15的公式描述是参考[Bzarg][How a Kalman filter works, in pictures | Bzarg作者的写法，以便理解。

​ 好，现在对于卡尔曼公式的推导已经基本完成，$x_k$就是我们所拿到的最优解，理论上讲它是最接近真实值的解，所以这也就是滤波作用，当然卡尔曼本身也是需要一个收敛过程的，在编写程序的时候要等一定的循环之后才会有较好的滤波作用。

扩展卡尔曼滤波器 EKF

​ 从上面的推导可以看到，卡尔曼滤波对于解决匀速或者匀加速度运动这种线性模型很适用，但实际问题中，基本不存在匀速或者匀加速这么简单的情况，机器人的运动很多时候相当复杂，绝不是线性运动这么简单，这也就引出了扩展卡尔曼滤波EKF，用来解决非线性问题。

​ 非线性问题的难点在于其无法用数学模型表示，所以相对于KF，EKF表达式上面的不同就在于运动公式上面：
$$\begin{gathered} x_{pre}=f(x_{k-1}) \\ {z}_{pre}=h(x_{pre}) \\ {x}_k=x_{pre}+K(z_k-h(x_{pre}))\text{\hspace{1em}(18)} \end{gathered}$$
​ EKF的核心就是将非线性问题转换为线性问题然后用KF求解，对于非线性化的$f(x_{k-1}),h(x_{pre})$，采用泰勒展开的方式线性化即可：
$$\begin{gathered} f(x_k)=f(x_{k|k}^{‘})+\frac{\partial f}{\partial x_k}(x_k-x_{k|k}^{‘})+o(x_k-x_{k|k}^{‘}) \\ A=\frac{\partial f}{\partial x_k}{|}_{x_k-x_{k|k}^{‘}} \\ {x}_{k+1}=f(x_{k|k}^{‘})+A(x_k-x_{k|k}^{‘})+w_k \\ h(x_{x+1})=h(x_{k+1|k}^{‘})+\frac{\partial h}{\partial x_{k+1}}(x_k-x_{k+1|k}^{‘})+o(x_k-x_{k+1|k}^{‘}) \\ H=\frac{\partial h}{\partial x_{k+1}}{|}_{x_k-x_{k+1|k}^{‘}} \\ {z}_{k+1}=h(x_{k+1|k}^{‘})+H(x_k-x_{k+1|k}^{‘})+v_{k+1}\text{\hspace{1em}(19)} \end{gathered}$$
​ 然后其余步骤都与KF相同，带入计算即可，无非就是公式编写长了些；但就我个人经验来讲，其实很多复杂的运动，当处理步长较小时，都可以用最简单的匀速或者匀加速模型解决，微积分的思想，在很短时间内机器人是匀速运动的。

C++实现

​ KF的计算过程基本是以矩阵为单位计算的，所以C++编写程序时，我采用了Eigen3作为矩阵运算库，当然OpenCV也可以进行矩阵运算，甚至你自己创建多维数组不用任何库计算也可以，但引入Eigen使C++矩阵运算非常简单，所以我个人还是比较推荐Eigen的，安装方法的话网上都有，很简单，我这里以Windows cmake编译为例。

​ 首先是对各种矩阵的定义，以及初始化，为了方便我只拿部分矩阵举例，详细可以看我上传到github的代码，文章一开头便有链接。

Eigen::VectorXd x_p_k;  // predicted state quantity
Eigen::MatrixXd	Q;  // process error covariance matrix
Eigen::MatrixXd R;  // measurement error covariance matrix

x_p_k = Eigen::VectorXd::Zero(dim_x, 1);
Q = Eigen::MatrixXd::Zero(dim_x, dim_x);
R = Eigen::MatrixXd::Zero(dim_z, dim_z);
// dim_x是状态向量的维数，比如机器人是二维匀速运动，它的维度就一般为4(x,y,vx,vy)
// dim_z是测量向量的维数，比如机器人是二维匀速运动，它的维度就一般为2(x,y)

​ 然后直接写公式就可以，KF在进行计算前要初始化，传入初始状态信息

void KalmanFilter::init(Eigen::VectorXd &x_k) {
    this->x_p_k = x_k;
    this->x_l_k = x_k;
    this->P = Eigen::MatrixXd::Zero(dim_x, dim_x);
}

​ 然后再进行预测部分，传入参数u是控制量，t是计算步长

Eigen::VectorXd KalmanFilter::predict(Eigen::VectorXd &u, double t) {
    double delta_t = t;
    for (int i = 0; i < dim_x/2; i++) {
        A(i, dim_x/2 + i) = delta_t;
    }	//	A矩阵的赋值，赋值方法是匀速运动模型，参考上文
    x_p_k = A * x_l_k + B * u;
	P = A * P * A.transpose() + Q;
	return x_p_k;
}

​ 这时候有了预测值之后就可以融合测量值进行更新，传入测量向量

Eigen::VectorXd KalmanFilter::update(Eigen::VectorXd &z_k) {
    for (int i = 0; i < dim_z; i++) {
        H(i,i) = 1;
    }	//	H矩阵的赋值，假设测量与预测的单位一致，参考上文
    K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
	x_k = x_p_k + K * (z_k - H * x_p_k);
	P = P - K * H * P;
    x_l_k = x_p_k;	//	更新x_l_k，因为下一个循环预测部分需要用到
	return x_k;
}

​ 这里只写了主要公式部分的C++编写，完整程序以及测试方法程序请去文章开头的github连接查看或者下载，这里就不过多赘述，代码里也包含一个采用ctrv模型的EKF代码程序，有兴趣可以稍微了解一下，但是由于是以前写的，所以格式或者某些地方不太合理，做个参考就好。
感谢阅读！