滤波笔记二：无迹卡尔曼滤波UKF CTRV&&CTRA模型

Reference:

1. 《Empirical Evaluation of Vehicular Models for Ego Motion Estimation》

无人驾驶的物体跟踪运动模型有很多种，包括：

恒定速度模型：CV----Constant Velocity
恒定加速度模型：CA----Constant Acceleration
恒定转弯率和速度模型：CTRV----Constant Turn Rate and Velocity
恒定转弯率和加速度模型：CTRA----Constant Turn Rate and Acceleration
恒定转向角和速度模型：CSAV----Constant Steering Angle and Velocity
常曲率和加速度模型：CCA----Constant Curvature and Acceleration

1. 一次运动模型

一次运动模型(每一段都是直线)也称 线性运动模型，有：

1. 恒定速度模型/等速模型(Constant Velocity, CV)
2. 恒定加速度模型/匀加速模型(Constant Acceleration, CA)

这些线性运动模型假定目标是直线运动的，并不考虑物体的转弯。

1.1 等速模型

等速模型 是用于物体跟踪的最基本的运动模型之一：

• 状态空间可以表示为：
  $$\vec{x}(t)={\left(\begin{array}{llll}x& y& v_x& v_y\end{array}\right)}^T$$转移函数为：
  $$\vec{x}(t+\Delta t)=\left(\begin{array}{c}x(t)+\Delta t{v}_x\\ y(t)+\Delta t{v}_y\\ v_x\\ v_y\end{array}\right)$$
• 状态变量表示方式 II：
  $$\begin{array}{l}{\mathrm{CV}}_{\mathbf{X}}={\left(\begin{array}{llll}x& y& \vartheta & v\end{array}\right)}^{\mathrm{T}}\end{array}$$四个状态变量依次为横坐标，纵坐标，与 x 轴夹角（逆时针为正），线速度。
  状态转移方程：
  $${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\left(\begin{array}{c}v\cos (\vartheta )T_k\\ v\sin (\vartheta )T_k\\ 0\\ 0\end{array}\right)$$
• 局限：
  假设速度是常量，我们实际上简化了车辆实际移动的形式，因为大多数车辆道路是有拐弯的，但速度是常量的模型会无法正确预测拐弯车辆。

1.2 匀加速模型

• 状态变量：
  $$\begin{array}{l}{\mathrm{CA}}_{\mathrm{X}}={\left(\begin{array}{lllll}x& y& \vartheta & v& a\end{array}\right)}^{\mathrm{T}}\end{array}$$四个状态变量依次为横坐标，纵坐标，与 x 轴夹角（逆时针为正），线速度，加速度。
  状态转移方程：
  $${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\left(\begin{array}{c}\left(v_k{T}_k+\frac{a}{2}{T}_k^2\right)\cos (\vartheta )\\ \left(v_k{T}_k+\frac{a}{2}{T}_k^2\right)\sin (\vartheta )\\ 0\\ a{T}_k\\ 0\end{array}\right)$$

观察 $x$ 和 $y$ 的状态转移，$v_k{T}_k+\frac{a}{2}{T}_k^2$ 为定值，很明显是一条直线。

2. 二次运动模型

1. 恒定转(弯)率和速度模型(Constant Turn Rate and Velocity，CTRV)
2. 恒定转(弯)率和加速度模型(Constant Turn Rate and Acceleration，CTRA)

CTRA 多用于机载追踪系统，这些二次运动模型大多假定速度 v 和偏航角速度 $\omega$ 没有关系。因此，在这类运动模型中，由于偏航角速度 $\omega$ 测量的扰动，即使车辆没有移动，运动模型下的角速度也会发生非常细微的变化。

为了解决这个问题，速度 v 和 偏航角速度 $\omega$ 的关联可以通过设定转向角 $\Phi$ 恒定来建立，这样就引出了 恒定转向角和速度模型(Constant Steering Angle and Velocity，CSAV)，另外，速度可以被假定为线性变化的，进而引出了 常曲率和加速度模型(Constant Curvature and Acceleration，CCA)。

2.1 恒定转弯率和速度模型(CTRV)

CTRV 是 CV 的一般形式，当角速度 $\omega =0$ 时，就是 CV。CTRV 模型假设对象沿直线前进，同时还能以固定的转弯速率和恒定的速度移动。

• 状态变量：
  $$x=\left[\begin{array}{c}{p}_x\\ p_y\\ v\\ \psi \\ \dot{\psi }\end{array}\right]$$
  五个状态变量依次为：横坐标—>纵坐标—>速度大小—>偏航角—>角速度
  一个恒定的转弯率，由公式可看推得，速度和角速度的变化率均为 0。

• CTRV积分
  那么怎么计算时间 k 到 k+1 呢？
  假设离散的时间步骤 k 和持续的时间值 $t_k$ 相关，离散的时间步骤 k+1 和持续的时间值 $t_{k+1}$ 相关，$t_{k+1}$ 和 $t_k$ 之间的时间差叫做 ${\Delta }_t$。
  首先，我们假设速度 v 是常量，这样，我们可以将时间 k 时在积分内的 $v_t$，移动到积分前。然后，将 sin 和 cos 表示为 t 的函数，待积分的最终公式如下：
  对上述公式积分，得到的结果为：
  
• CTRV 过程噪声矢量
  上述所说的，都是过程模型的确定部分，当然，也需要考虑过程模型的随机部分。这就涉及到过程噪声 $v_k$ 。
  
  我们使用一个二维噪声向量 $v_k$ 来描述过程模型。不确定性 ${\nu }_k$ 由两个独立的变量噪声过程组成----第一个是纵向加速噪音 $v_{a,k}$，它影响车辆的纵向速度，并且在每一个时间阶段改变它的值。纵向加速是随机分布的白噪声，均值为0，方差为 ${\sigma }_a^2$ 的平方。（不过纵向加速度好像没什么卵用----更新，好吧，是有用的，加在了速度上）
  在 YAW 方向上，噪声分布 ${\nu }_{\ddot{\psi },k}$ 与 ${\nu }_{a,k}$ 相同，均为高斯分布：
  
  过程模型添加噪声后，公式如下：
  $$x_{k+1}=x_k+\left[\begin{array}{c}\frac{v_k}{{\psi }_k}\left(\sin \left({\psi }_k+{\dot{\psi }}_k\Delta t\right)-\sin \left({\psi }_k\right)\right)\\ \frac{v_k}{{\psi }_k}\left(-\cos \left({\psi }_k+{\dot{\psi }}_k\Delta t\right)+\cos \left({\psi }_k\right)\right)\\ 0\\ {\dot{\psi }}_k\Delta t\\ 0\end{array}\right]+\left[\begin{array}{l}a\\ b\\ c\\ d\\ e\end{array}\right]$$
  假设偏航角加速度 ${\nu }_{\ddot{\psi }}$ 在 k 与 k+1 之间是恒定的，它会随着时间的增加线性增加偏航角速度。因此，偏航角加速度的对偏航角速度的影响等于角加速度乘以 ${\Delta }_t$
  $$x_{k+1}=x_k+\left[\begin{array}{c}\frac{v_k}{{\dot{\psi }}_k}\left(\sin \left({\psi }_k+{\dot{\psi }}_k\Delta t\right)-\sin \left({\psi }_k\right)\right)\\ \frac{v_k}{{\psi }_k}\left(-\cos \left({\psi }_k+{\dot{\psi }}_k\Delta t\right)+\cos \left({\psi }_k\right)\right)\\ 0\\ \dot{{\psi }_k}\Delta t\\ 0\end{array}\right]+\left[\begin{array}{c}\frac{1}{2}(\Delta t{)}^{2}cos({\psi }_k)\cdot {\nu }_{a,k}\\ \frac{1}{2}(\Delta t{)}^{2}sin({\psi }_k)\cdot {\nu }_{a,k}\\ \Delta t\cdot {\nu }_{a,k}\\ \frac{1}{2}(\Delta t{)}^2\cdot {\nu }_{\ddot{\psi },k}\\ \Delta t\cdot {\nu }_{\ddot{\psi },k}\end{array}\right]$$

3. UKF

EKF 利用线性方法来解决非线性问题是一种思路，但这种方法的不足之处主要有两点：
1. 雅克比矩阵的计算量比较大
2. 高度非线性的时候误差可能比较大

3.1 UKF 和 EKF 的逻辑不同点

我们假设车辆的运动符合高斯分布，比如当前在 10m 的位置，速度为 1m/s，那么车下一时刻的位置理论上应该服从均值为11的高斯分布。那么从这种逻辑出发，对于复杂模型，如果我们知道了 k-1 时刻的高斯分布，对 k 时刻，
一种方法是找一个线性变换对原来的高斯分布进行映射，得到一个新的高斯分布，这就是 EKF。
另一种逻辑是，既然下一时刻也服从高斯分布，去线性变化是否不太准确，可以不去线性化而是去找一个同下一时刻的真实分布相接近的高斯分布吗，这就是UKF。

3.1 UKF 顺序

预测：产生 Sigma 点---->预测 Sigma 点---->预测均值和方差
更新：预测测量---->更新状态

3.1.1 产生 sigma 值

在预测步骤开始，并希望产生 sigma 点。我们有从上一次迭代中获得的后验状态 $x_{k|k}$ 和后验协方差矩阵 $P_{k|k}$----它们呈现了我们当前状态的分布，并且对于这个分布，我们想要生成 sigma 点。sigma 点的个数取决于状态维度。在这里是 CTRV 模型的状态向量，所以我们的状态维度是 $n_x=5$。我们要选择 $2n_x+1$ 个 sigma 点。第一个点是状态的均值，然后每个状态维度增加两个点，将在不同方向上延伸。在该场景中，会有11个 sigma 点。为了让现在的事情看起来更简单，我们只考虑状态向量的两个维度----位置 $P_x$ 和 $P_y$。

这样更易于可视化。现在我们的状态维度是2，我们可以仅寻找五个 sigma 点。现在，我们准备计算五个 sigma 点并将它们储存在矩阵 ${\mathcal{X}}_{k\mid k}$ 中，这个矩阵的每一列都代表一个 sigma 点。计算 sigma 点矩阵的公式如下：
$$X_{k\mid k}=\left[x_{k\mid k}{x}_{k\mid k}+\sqrt{\left(\lambda +n_x\right)P_{k]k}}{x}_{k\mid k}-\sqrt{\left(\lambda +n_x\right)P_{k\mid k}}\right]$$

这个公式看起来有点丑，但事实上很容易来运用。

第一件需要知道的事是什么是 $\lambda$，这是一个设计参数。你可以选择和箭头椭圆相关的位置来放置你的 sigma 点。有些人说在 $\lambda =3-n_x$ 的时候可以得到不错的结果(注意后面是增广矩阵，所以是 $\lambda =3-n_a$)。(需要注意的是，实际测试中发现，这个值使用 $\lambda =3-n_a$，在参数个数较多时导致 $\lambda$ 为负数，造成该模型并不相信中心的 sigma 点，就很奇怪。进而造成在多帧之后的协方差预测中，预测出的协方差 $P$ 非正定，这时求取 square root 的 P.llt().matrixL() 在接下来的帧或多帧时计算不了平方根，这时 $P$ 的值会异常且爆炸的大。因此，这里的值使用 $\lambda =3-n_a$ 感觉并不是一个很好的参考)

另一件你可能想要知道的事是，什么是矩阵的平方根？(图中一个星表示一个 sigma 点)

更准确的说，我们是在寻找一个矩阵 $A$，满足与 $A$ 的转置相乘，即 $A^{T}A=P$。现在将真实值带入平均状态和协方差矩阵中，求解平方根，得到矩阵 $A$ 的值。

// calculate square root of P
MatrixXd A = P.llt().matrixL();

此时已经得到了公式中所有需要的值，所以公式该如何解读呢？
矩阵的第一列告诉了我们第一个 sigma 点是什么----这个比较简单，因为第一个 sigma 点就是状态估计的均值。第二项包含两个 sigma 点，在平方根矩阵中，有两个向量。第一个指向一个方向，第二个指向另一个方向(2*2矩阵中两个2*1)，如下图所示：
如果 $\lambda$ 的较大，sigma 点会离平均状态更远；如果值较小，点会离平均状态更近。

第三项与第二项相似，但方向相反（可以看成均匀的从不同方向找 $\sigma$ 点）。在这个例子中，令 $\lambda +n_x=3$，sigma 点矩阵可得。

3.1.2 UKF 增广(augmentation)

现在你知道如何用带 sigma 点的后验状态估计表示不确定性，并且能将 sigma 点放入过程函数内。但是，过程函数还要考虑过程噪声向量 ${\nu }_k$ ，这也会有一个非线性影响。
$$x_{k|k}\&\&P_{k|k}\Rightarrow x_{k+1}=f(x_k,{\nu }_k)$$

幸运的是，UKF 提供了一个非常简单的方式来处理非线性过程噪声影响。让我们再来看看过程噪声：
$${\nu }_k=\left[\begin{array}{l}{\nu }_{a,k}\\ {\nu }_{{\ddot{\psi }}_{,k}}\end{array}\right]$$

下面是完整的过程模型，考虑过程噪声向量：
$$x_{k+1}=x_k+\left[\begin{array}{c}\frac{v_k}{{\dot{\psi }}_k}\left(\sin \left({\psi }_k+{\dot{\psi }}_k\Delta t\right)-\sin \left({\psi }_k\right)\right)\\ \frac{v_k}{{\psi }_k}\left(-\cos \left({\psi }_k+{\dot{\psi }}_k\Delta t\right)+\cos \left({\psi }_k\right)\right)\\ 0\\ \dot{{\psi }_k}\Delta t\\ 0\end{array}\right]+\left[\begin{array}{c}\frac{1}{2}(\Delta t{)}^{2}cos({\psi }_k)\cdot {\nu }_{a,k}\\ \frac{1}{2}(\Delta t{)}^{2}sin({\psi }_k)\cdot {\nu }_{a,k}\\ \Delta t\cdot {\nu }_{a,k}\\ \frac{1}{2}(\Delta t{)}^2\cdot {\nu }_{\ddot{\psi },k}\\ \Delta t\cdot {\nu }_{\ddot{\psi },k}\end{array}\right]$$

这里的最后一部分在 KF 中也被称为过程向量 ${\nu }_k$。这里有一个很大的不同因为在 EKF 中需要计算一个大的噪声协方差矩阵 $Q$ Q = E { ν k ⋅ ν k T } Q=E\{\nu_k \cdot \nu_k^T \} Q=E{νk​⋅νkT​}。这两个 ${\nu }_k$ 在文献中都会被提起，需要很注意，作者提到的是哪个。

有时候作者说的是下面的那个向量，向量的每一排指的是在指定 $K$ 和 $K+1$ 状态的过程噪声所造成的影响。因此它有着和状态向量相同的维度，且取决于 ${\Delta }_t$。这种大多数情况发生在当你阅读标准线性卡尔曼时。

而更多数的情况中，作者的意思是更上面那种向量。这种更为简易，因为这种向量只列出了单独的独立噪声过程。这个向量并没有说明噪声过程对状态向量的影响，它也不依赖于 ${\Delta }_t$。通常情况下，这是在作者写 UKF 时所要表达的。因此，在这里只要提到了 ${\nu }_k$，说的都是更上面的那种情况。这也是一个好事，因为在这个过程噪声里的协方差矩阵 $Q$ 更简单。

为了写出这个矩阵，你只需要知道这些噪声过程的随机特性，我们已经定义过了：
$$\begin{array}{l}{\nu }_{a,k}\sim N\left(0,{\sigma }_a^2\right)\\ {\nu }_{\psi ,k}\sim N\left(0,{\sigma }_{\dot{\psi }}^2\right)\end{array}$$

$Q$ 由以下矩阵给出，这个矩阵包含噪声过程方差：
$$Q=E\left\{{\nu }_k\cdot {\nu }_k^T\right\}=\left[\begin{array}{cc}{\sigma }_a^2& 0\\ 0& {\sigma }_{\ddot{\psi }}^2\end{array}\right]$$

该矩阵内为 0 的部分是因为纵向加速度噪声和偏航角加速度噪声被认为是独立的。

现在要展示的是如何使用 sigma 点来表示（系统噪声）协方差矩阵 $Q$，这个解被称为 augmentation。

现在来搭建增广状态，我们将噪声向量添加进状态向量内，将这个状态称为 $x_a$，这里和后面的 $a$ 代表增广：
$$x_{a,k}=\left[\begin{array}{c}{p}_x\\ p_y\\ v\\ \psi \\ \dot{\psi }\\ {\nu }_a\\ {\nu }_{\psi }\end{array}\right]$$
增广状态维度在我们的情况下是 7，这也意味着我们现在需要 15 个 sigma 点了。额外的四个 sigma 点将被用来表示由过程噪声产生的不确定性：

$$P_{a,k\mid k}=\left[\begin{array}{cc}{P}_{k\mid k}& 0\\ 0& Q\end{array}\right]$$

我们也搭建增广协方差矩阵 $P_a$，这个矩阵有七行七列，只需将矩阵 $Q$ 作为右下角块添加，然后其他位置填充0即可。最后，生成15个 sigma 点的方式与之前相同。只需要使用增广元素作为输入。

3.1.3 sigma 点预测

我们现在有的是下图为蓝色的增广 sigma 点，对于预测步骤，我们简单的插入所有的 sigma 点到过程模型：
需要注意的事是，过程模型的输入是一个七维的增广向量：五个状态维度+两个噪声维度。输出是一个五维向量，因为这是过程模型返回的，意味着被预测的 sigma 点有 5 行和 15 列。输入是 15 个 sigma 点，输出也应该是 15 个 sigma 点。

过程模型如下：如公式所见，增广向量的作用就是将噪声量添加进预测状态。使用 $f$，添加了噪声量，还将状态变为了下一状态的预测。
如果 ${\dot{\psi }}_k$ 不为0：
$$\text{ State }=x_{k+1}=x_k+\left[\begin{array}{c}\frac{v_k}{{\dot{\psi }}_k}\left(\sin \left({\psi }_k+{\dot{\psi }}_k\Delta t\right)-\sin \left({\psi }_k\right)\right)\\ \frac{v_k}{{\dot{\psi }}_k}\left(-\cos \left({\psi }_k+{\dot{\psi }}_k\Delta t\right)+\cos \left({\psi }_k\right)\right)\\ 0\\ {\dot{\psi }}_k\Delta t\\ 0\end{array}\right]+\left[\begin{array}{c}\frac{1}{2}(\Delta t{)}^2\cos \left({\psi }_k\right){\nu }_{a,k}\\ \frac{1}{2}(\Delta t{)}^2\sin \left({\psi }_k\right){\nu }_{a,k}\\ \Delta t\cdot {\nu }_{a,k}\\ \frac{1}{2}(\Delta t{)}^2{\nu }_{\ddot{\psi },k}\\ \Delta t\cdot {\nu }_{\ddot{\psi },k}\end{array}\right]$$
如果 ${\dot{\psi }}_k$ 为0：
$$\text{ State }=x_{k+1}=x_k+\left[\begin{array}{c}{v}_k\cos \left({\psi }_k\right)\Delta t\\ v_k\sin \left({\psi }_k\right)\Delta t\\ 0\\ {\dot{\psi }}_k\Delta t\\ 0\end{array}\right]+\left[\begin{array}{c}\frac{1}{2}(\Delta t{)}^2\cos \left({\psi }_k\right){\nu }_{a,k}\\ \frac{1}{2}(\Delta t{)}^2\sin \left({\psi }_k\right){\nu }_{a,k}\\ \Delta t\cdot {\nu }_{a,k}\\ \frac{1}{2}(\Delta t{)}^2{\nu }_{\ddot{\psi },k}\\ \Delta t\cdot {\nu }_{\ddot{\psi },k}\end{array}\right]$$

3.1.4 预测平均值和协方差

预测平均值和协方差的方式与常用的相同：
然而，在这里多了一个权重 $w_i$ 需要被考虑。这里的权重与 $\lambda$ 有关，我们之前使用权重来设置 sigma 点应该离平均值有多远。现在是做的反向操作，因此要把先前乘了的 $\lambda$ 加权重平衡回来。

这时就得到了预测状态的均值和协方差。

3.1.5 测量预测

现在，我们需要转换预测状态到测量状态。定义这个转换的函数是测量模型。我们需要通过非线性函数变换分布，这样我们就可以在中心变换方法上应用与之前在状态预测中所做的完全相同的方法。不过，我们可以走两个捷径，让它更简单一些。

我们在预测步骤做的第一件事是产生 sigma 点，我们可以使用预测平均值和协方差矩阵做相同的事。然而，一个流行的捷径是重用我们从预测步骤中已经得到的信号点。因此在这一个步骤中，我们可以省略生成 sigma 点-----我们需要增广是因为过程噪声在该状态下有一个非线性影响，但是测量噪音具备单纯的累加效果。因此我们只需要做一件事情，就是把已经有的 sigma 点转换为测量空间，并使用这些点来计算预测测量值的均值 $z$ 和协方差矩阵 $S$。
同样地，转换后的测量 sigma 点存储为矩阵中的列，记为 $\mathcal{Z}$。这里有径向距离 $\rho$、角度 $\phi$ 以及径向速度 $\dot{\rho }$ ----- 因此这是一个三维空间。这表示本例中 包括 sigma 点的矩阵有 3 行 15 列。（测量空间与状态空间不太一样是因为咱们只测量了这三个量，而CTRV中的五个量部分是推出来的）至于测量噪音 $\omega$，在这里输入 0 就可以，因为我们后面会处理测量噪音(对更新无影响，本身就是线性的)。现在我们有了 sigma 点和测量空间，需要计算得到的预测测量值的均值和协方差。这和之前学过的也很相似，根据这些方程计算即可。这里唯一多出来的是，我们添加了测量协方差噪声，这是测量噪音的处理方式。记住，这里可以使用这个来替代增广。因为本例中的测量噪声，对测量不具备非线性效应，它是单纯的累积性的。

测量模型：
$$\begin{array}{l}{z}_{k+1\mid k}=h\left(x_{k+1}\right)+w_{k+1}\\ \rho =\sqrt{p_x^2+p_y^2}\\ \phi =\arctan \left(\frac{p_y}{p_x}\right)\\ \dot{\rho }=\frac{p_x\cos (\psi )v+p_y\sin (\psi )v}{\sqrt{p_x^2+p_y^2}}\end{array}$$

预测的测量平均值：
$$z_{k+1\mid k}=\sum _{i=1}^{n_{\sigma }}{w}_i{Z}_{k+1\mid k,i}$$

预测的协方差：
$$\begin{array}{l}{S}_{k+1\mid k}={\sum }_{i=1}^{n_{\sigma }}{w}_i\left(Z_{k+1\mid k,i}-z_{k+1\mid k}\right){\left(Z_{k+1\mid k,i}-z_{k+1\mid k}\right)}^T+R\\ R=E\left(w_k\cdot w_k^T\right)=\left[\begin{array}{ccc}{\sigma }_{\rho }^2& 0& 0\\ 0& {\sigma }_{\phi }^2& 0\\ 0& 0& {\sigma }_{\dot{\rho }}^2\end{array}\right]\end{array}$$

3.1.6 更新

我们现在有的东西：

1. 预测状态平均值和协方差
2. 预测测量平均值和协方差

但是我们还需要一个额外的东西，这就是我们在时间阶 $k+1$ 接收到的真实测量值 $z_{k+1}$。这实际上是第一时间我们真正需要知道的测量值。

更新计算与过程链类似，它产生更新状态平均值和更新状态协方差矩阵。这些公式与标准卡尔曼滤波中的完全相同。唯一的区别是，如何来计算卡尔曼增益 $K$。这里需要用到在状态空间中的预测 sigma 点和在测量空间中的预测 sigma 点之间的互相关矩阵 $T$（其实就是说的协方差矩阵？ ）

3.1.7 总结

UKF 作为无人驾驶的传感器融合工具有三个最重要的功能：

1. 在 UKF 中，你可以输入噪音测量数据，并获得周围动态对象的位置和速度的平滑估算，不会引发延时。
2. 即使使用的传感器无法直接观察，依旧可以获得其他车辆的方向和角速度的估算
3. UKF 还能给出结果的准确度，因为它能够提供每个估计一个协方差矩阵。如果 UKF 已经执行了一致性检查，你就能知道协方差矩阵是现实的。估算结果的不确定性对无人驾驶车非常重要。因为如果在某个时刻，前方车辆的位置非常不确定，你最好多拉开点车距。

所有的卡尔曼滤波都有相同的三个步骤：

1. Initialization
2. Prediction
3. Update

一个标准的卡尔曼滤波只能处理线性方程。EKF 和 UKF 允许来使用非线性方程。EKF 和 UKF 的区别是如何处理非线性方程，但是基本点都是一样的：初始化+预测+更新。

点击跳转：
滤波笔记一：卡尔曼滤波.
滤波笔记二：无迹卡尔曼滤波 CTRV&&CTRA模型.
滤波笔记四：扩展卡尔曼滤波.