零基础-学习卡尔曼滤波(第二章：逐步推导公式)

前言

第一章内有原理理解的两部分，没看建议先看。闲话少说，祝君好运！！

参考

1. 【卡尔曼滤波器】,真的是大佬讲课！！牛逼就完事了。（主要参考）
2. 图解卡尔曼滤波(Kalman Filter)

3. 卡尔曼增益超详细数学推导

(一大波公式即将来临，如果看不懂，还是建议看参考1的DR_CAN的视频！！)

实际情况中我们无法得到下面这种带有噪声的精确模型：

$$\begin{gathered} 预测方程：x_k=A{x}_{k-1}+B{u}_k+w_{k-1} \\ 观测方程：z_k=H{x}_k+v_k \end{gathered}$$

我们可以得到的就是这种不带噪声的预测方程和观测方程：

$$\begin{gathered} 预测方程：x_k=A{x}_{k-1}+B{u}_k \\ 观测方程：z_k=H{x}_k \end{gathered}$$

将观测方程转换一下就得到下面两个方程：

$$\begin{gathered} {\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k \\ {\hat{x}}_{kmea}=H^{-}{z}_k \end{gathered}$$

其中${\hat{x}}_k^{-}$表示只经过预测得到的先验估计，${\hat{x}}_{k-1}$表示上一次的后验估计（融合后得出来的更准确的值），${\hat{x}}_{kmea}$表示测出来的结果。

现在我们拥有两个结果，一个是算出来的结果${\hat{x}}_k^{-}$，一个是测出来的结果${\hat{x}}_{kmea}$，但是我们的这两个结果都不准确，我们可以利用上面数据融合的思想进行融合得到更加准确的结果。

$$\begin{gathered} 后验估计：{\hat{x}}_k={\hat{x}}_k^{-}+G(H^{-}{z}_k-{\hat{x}}_k^{-}) \\ G\in [0,1] \\ 我们对G进行一个变换：G=K_{k}H，即可得到下面公式： \\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-}) \\ {K}_k\in [0,H^{-}] \end{gathered}$$

这个公式就是卡尔曼滤波中的一个重要公式！！

$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})-------（1）$$

• 我们得到了后验估计的计算方法，现在目标很明确就是需要找到一个$K_k$使得后验估计${\hat{x}}_k$趋近于真实值$x_k$

可以显而易见的发现这个$K_k$肯定和两个噪声有关，如果测量噪声很大，则让后验估计偏向于预测值，如果预测噪声很大则让后验估计偏向于测量值。

为了求解问题，我们需要首先定义误差$e_k=x_k-{\hat{x}}_k$,这个是后验预测和真实值的误差。假设这个误差也服从正态分布，即为$P(e_k)\sim (0,p)$,其中协方差p的计算为：$p=E[e{e}^T]$，我们需要后验估计${\hat{x}}_k$最接近真实值$x_k$就说明需要误差的协方差的迹$tr(p)$最小（因为误差服从期望为0的正态分布）。

这里提到真实值并不是需要得到真实，因为真实值是无法得知的，只是理论计算的过程需要。
迹：就是矩阵的对角线元素和。
为什么是协方差的迹：将协方差写出来即可知对角线元素为每个元素的方差，方差和最小就说明最接近真实值。

接下来我们需要求解$K_k$取哪个值的时，协方差的迹（$tr(p)$）最小，接下来将会是比较多公式的一个推导，但是公式一步一步看都能看懂。

$$\begin{gathered} 我们知道误差的协方差为：p=E[e{e}^T] \\ 将误差的定义公式e_k=x_k-{\hat{x}}_k带入可得如下： \\ p=E[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T]-----（2） \end{gathered}$$

$$\begin{gathered} 然后将后验误差的公式（1）带入可得(x_k-{\hat{x}}_k)为如下： \\ {x}_k-{\hat{x}}_k=x_k-({\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})) \\ 将上式展开可得如下： \\ {x}_k-{\hat{x}}_k=x_k-{\hat{x}}_k^{-}-K_k{z}_k+K_{k}H{\hat{x}}_k^{-} \\ 然后将真实的观测值的公式z_k=H{x}_k+v_k带入上式可得如下： \\ {x}_k-{\hat{x}}_k=x_k-{\hat{x}}_k^{-}-K_{k}H{x}_k-K_k{v}_k+K_{k}H{\hat{x}}_k^{-} \\ 然后提出K_{k}H可得如下： \\ {x}_k-{\hat{x}}_k=(x_k-{\hat{x}}_k^{-})-K_{k}H(x_k-{\hat{x}}_k^{-})-K_k{v}_k \\ 合并x_k-{\hat{x}}_k^{-}可得如下： \\ {x}_k-{\hat{x}}_k=(I-K_{k}H)(x_k-{\hat{x}}_k^{-})-K_k{v}_k \\ 我们将x_k-{\hat{x}}_k^{-}作为先验估计{\hat{x}}_k^{-}的误差(回忆e_k的定义)， \\ 将先验估计{\hat{x}}_k^{-}的误差记作e_k^{-},整理可得如下： \\ {x}_k-{\hat{x}}_k=(I-K_{k}H)e_k^{-}-K_k{v}_k \end{gathered}$$

$$\begin{gathered} 然后带入（2）中可得如下： \\ p=E[((I-K_{k}H)e_k^{-}-K_k{v}_k)((I-K_{k}H)e_k^{-}-K_k{v}_k{)}^T] \\ 然后将后面转置部分展开可如下： \\ -------------------- \\ 补充知识：(AB{)}^T=B^T{A}^T,(A+B{)}^T=A^T+B^T \\ -------------------- \\ p=E[((I-K_{k}H)e_k^{-}-K_k{v}_k)(e_k^{-T}(I-K_{k}H{)}^T-v_k^T{K}_k^T)] \\ 然后将括号展开可得如下： \\ p=E[(I-K_{k}H)e_k^{-}{e}_k^{-T}(I-K_{k}H{)}^T-(I-K_{k}H)e_k^{-}{v}_k^T{K}_k^T \\ -K_k{v}_k{e}_k^{-T}(I-K_{k}H{)}^T+K_k{v}_k{v}_k^T{K}_k^T] \\ 将求期望的符号E拿进去： \\ p=E((I-K_{k}H)e_k^{-}{e}_k^{-T}(I-K_{k}H{)}^T)-E((I-K_{k}H)e_k^{-}{v}_k^T{K}_k^T) \\ -E(K_k{v}_k{e}_k^{-T}(I-K_{k}H{)}^T)+E(K_k{v}_k{v}_k^T{K}_k^T) \end{gathered}$$

$$\begin{gathered} 对第二部分E((I-K_{k}H)e_k^{-}{v}_k^T{K}_k^T)进行单独分析： \\ 其中I-K_{k}H和K_k^T为常数可以直接提出来，可得如下： \\ (I-K_{k}H)E(e_k^{-}{v}_k^T)K_k^T \\ -------------------- \\ 补充知识：如果AB相互独立有：E(AB)=E(A)E(B) \\ -------------------- \\ 很明显e_k^{-},v_k^T这两个相互独立，可以使用上面补充知识 \\ 由E(e_k^{-}),E(v_k^T)都为0，所以本部分就为0，可以忽略。 \\ 同理可得第三部分也为0，忽略。 \end{gathered}$$

$$\begin{gathered} 最终得到p为如下： \\ p=E((I-K_{k}H)e_k^{-}{e}_k^{-T}(I-K_{k}H{)}^T)+E(K_k{v}_k{v}_k^T{K}_k^T) \\ 同样将常数项提取出来，可得如下： \\ p=(I-K_{k}H)E(e_k^{-}{e}_k^{-T})(I-K_{k}H{)}^T+K_{k}E(v_k{v}_k^T)K_k^T \\ 我们可以得到E(e_k^{-}{e}_k^{-T})即为先验误差的协方差矩阵p_k^{-} \\ 并且，我们上面已经定义了E(v_k{v}_k^T)即为观测值的噪声协方差矩阵R \\ 于是上面的公式可以写成如下： \\ p=(I-K_{k}H)p_k^{-}(I-K_{k}H{)}^T+K_{k}R{K}_k^T \\ 然后将前半部分展开可得如下： \\ p=(p_k^{-}-K_{k}H{p}_k^{-})(I-K_{k}H{)}^T+K_{k}R{K}_k^T \\ p=(p_k^{-}-K_{k}H{p}_k^{-})(I^T-H^T{K}_k^T)+K_{k}R{K}_k^T \\ p=(p_k^{-}-p_k^{-}{H}^T{K}_k^T-K_{k}H{p}_k^{-}+K_{k}H{p}_k^{-}{H}^T{K}_k^T)+K_{k}R{K}_k^T \\ 最终得到第k次的协方差矩阵为： \\ {p}_k=p_k^{-}-p_k^{-}{H}^T{K}_k^T-K_{k}H{p}_k^{-}+K_{k}H{p}_k^{-}{H}^T{K}_k^T+K_{k}R{K}_k^T--(7) \end{gathered}$$

$$\begin{gathered} 现在需要求迹tr(p_k)的最小值，即为如下： \\ tr(p_k)=tr(p_k^{-})-tr(p_k^{-}{H}^T{K}_k^T)-tr(K_{k}H{p}_k^{-})+ \\ tr(K_{k}H{p}_k^{-}{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T) \end{gathered}$$

$$\begin{gathered} 我们分析一下第三部分K_{k}H{p}_k^{-}的转置： \\ (K_{k}H{p}_k^{-}{)}^T=p_k^{-T}(K_{k}H{)}^T=p_k^{-T}{H}^T{K}_k^T \\ 并且我们知道协方差矩阵的转置是其本身，所以有如下： \\ (K_{k}H{p}_k^{-}{)}^T=p_k^{-}{H}^T{K}_k^T=第二部分 \end{gathered}$$

$$\begin{gathered} 所以可得如下： \\ tr(p_k)=tr(p_k^{-})-2tr(K_{k}H{p}_k^{-})+tr(K_{k}H{p}_k^{-}{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T) \end{gathered}$$

$$\begin{gathered} 现在目标很明确就是取一个合适的K_k使得tr(p_k)最小： \\ 很明显就是直接对tr(p_k)求导令其为0即可，如下： \\ \frac{d_{tr(p_k)}}{d_{K_k}}=0 \\ \frac{d_{tr(p_k^{-})-2tr(K_{k}H{p}_k^{-})+tr(K_{k}H{p}_k^{-}{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T)}}{d_{K_k}}=0 \end{gathered}$$

$$\begin{gathered} -------------------- \\ 补充知识：矩阵求导(可自行举例验证) \\ \frac{d_{tr(AB)}}{d_A}=B^T \\ \frac{d_{tr(AB{A}^T)}}{d_A}=2AB \\ 提示如下： \\ 假设A=\left[\begin{array}{cc}{a}_{11}& a_{12}\\ a_{21}& a_{22}\end{array}\right] \\ \frac{d_{tr(AB)}}{d_A}=\left[\begin{array}{cc}\frac{\partial tr(AB)}{\partial a_{11}}& \frac{\partial tr(AB)}{\partial a_{12}}\\ \frac{\partial tr(AB)}{\partial a_{21}}& \frac{\partial tr(AB)}{\partial a_{22}}\end{array}\right] \\ -------------------- \end{gathered}$$

$$\begin{gathered} 求导可得如下： \\ 0-2(H{p}_k^{-}{)}^T+2K_{k}H{p}_k^{-}{H}^T+2K_{k}R=0 \\ -p_k^{-T}{H}^T+K_{k}H{p}_k^{-}{H}^T+K_{k}R=0 \\ -p_k^{-T}{H}^T+K_k(H{p}_k^{-}{H}^T+R)=0 \\ 协方差矩阵的转置是其本身，所以有如下： \\ -p_k^{-}{H}^T+K_k(H{p}_k^{-}{H}^T+R)=0 \\ {K}_k(H{p}_k^{-}{H}^T+R)=p_k^{-}{H}^T \\ 最终得到K_k的表达式： \\ {K}_k=\frac{p_k^{-}{H}^T}{H{p}_k^{-}{H}^T+R}------(3) \\ 这个就是大名鼎鼎的卡尔曼增益！！！！ \end{gathered}$$

公式推导在这里算是结束了一大步了，依据公式（1）结合公式（3）可以分析出来，当$R$很小时（观测噪声很小），$K_k$为$H^{-1}$，带入（1）可得后验估计偏向于观测值。当$R$很da时（观测噪声很大），$K_k$为$0$，带入（1）可得后验估计偏向于预测值。这是符合我们的认知的。

其实我们现在一共已经有三个方程了（先验估计、后验估计、卡尔曼增益），现在只差误差协方差矩阵的推导就完成了整个卡尔曼滤波的推导了。这将会在下一节进行数学推导。胜利就在眼前！！！

4. 误差协方差矩阵的推导

首先我们回顾一下已经有的方程：

$$\begin{gathered} 1.系统状态方程 \\ 预测方程：x_k=A{x}_{k-1}+B{u}_k+w_{k-1}----w\sim p(0,Q) \\ 观测方程：z_k=H{x}_k+v_k----v\sim p(0,R) \end{gathered}$$

$$\begin{gathered} 2.融合方程 \\ 先验估计：{\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k \\ 后验估计：{\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-}) \\ 卡尔曼增益：K_k=\frac{p_k^{-}{H}^T}{H{p}_k^{-}{H}^T+R} \end{gathered}$$

可以看出融合方程这一步中只有$p_k^{-}$是未知的所以现在求$p_k^{-}$就可以完成整个卡尔曼滤波的公式：

$p_k^{-}$是先验误差的协方差矩阵，下面开始推导先验误差的协方差矩阵的方程：

$$\begin{gathered} p_k^{-}=E[e_k^{-}{e}_k^{-T}]----（4） \\ 由上面的方程可以得到先验误差的方程为： \\ {e}_k^{-}=x_k-{\hat{x}}_k^{-} \\ 将x_k，{\hat{x}}_k^{-}带入方程可以得到如下： \\ {e}_k^{-}=(A{x}_{k-1}+B{u}_k+w_{k-1})-(A{\hat{x}}_{k-1}+B{u}_k) \\ {e}_k^{-}=A{x}_{k-1}+B{u}_k+w_{k-1}-A{\hat{x}}_{k-1}-B{u}_k \\ {e}_k^{-}=A{x}_{k-1}+w_{k-1}-A{\hat{x}}_{k-1} \\ {e}_k^{-}=A(x_{k-1}-{\hat{x}}_{k-1})+w_{k-1} \\ 其中x_{k-1}-{\hat{x}}_{k-1}就是后验误差e_{k-1}，所以可得： \\ {e}_k^{-}=A{e}_{k-1}+w_{k-1}---（5） \end{gathered}$$

$$\begin{gathered} 然后将方程（5）带入（4）中可以得到如下： \\ {p}_k^{-}=E[(A{e}_{k-1}+w_{k-1})(A{e}_{k-1}+w_{k-1}{)}^T] \\ 展开转置部分： \\ {p}_k^{-}=E[(A{e}_{k-1}+w_{k-1})(e_{k-1}^T{A}^T+w_{k-1}^T)] \\ 继续展开可得： \\ {p}_k^{-}=E[A{e}_{k-1}{e}_{k-1}^T{A}^T+A{e}_{k-1}{w}_{k-1}^T+w_{k-1}{e}_{k-1}^T{A}^T+w_{k-1}{w}_{k-1}^T] \\ {p}_k^{-}=E(A{e}_{k-1}{e}_{k-1}^T{A}^T)+E(A{e}_{k-1}{w}_{k-1}^T)+E(w_{k-1}{e}_{k-1}^T{A}^T)+E(w_{k-1}{w}_{k-1}^T) \\ 分析第二部分(A{e}_{k-1}{w}_{k-1}^T)： \\ 其中e_{k-1}和w_{k-1}相互独立（因为e_{k-1}=x_{k-1}-{\hat{x}}_{k-1} \\ 并且x_{k-1}只和w_{k-2}有关，所以e_{k-1}和w_{k-1}相互独立) \\ 所以第二部分(A{e}_{k-1}{w}_{k-1}^T)为0 \\ 第三部分同理为0，整理上式有： \\ {p}_k^{-}=E(A{e}_{k-1}{e}_{k-1}^T{A}^T)+E(w_{k-1}{w}_{k-1}^T) \\ {p}_k^{-}=AE(e_{k-1}{e}_{k-1}^T)A^T+E(w_{k-1}{w}_{k-1}^T) \\ 由之前计算可得： \\ E(e_{k-1}{e}_{k-1}^T)为后验误差的协方差矩阵，记为p_{k-1}： \\ E(w_{k-1}{w}_{k-1}^T)为预测方程的噪声的协方差矩阵Q \end{gathered}$$

$$\begin{gathered} 总结以上计算可得，先验误差的协方差矩阵为： \\ {p}_k^{-}=A{p}_{k-1}{A}^T+Q----(6) \end{gathered}$$

注意这里先验误差的协方差矩阵的计算方程中还有一个未知的东西就是$p_{k-1}$就是上一次的后验误差的协方差矩阵，接下来继续推导最后一个方程。

$$\begin{gathered} 这里回忆一下好像之前我们推导过这个p_{k-1} \\ 往上翻我们找到方程（7） \\ {p}_k=p_k^{-}-p_k^{-}{H}^T{K}_k^T-K_{k}H{p}_k^{-}+K_{k}H{p}_k^{-}{H}^T{K}_k^T+K_{k}R{K}_k^T \\ 我们进行变换： \\ {p}_k=p_k^{-}-p_k^{-}{H}^T{K}_k^T-K_{k}H{p}_k^{-}+K_k(H{p}_k^{-}{H}^T+R)K_k^T \\ 然后将卡尔曼增益K_k带入第三部分可得如下： \\ {p}_k=p_k^{-}-p_k^{-}{H}^T{K}_k^T-K_{k}H{p}_k^{-}+\frac{p_k^{-}{H}^T}{H{p}_k^{-}{H}^T+R}(H{p}_k^{-}{H}^T+R)K_k^T \\ {p}_k=p_k^{-}-p_k^{-}{H}^T{K}_k^T-K_{k}H{p}_k^{-}+p_k^{-}{H}^T{K}_k^T \\ 第二部分和最后一部分相同，抵消掉，得到如下： \\ {p}_k=p_k^{-}-K_{k}H{p}_k^{-} \\ {p}_k=(I-K_{k}H)p_k^{-}----（8） \\ 这就是完成了误差协方差的更新。 \end{gathered}$$

直到这一步我们将所有未知的东西都给出了计算方法，以上已经完成了整个卡尔曼滤波的数学推导。接下来进行一下总结。

5. 总结

$$\begin{gathered} 1.系统状态方程 \\ 预测方程：x_k=A{x}_{k-1}+B{u}_k+w_{k-1}----w\sim p(0,Q) \\ 观测方程：z_k=H{x}_k+v_k----v\sim p(0,R) \end{gathered}$$

$$\begin{gathered} 2.融合方程 \\ 先验估计：{\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k \\ 后验估计：{\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-}) \\ 卡尔曼增益：K_k=\frac{p_k^{-}{H}^T}{H{p}_k^{-}{H}^T+R} \\ 先验误差协方差：p_k^{-}=A{p}_{k-1}{A}^T+Q \\ 更新误差协方差：p_k=(I-K_{k}H)p_k^{-} \end{gathered}$$

将第二部分按照预测和矫正进行分列，可得如下两组卡尔曼滤波的方程（最终方程）：

$$\begin{gathered} 预测： \\ 先验估计：{\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k \\ 先验误差协方差：p_k^{-}=A{p}_{k-1}{A}^T+Q \end{gathered}$$

$$\begin{gathered} 矫正： \\ 卡尔曼增益：K_k=\frac{p_k^{-}{H}^T}{H{p}_k^{-}{H}^T+R} \\ 后验估计：{\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-}) \\ 更新误差协方差：p_k=(I-K_{k}H)p_k^{-} \end{gathered}$$

完结撒花！！！

---

代码实现

#include <cmath>
#include <random> // Need this for sampling from distributions
#include <vector>
#include <fstream>
#include <iostream>
#include <Eigen/Eigen>

using namespace std;

class kalman_filter
{
private:
	// -------------预测方程参数-------------
	// 状态转移矩阵，上一时刻的状态转移到当前时刻
	Eigen::Matrix2d A;
	// 控制输入矩阵B
	Eigen::Matrix2d B;
	// 过程噪声协方差矩阵Q，p(w)~N(0,Q)，噪声来自真实世界中的不确定性
	Eigen::Matrix2d Q;

	// -------------观测方程参数-------------
	// 状态观测矩阵
	Eigen::Matrix2d H;
	// # 观测噪声协方差矩阵R，p(v)~N(0,R)，噪声来自测量过程中的不确定性
	Eigen::Matrix2d R;

	// -------------系统变量-------------
	Eigen::Vector2d X0; // 初始位置与速度
	Eigen::Matrix2d P0; // 状态估计协方差矩阵P初始化
	Eigen::Vector2d X_true; // 真实状态
	Eigen::Vector2d X_Prior; // 先验估计
	Eigen::Vector2d X_posterior; // 后验估计
	Eigen::Matrix2d P_Prior; // 状态先验误差协方差矩阵P
	Eigen::Matrix2d P_posterior; // 状态后验误差协方差矩阵P
	Eigen::Vector2d Z_measure; // 观测值
	Eigen::Matrix2d K; // 卡尔曼增益
	Eigen::Matrix2d I; // 单位矩阵

	// -------------数据记录-------------
	std::vector<double> speed_true; // 真实记录
	std::vector<double> position_true;

	std::vector<double> speed_measure; // 测量记录
    std::vector<double> position_measure;

    std::vector<double> speed_prior_est; //先验估计
    std::vector<double> position_prior_est;

    std::vector<double> speed_posterior_est; // 后验估计
    std::vector<double> position_posterior_est;

public:
	kalman_filter(/* args */);
	~kalman_filter();
	void kalman_filter_demo();
	double gaussian_distribution_generator(double mu, double sigma);
};

kalman_filter::kalman_filter(/* args */) //初始化参数
{
	// 系统参数初始化
	A << 1,1,  0,1;
	B << 0,0,  0,0;
	Q << 0.1,0,  0,0.1;
	H << 1,0,  0,1;
	R << 1,0,  0,1;
	X0 << 0,1;
	P0 << 1,0,  0,1;
	I << 1,0, 0,1;

	X_true = X0;
	X_posterior = X0;
	P_posterior = P0;
}

kalman_filter::~kalman_filter()
{
}

double kalman_filter::gaussian_distribution_generator(double mu, double sigma)
{
	std::random_device rd{};
    std::mt19937 gen{rd()};
	std::normal_distribution<double> w{mu,sigma};
	return w(gen);
}

void kalman_filter::kalman_filter_demo()
{
	fstream f;
	f.open("../data.txt",ios::out);
	for(int i = 0; i < 30; i++)
	{
		// -----------------------生成真实值----------------------
		Eigen::Vector2d w;
		w << gaussian_distribution_generator(0,Q(0,0)),
			 gaussian_distribution_generator(0,Q(1,1));
		// 生成当前时刻状态的估计值(这里没有控制矩阵)
		X_true = A*X_true + w;
		position_true.push_back(X_true(0,0));
		speed_true.push_back(X_true(1,0));

		// -----------------------生成观测值----------------------
		Eigen::Vector2d v;
		v << gaussian_distribution_generator(0,R(0,0)),
			 gaussian_distribution_generator(0,R(1,1));
		// 生成当前时刻状态的测量值
		Z_measure = H*X_true + v;
		position_measure.push_back(Z_measure(0,0));
		speed_measure.push_back(Z_measure(1,0));

		// ----------------------以下开始进行卡尔曼滤波---------------------

		// ----------------------先验估计---------------------
		X_Prior = A*X_posterior;
		position_prior_est.push_back(X_Prior(0,0));
		speed_prior_est.push_back(X_Prior(1,0));

		// // ----------------------先验误差协方差---------------------
		P_Prior = (A*P_posterior)*A.transpose() + Q;

		// // ----------------------卡尔曼增益---------------------
		K = (P_Prior*H.transpose())*((H*P_Prior)*H.transpose()+R).inverse();

		// // ----------------------后验估计---------------------
		X_posterior = X_Prior + K*(Z_measure - H*X_Prior);
		position_posterior_est.push_back(X_posterior(0,0));
		speed_posterior_est.push_back(X_posterior(1,0));

		// // ----------------------后验估计---------------------
		P_posterior = (I - K*H)*P_Prior;

		// 保存到文件中
		f<< position_true.back() << " " << speed_true.back() << " "
		 << position_measure.back() << " " << speed_measure.back() << " "
		 << position_prior_est.back() << " " << speed_prior_est.back() << " "
		 << position_posterior_est.back() << " " << speed_posterior_est.back() << " "
		<<endl;
	}
	f.close();
}



int main(int argc, char **argv)
{
	kalman_filter kf;
	kf.kalman_filter_demo();
    return 0;
}

第一次写博客，希望给点建议。这算是一个个人学习笔记吧，主要是参考DR_CAN的视频，讲的非常好！！！！