Kalman滤波器的原理与实现

卡尔曼滤波器(Kalman Filter)是一个十分强大滤波器,虽然叫做滤波器，卡尔曼滤波器其实可以起到到两个作用，即预测与更新，这与我们在其运行时所关注的环节有关。当我们关注预测状态量这一步时，我们可以通过卡尔曼滤波器获取状态量的超前预测值，预测的值取决于滤波对象的运动建模。而当我们关注更新来获取最优估计状态时，我们关心的是如何通过卡尔曼增益以及测量量与估计量来获取当前的最优估计状态，这时卡尔曼滤波就像一个正常的滤波器。本篇是看完Dr_Can博士对于卡尔曼滤波的讲解后的小笔记。
链接在这Dr_Can的卡尔曼滤波器讲解

一些参数明确

在解释卡尔曼滤波之前，需要先明确一些参数，如下

$x_{k}$ 为 $k$ 时刻的状态量的真实值
${\hat{x}}_{k}^{-}$ 为 $k$ 时刻的状态量的先验预测值
${\hat{x}}_{k - 1}$ 为 $k - 1$ 时刻的状态量的最优后验估计值
$z_{k}$ 为 $k$ 时刻的测量量
$u$ 为外部输入的控制量
$B$ 为控制量矩阵
$A$ 为状态转移矩阵
$P^{-}$ 为误差协方差矩阵的先验预测值
$P$ 为误差协方差矩阵
$H$ 为观测转移矩阵
$Q$ 为状态噪声协方差矩阵
$R$ 为测量噪声协方差矩阵
$K_{k}$ 为卡尔曼增益

例子引出

给出一个匀速运动模型的例子来引出卡尔曼滤波。这也是一个经典的例子。

给出一个做匀速直线运动的小球，其速度为 $V$ ,其相对于某坐标系的位置为 $x$ ,根据运动学相关的知识，可以写出小球的运动方程 $x_{t} = x_{t - 1} + V_{x} * t$ 。假设小球不仅在 $x$ 方向上有运动，在 $y$ 方向上也有同样的匀速直线运动，其 $y$ 方向的运动方程为 $y_{t} = y_{t - 1} + V_{y} * t$ 。即我们现在有了描述小球运动的两个方程,再加上速度的两个方程 $V_{x t} = V_{x t - 1}$ 以及 $V_{y t} = V_{y t - 1}$ 。我们称物体的以上的变量为物体的状态量，也就是我们想要获得一些量。

x_{t} = x_{t - 1} + V_{x t - 1} t y_{t} = y_{t - 1} + V_{y t - 1} t V_{x t} = V_{x t - 1} V_{y t} = V_{y t - 1}

不妨写为矩阵相乘的形式

[\begin{matrix} x_{t} \\ y_{t} \\ V_{x t} \\ V_{y t} \end{matrix}] = [\begin{matrix} 1 & 0 & t & 0 \\ 0 & 1 & 0 & t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{t - 1} \\ y_{t - 1} \\ V_{x t - 1} \\ V_{y t - 1} \end{matrix}]

倘若假设采样时间为1即 $t = 1$ ,可以写成

[\begin{matrix} x_{t} \\ y_{t} \\ V_{x t} \\ V_{y t} \end{matrix}] = [\begin{matrix} 1 & 0 & 1 & 0 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{t - 1} \\ y_{t - 1} \\ V_{x t - 1} \\ V_{y t - 1} \end{matrix}]

可以看出， $t$ 时刻物体的状态量可以由 $t - 1$ 时刻物体的状态量来推算出来。此时我们将上式记为

x_{t} = A x_{t - 1}

显然有时物体并不会一直线性运动，假设此时物体被外部因素赋予(输入)一个加速度 $a$ ,则物体此时的运动方程变为

x_{t} = x_{t - 1} + V_{x t - 1} t + \frac{1}{2} a t^{2} V_{x t} = V_{x t - 1} + a t

则矩阵形式的方程变为

[\begin{matrix} x_{t} \\ y_{t} \\ V_{x t} \\ V_{y t} \end{matrix}] = [\begin{matrix} 1 & 0 & t & 0 \\ 0 & 1 & 0 & t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{t - 1} \\ y_{t - 1} \\ V_{x t - 1} \\ V_{y t - 1} \end{matrix}] + [\begin{matrix} 0 & \frac{1}{2} t^{2} \\ 0 & \frac{1}{2} t^{2} \\ 0 & t \\ 0 & t \end{matrix}] [\begin{matrix} a_{x} \\ a_{y} \end{matrix}]

不妨记为

x_{t} = A x_{t - 1} + B u

这样我们就得出了如何使用前一时刻或者说前一次的状态量来获得预测当前的状态量。不难看出，预测的准不准，取决于对观测物体运动建模的准确性。

倘若模型不准确，根据该模型推理出的数据是有噪声干扰和误差的,我们将这个噪声干扰叫做 $w_{i}$ 。并假设噪声的概率分布符合期望为0的正态分布(这是卡尔曼滤波的重要假设之一)，即 $P (w_{i}) \sim N (0, Q)$ 。加上这个误差后的公式为

x_{t} = A x_{t - 1} + B u + w_{i}

需要注意的是 $w_{i}$ 也为一个矩阵，同理 $Q$ 也为一个协方差矩阵,这是一个多元正态分布。

此时我们再来考虑更多一种情况。倘若我们不仅可以通过模型对状态量进行预测得出当前时刻的状态量，同时还可以通过一些测量仪器来获得物体的当前状态的测量值。

在这个例子中可以认为有一个雷达来检测物体每一时刻的位置信息以及速度信息。这些测量值统称为 $z_{k}$ ,为一个矩阵。注意测量量的维度并不要求与状态量相同。考虑到有时测量量与状态量可能需要些许转换，如测量电阻时我们总是使用电压处以电流( $R = \frac{U}{I}$ )的转换，我们将这种转换关系使用一个矩阵 $H$ 表示， $H$ 就表示状态量可以通过某个公式或者关系转换到测量量。在上面的例子中， $H$ 为单位矩阵。综上，可以得出以下式子。

z_{k} = H x_{k}

当然，考虑到测量仪器测量时的各种噪声以及误差，我们将其记为 $v_{i}$ ,并假设其概率分布也满足正态分布，即 $P (v_{i}) \sim N (0, R)$ 。
最终得出

z_{k} = H x_{k} + v_{i}

最后，我们好像得出了观测物体的两个值，一个是通过不太精确的模型得出的预测值，另一个是通过不太准确的仪器测出的当前的测量值。我们所有的信息只有这两个值。那么如何在一些合理的假设下，通过这两个不太准确的值，获得最接近物体真实值的最优解呢？这便是这便是卡尔曼滤波所要做的事情，通过两个不准确值来估计出最接近真实值的最优解。

写在题外：这两个公式实质上的严格数学推导是由现代控制理论里的状态空间表达式的离散形式得出的，想要了解更多的推导请看Dr_Can的卡尔曼滤波器讲解，讲的非常详细易懂！

卡尔曼滤波简单推导

下面对卡尔曼滤波的一些思想和公式进行简单的说明

数据融合

考虑最简单的均值滤波，对于物体的某一属性(如上述的位置 $x$ )我们已经有 $k - 1$ 个已经测量到的值，以及当前第 $k$ 次的测量值。为了利用当前这些数据获取接近真实值的值，我们最容易想到的就是均值滤波，也就是从1到 $k$ 的测量取均值。

\hat{x_{k}} = \frac{1}{k} \sum_{i = 1}^{k} x_{i}

不妨将其做以下变换

\hat{x_{k}} = \frac{1}{k} (x_{1} + . . . + x_{k - 1} + x_{k}) \hat{x_{k}} = \frac{1}{k} \sum_{i = 1}^{k - 1} x_{i} + \frac{1}{k} x_{k} \hat{x_{k}} = \frac{k - 1}{k} \frac{1}{k - 1} \sum_{i = 1}^{k - 1} x_{i} + \frac{1}{k} x_{k} 记 \frac{1}{k - 1} \sum_{i = 1}^{k - 1} x_{i} = \hat{x_{k - 1}} \hat{x_{k}} = \frac{k - 1}{k} \hat{x_{k - 1}} + \frac{1}{k} x_{k} = \hat{x_{k - 1}} + \frac{1}{k} (\hat{x_{k - 1}} - x_{k})

最终我们得出了这个式子

\hat{x_{k}} = \hat{x_{k - 1}} + \frac{1}{k} (x_{k} - \hat{x_{k - 1}})

不难看出，当前 $k$ 时刻的估计值由 $x_{k - 1}$ 时刻的估计值 $\hat{x_{k - 1}}$ 以及 $k$ 时刻的测量值 $x_{k}$ 来共同决定，而 $\frac{1}{k}$ 这个系数决定了这两者的占比。我们不妨令 $\frac{1}{k} = G$ ，这里姑且称其为卡尔曼增益。且 $\hat{x_{k - 1}}$ 可以视为基于前 $k - 1$ 次的数据对第 $k$ 次的数据的一个预测或者说估计。不妨即为 ${\hat{x_{k}}}^{-}$

利用这个思想，对于小球的例子中， $x_{k}$ 时刻的预测值可以由运动方程给出

{\hat{x_{k}}}^{-} = A \hat{x_{k - 1}} + B u_{k - 1}

且 $k$ 时刻的测量量满足以下

z_{k} = H x_{k} x_{k} = H^{-} z_{k}

利用上述思想，得出 $k$ 时刻的估计值为

\hat{x_{k}} = {\hat{x_{k}}}^{-} + G (x_{k} - {\hat{x_{k}}}^{-}) = {\hat{x_{k}}}^{-} + G (H^{-} z_{k} - {\hat{x_{k}}}^{-})

不妨假设 $G = K_{k} H$ ,带入得出

\hat{x_{k}} = {\hat{x_{k}}}^{-} + K_{k} (z_{k} - H {\hat{x_{k}}}^{-})

这便是卡尔曼滤波的核心公式之一，我们使用这个公式来估计当前状态量的最优值。至于为何为最优，后续会给出简单说明。

协方差矩阵

这部分是概率统计的知识。对于随机变量 $X$ ，我们可以用期望 $E (x)$ 来描述它的平均水平或者一个趋势，同时可以使用方差 $D (x)$ 或者 $V a r (x)$ 来描述其波动情况。同时我们定义这样一个式子

C o v (X, Y) = E ((X - E (X)) (Y - E (Y)))

称 $C o v (X, Y)$ 为两个随机变量的协方差。不难看出，协方差描述了两个变量之间的相关程度，具体可以看这个通俗理解协方差。当两个变量相互独立时，其协方差为0,说明彼此毫无关系，互不影响。

对于两个随机变量 $X$ 和 $Y$ ，可以给出一个矩阵来描述其各个变量之间的关系

P = [\begin{matrix} σ_{x x} & σ_{x y} \\ σ_{y x} & σ_{y y} \end{matrix}]

这样使用协方差矩阵就可以记录出2个以及多个随机变量之间的关系。

对于 $e = [e_{1} e_{2}]$ 假设随机变量 $e_{1}, e_{2}$ 符合正态分布 $(0, σ)$ (基本上卡尔曼滤波就是建立在所有误差都是正态分布这个假设上的),其各个元素协方差矩阵可以这样求

E [e * e^{T}]

展开即为

[\begin{matrix} E (e_{1} e_{1}) & E (e_{1} e_{2}) \\ E (e_{2} e_{1}) & E (e_{2} e_{2}) \end{matrix}]

已知方差的计算式有 $D (x) = E (x^{2}) - E^{2} (x)$ ,且我们已知符合期望为0的正态分布，则 $E (x) = 0$ ，故有 $D (x) = E (x^{2})$ ，同理对于协方差的计算式 $C o v (X, Y) = E (X Y) - E (X) E (Y)$ ,应用如上条件变为 $C o v (X, Y) = E (X Y)$ ,这两个公式应用于上式有

[\begin{matrix} E (e_{1}^{2}) & E (e_{1} e_{2}) \\ E (e_{2} e_{1}) & E (e_{2}^{2}) \end{matrix}] = [\begin{matrix} σ_{e_{1} e_{1}} & σ_{e_{1} e_{2}} \\ σ_{e_{2} e_{1}} & σ_{e_{2} e_{2}} \end{matrix}]

这个公式在推导卡尔曼增益的计算式时会用到。

卡尔曼公式简单推导

先给出卡尔曼滤波的五个核心公式，然后对其做一些说明

{\hat{x_{k}}}^{-} = A \hat{x_{k - 1}} + B u_{k - 1} [1] P_{k}^{-} = A P_{k - 1} A^{T} + Q [2] K_{k} = P_{k}^{-} H^{T} / (H P_{k}^{-} H^{T} + R) [3] \hat{x_{k}} = {\hat{x_{k}}}^{-} + K_{k} (z_{k} - H {\hat{x_{k}}}^{-}) [4] P_{k} = P_{k}^{-} - K_{k} H P_{k}^{-} 或 者 (I - K_{k} H) P_{k}^{-} [5]

[1]式

{\hat{x_{k}}}^{-} = A \hat{x_{k - 1}} + B u_{k - 1}

对于1式，我们将上一部分推断的公式写到这里

x_{k} = A x_{k - 1} + B u_{k - 1} + w_{i}

估计时，误差已经融入估计的计算，也就是误差与噪声是包含在模型本身中的，无法像上面一样直接提出来一个 $w_{i}$ ,因此我们将这个带有误差与噪声的预测值记为 ${\hat{x_{k}}}^{-}$ ,代表着状态量的先验预测值。这样我们就得出来第一个公式

\hat{x_{k}} = A \hat{x_{k - 1}} + B u_{k - 1}

[4]式

\hat{x_{k}} = {\hat{x_{k}}}^{-} + K_{k} (z_{k} - H {\hat{x_{k}}}^{-})

对于4式，我们可以看到出现了当前的最优估计值 $\hat{x_{k}}$ 以及先验预测值 ${\hat{x_{k}}}^{-}$ 和测量量 $z_{k}$ 。还有一个新的量 $K_{k}$ ,我们称之为卡尔曼增益。卡尔曼增益是卡尔曼滤波中最为灵魂最为重要的一个量。

不难看出，当 $K_{k} = 0$ 时(实际上是一个全0矩阵), $x$ 在 $k$ 时刻的最优估计值 $\hat{x_{k}}$ 就为 $k$ 时刻的先验预测值 ${\hat{x_{k}}}^{-}$ ,当 $K_{k} = H$ 时， $\hat{x_{k}}$ 就为测量值 $H^{-} z_{k}$ (这是由测量量 $z_{k} = H x_{k}$ 两边同乘 $H^{-}$ 得 $x_{k} = H^{-} z_{k}$ )。因此 $K_{k}$ 这个参数决定了测量量与预测量对于最优估计值的贡献程度。

利用上面中数据融合的思想可以知道，为了求出当前状态下最优的估计值，我们唯一不知道的量就是 $K_{k}$ 。而衡量最优的条件是什么呢？显然在概率统计中，方差越小，数据围绕真实值越集中。因此我们此时的目标就是寻找一个 $K_{k}$ 的计算式，这样的 $K_{k}$ 使得 $x_{k} - \hat{x_{k}}$ 的方差(多元数据使用协方差矩阵的迹)最小。

[3]式

K_{k} = P_{k}^{-} H^{T} / (H P_{k}^{-} H^{T} + R)

再讲这个式子之前，我们不妨给出一个真实状态量与后验最优估计量之间的误差的量化。

e_{k} = x_{k} - \hat{x_{k}}

同理，假设 $e_{k}$ 的误差也符合正态分布 $P (e_{k}) \sim N (0, P)$ 。与噪声的方差一样， $P$ 也是一个协方差矩阵，反映了 $e_{k}$ 的各个状态之间的关系以及误差。以匀速运动的物体为例,我们仅关注其位置为状态量。

P = [\begin{matrix} σ_{x x} & σ_{x y} \\ σ_{x y} & σ_{y y} \end{matrix}]

因此当前的目的就是寻求一个 $K_{k}$ ，使得 $e_{k}$ 的误差最小，也就是协方差矩阵的迹最小(迹上的元素描述的是各个变量相对于其真实值的误差，而其他元素描述的是各个变量之间的相关性)。我们依旧认为 $e_{k}$ 符合正态分布 $(0, P)$ ,由上述协方差矩阵的计算式可知

P = E (e_{k} e_{k}^{T})

带入 $e_{k}$

P = E ((x_{k} - \hat{x_{k}}) (x_{k} - \hat{x_{k}})^{T})

又已知 $\hat{x_{k}}$ 的表达式为

\hat{x_{k}} = {\hat{x_{k}}}^{-} + K_{k} (z_{k} - H {\hat{x_{k}}}^{-})

将其带入计算P的式子，这样 $K_{k}$ 就引入进来了。接着只需要应用一些线性代数的技巧化简即可。最后对得到的式子求导取极值可以得到 $K_{k}$ 的计算式

K_{k} = P_{k}^{-} H^{T} / (H P_{k}^{-} H^{T} + R)

其中

P_{k}^{-} = E (e_{k}^{-} {e_{k}^{-}}^{T})

[2]式

对于[2]式，是由 $k - 1$ 次的误差协方差矩阵推算出 $k$ 次的先验误差协方差矩阵的过程。使用了计算公式 $D (A X) = A D (X) A^{T}$ ,外加上噪声矩阵Q的测量噪声，可以得出[2]式

P_{k}^{-} = A P_{k - 1} A^{T} + Q

这是很不严谨的推理，算不上推导，推导请看Dr_Can的卡尔曼滤波讲解

[5]式

最后，我们要更新误差协方差矩阵，得出根据 $\hat{x_{k}}$ 计算出的最新的误差协方差矩阵。直接使用公式

P = E ((x_{k} - \hat{x_{k}}) (x_{k} - \hat{x_{k}})^{T})

带入求出表达式即可。最终得出

P_{k} = P_{k}^{-} - K_{k} H P_{k}^{-} 或 者 (I - K_{k} H) P_{k}^{-}

综上，[1][2]式用来根据模型进行当前状态的预测，[3][4][5]式用来对当前状态进行最优估计，这两部分不断循环执行，一个卡尔曼滤波器就开始工作了。

相对于简单的均值滤波，我们可以看到卡尔曼滤波并不太依赖于之前的一些数据与状态，本次的更新仅取决于上一次的数据，而且可以取得不错的效果。而均值的话非常依赖之前的样本数量，样本数量少的话会导致本次的估计不太准确

至此，卡尔曼滤波的黄金五式就解释完啦，知识水平有限，讲的很粗糙，有错误的话记得提醒我。

简单扩展Kalman滤波器

显然，kalman滤波器的预测的第一个式子

{\hat{x_{k}}}^{-} = A \hat{x_{k - 1}} + B u_{k - 1}

注定了卡尔曼滤波只能使用与线性系统，且系统的各种误差与噪声均符合期望为0的正态分布。那么，对于非线性系统，怎么才可以使用卡尔曼滤波呢？答案是线性化。

对于一个使用非线性模型描述的系统(即包含 $s i n x ， c o s x ， e^{x}$ )，我们可以通过泰勒展开的方式将其线性化。通过非线性函数在上一次最优估计 $\hat{x_{k - 1}}$ 处的一阶泰勒展开，将其化为多项式的形式。对于一些矩阵如A,B,H等，可以利用矩阵求导的知识将其化为对应的雅可比矩阵来求解。

也就是说扩展卡尔曼滤波与卡尔曼滤波的不同之处仅在于预测这一步中的模型是非线性系统线性化后的模型，其他的步骤与普通而卡尔曼滤波一模一样。

c++实现

下面给出一个用Eigen库以及模板写的卡尔曼滤波，其中模板参数1为状态量的维度，2为测量量的维度

kalman.h

 #ifndef _KALMAN_H_
#define _KALMAN_H_
#include <Eigen/Core>
#include <Eigen/Dense>
/*
 * Kalman滤波模板类，不考虑控制量B以及u
 * 模板dimx = 状态量维度，dimz = 测量量维度
 */
template <int dimx, int dimz>
class Kalman
{
    /*
     * 常用矩阵定义
     */
public:
    using Vec_x = Eigen::Matrix<double, dimx, 1>;
    using Vec_z = Eigen::Matrix<double, dimz, 1>;
    using Mat_xx = Eigen::Matrix<double, dimx, dimx>;
    using Mat_zx = Eigen::Matrix<double, dimz, dimx>;
    using Mat_xz = Eigen::Matrix<double, dimx, dimz>;
    using Mat_zz = Eigen::Matrix<double, dimz, dimz>;
 
private:
    Vec_x x;  // 状态量，dimx * 1 维度列向量
    Vec_z z;  // 测量量，dimz * 1 维度列向量
    Mat_xx A; // 状态转移矩阵， dimx * dimx 维度矩阵
    Mat_zx H; // 观测转移矩阵， dimz * dimx 维度矩阵
    Mat_xx P; // 状态量的误差协方差矩阵， dimx * dimx 维度矩阵
    Mat_xx Q; // 状态噪声协方差矩阵， dimx * dimx 维度矩阵
    Mat_zz R; // 测量噪声协方差矩阵， dimz * dimz 维度矩阵
    Mat_xz K; // 卡尔曼增益， dimx * dimz 维度矩阵
    Mat_xx I; // 单位矩阵， dimx * dimx 维度矩阵
 
public:
    Kalman();
    ~Kalman();
    void Init(Vec_x _x, Mat_xx _P, Mat_xx A_, Mat_zx H_, Mat_xx Q_, Mat_zz R_);
    Vec_x Predict();
    Vec_x Predict(Mat_xx B, Vec_x u);
    void Update(Vec_z _z);
    Vec_x Get_Best_x();
};
 
template class Kalman<2, 2>;
template class Kalman<4, 2>;
template class Kalman<4, 4>;
#endif

kalman.cpp

 #include "Kalman.h"
#include 
#include 
using namespace std;
template <int dimx, int dimz>
Kalman<dimx, dimz>::Kalman()
{
}
 
template <int dimx, int dimz>
Kalman<dimx, dimz>::~Kalman()
{
}
 
template <int dimx, int dimz>
void Kalman<dimx, dimz>::Init(Vec_x _x, Mat_xx _P, Mat_xx _A, Mat_zx _H, Mat_xx _Q, Mat_zz _R)
{
    this->x = _x;
    this->P = _P;
    this->A = _A;
    this->H = _H;
    this->Q = _Q;
    this->R = _R;
    this->I.setIdentity();
}
 
/*
 * 这里的Kalman::Vec_x是整一个返回类型，因为Vec_x是在类内定义的类型
 */
template <int dimx, int dimz>
typename Kalman<dimx, dimz>::Vec_x Kalman<dimx, dimz>::Predict()
{
    x = A * x;                     // 状态预测估计，不带输入
    P = A * P * A.transpose() + Q; // 预测估计误差协方差矩阵
    return x;
}
 
template <int dimx, int dimz>
typename Kalman::Vec_x Kalman::Predict(Mat_xx B, Vec_x u)
{
    x = A * x + B * u; // 状态预测估计，带输入
    P = A * P * A.transpose() + Q; // 预测估计误差协方差矩阵
    return x;
}
 
template <int dimx, int dimz>
void Kalman::Update(Vec_z _z)
{
    K = P * H.transpose() * (H * P * H.transpose() + R).inverse(); // 计算卡尔曼增益
    x = x + K * (_z - H * x);                                      // 更新最优状态值
    P = (I - K * H) * P;
}
 
template <int dimx, int dimz>
typename Kalman::Vec_x Kalman::Get_Best_x()
{
    return x;
}

相关阅读:
亚马逊英国站自动洒水器、自动浇花器UKCA认证检测标准
 GPT-SoVITS声音训练报错ZeroDivisionError: division by zero
四、vue-cli 介绍与使用
 MIT6.5830 Lab0-Go tutorial实验记录（四）
MySQL高级SQL语句
 基于Spring Boot+ MyBatis Plus 的多端后台管理框架（若依pro）
Java学习 --- JVM概述
 腾讯视频共享设备ip会不会出现错误
 wpa_supplicant介绍
 java项目-第148期ssm社区疫情防控管理信息系统-ssm毕业设计_计算机毕业设计
原文地址：https://www.cnblogs.com/CrescentWind/p/18132934

	#ifndef _KALMAN_H_
	#define _KALMAN_H_
	#include <Eigen/Core>
	#include <Eigen/Dense>
	/*
	* Kalman滤波模板类，不考虑控制量B以及u
	* 模板dimx = 状态量维度，dimz = 测量量维度
	*/
	template <int dimx, int dimz>
	class Kalman
	{
	/*
	* 常用矩阵定义
	*/
	public:
	using Vec_x = Eigen::Matrix<double, dimx, 1>;
	using Vec_z = Eigen::Matrix<double, dimz, 1>;
	using Mat_xx = Eigen::Matrix<double, dimx, dimx>;
	using Mat_zx = Eigen::Matrix<double, dimz, dimx>;
	using Mat_xz = Eigen::Matrix<double, dimx, dimz>;
	using Mat_zz = Eigen::Matrix<double, dimz, dimz>;

	private:
	Vec_x x; // 状态量，dimx * 1 维度列向量
	Vec_z z; // 测量量，dimz * 1 维度列向量
	Mat_xx A; // 状态转移矩阵， dimx * dimx 维度矩阵
	Mat_zx H; // 观测转移矩阵， dimz * dimx 维度矩阵
	Mat_xx P; // 状态量的误差协方差矩阵， dimx * dimx 维度矩阵
	Mat_xx Q; // 状态噪声协方差矩阵， dimx * dimx 维度矩阵
	Mat_zz R; // 测量噪声协方差矩阵， dimz * dimz 维度矩阵
	Mat_xz K; // 卡尔曼增益， dimx * dimz 维度矩阵
	Mat_xx I; // 单位矩阵， dimx * dimx 维度矩阵

	public:
	Kalman();
	~Kalman();
	void Init(Vec_x _x, Mat_xx _P, Mat_xx A_, Mat_zx H_, Mat_xx Q_, Mat_zz R_);
	Vec_x Predict();
	Vec_x Predict(Mat_xx B, Vec_x u);
	void Update(Vec_z _z);
	Vec_x Get_Best_x();
	};

	template class Kalman<2, 2>;
	template class Kalman<4, 2>;
	template class Kalman<4, 4>;
	#endif

	#include "Kalman.h"
	#include
	#include
	using namespace std;
	template <int dimx, int dimz>
	Kalman<dimx, dimz>::Kalman()
	{
	}

	template <int dimx, int dimz>
	Kalman<dimx, dimz>::~Kalman()
	{
	}

	template <int dimx, int dimz>
	void Kalman<dimx, dimz>::Init(Vec_x _x, Mat_xx _P, Mat_xx _A, Mat_zx _H, Mat_xx _Q, Mat_zz _R)
	{
	this->x = _x;
	this->P = _P;
	this->A = _A;
	this->H = _H;
	this->Q = _Q;
	this->R = _R;
	this->I.setIdentity();
	}

	/*
	* 这里的Kalman::Vec_x是整一个返回类型，因为Vec_x是在类内定义的类型
	*/
	template <int dimx, int dimz>
	typename Kalman<dimx, dimz>::Vec_x Kalman<dimx, dimz>::Predict()
	{
	x = A * x; // 状态预测估计，不带输入
	P = A * P * A.transpose() + Q; // 预测估计误差协方差矩阵
	return x;
	}

	template <int dimx, int dimz>
	typename Kalman::Vec_x Kalman::Predict(Mat_xx B, Vec_x u)
	{
	x = A * x + B * u; // 状态预测估计，带输入
	P = A * P * A.transpose() + Q; // 预测估计误差协方差矩阵
	return x;
	}

	template <int dimx, int dimz>
	void Kalman::Update(Vec_z _z)
	{
	K = P * H.transpose() * (H * P * H.transpose() + R).inverse(); // 计算卡尔曼增益
	x = x + K * (_z - H * x); // 更新最优状态值
	P = (I - K * H) * P;
	}

	template <int dimx, int dimz>
	typename Kalman::Vec_x Kalman::Get_Best_x()
	{
	return x;
	}