KF及其优化
使用卡尔曼滤波器或其他观测器，都是为了达到消除噪声从而获取更好的位置估计的目的，这样计算出来的差分和二阶差分等相对前一种方法会更加可靠。
若采用匀速模型，则在状态变量中加入速度，update步应该分为两部分：
- 使用于更新状态变量的状态转移矩阵中对应速度的位置的值为Δ t \Delta t Δt，即 两帧之间的时间差
- 增加一个新的状态转移矩阵，在对应的位置上设置的值为Δ T \Delta T ΔT，这是 需要超前的预测时间。
第一步得到的状态变量会一直继续使用下去，这将作为 当前位置和速度的最优估计并不断保持更新，和前面朴素方法介绍的多帧平均的作用一致；而第二部分计算出来的值，是用来提供超前位置的，每次都根据第一步的结果重新计算。
对于协方差矩阵的设置，我们从两个 极端入手考虑：若将预测值的速度协方差设定的非常大，则我们几乎只相信测量值，即两帧之间的差分值，那么这就不是一个匀速直线运动了，得到的融合结果可能会呈现 锯齿状，需要稍微进行平滑以获得连贯一些的控制数据；将与预测值的协方差设定的很小的时候，那么我们会更相信预测值，认为其是一个近似的匀速直线运动。
当然，我们也可以把加速度加入状态变量中，用二阶差分来计算加速度，同样可以采用前面平均速度的方法来平滑加速度。不过二阶差分本身就会带来较大的噪声，从观测器的状态反馈权衡的角度考虑，相当于给了一个很大的增益，其 带宽会扩展但是容易被高频噪声所干扰。
上述的方法本质上还是基于匀加速或匀速这种较好处理的模型，只不过采用观测器尽量减少噪声，还有一些稍微复杂一些的模型采用了不同的假设和约束，使得预测的效果更好。我们在此简要列出几个供读者进一步参考。
首先很容易想到的就是其他constant模型：既然有平动那肯定也有转动的匀速模型和匀加速模型吧？这就是Constant Turn和Constant Angular acceleration。那能不能把转动和平动结合起来？肯定能啊，这就有了CTRV、CSAV、CCA，这些模型在自动驾驶的运动学建模中都用得非常广泛。
再比如将加速度或速度建模为服从一定分布的思想也是很巧妙的。singer模型假设目标的加速度符合一个[ − a m a x , a m a x ] [-a_{max},a_{max}][−a m a x ,a m a x ]的均值分布，其服从一阶随机微分方程，可以用 目标机动时间常数衡量一段时间内加速度的相关性。哈工威的视觉有一场圆桌介绍了他们的singer模型，参考：哈工大威海hero：神经网络优化与运动预测算法分享。而 当前模型（current model）考虑了加速度的均值，不像前者将加速度均值设为零，其加速度是一个条件分布，将根据一段时间内观测到的加速度均值进行计算更新，因此又被成为均值自适应加速度模型。
最后就是不同模型的集大成者，可以看作机器学习中的 集成方法—— IMM（Interact multi model），它综合了多个单一模型（就是上述的那些），分为四步进行运动预测和更新，相信已经学习过卡尔曼滤波的你应该大概能猜想到他的流程，IMM最大的不同就是增加了第一步，让模型进行交互计算相互转移的概率：
- 模型交互，计算目标运动模型从一种模型i i i切换到另一种模型j j j的概率μ i j μ_{ij}μi j （一会看了后面三步会更加清晰），然后为所有模型两两计算转移概率并乘以每个模型的输出得到新一轮的初始状态。
- 以第1步得到的初始状态（1个）作为 每个单一模型的输入，然后用各自的模型进行预测和量测更新。这一步和KF完全相同，而且实践中大部分单一模型的状态转移和融合都是使用EKF或UKF。
- 在第2步更新完之后，利用在更新时每个模型 预测值和 测量值的差值，计算每个单一模型的似然函数值进而得到该模型的置信度。也就是说，单一模型的预测值和真值（其实是测量值）差距越大，说明这个模型和实际情况相差的越远。
- 根据第3步得到的每个模型的置信度（权重），对所有模型的结果进行加权融合得到输出。
IMM实际上就像一个超大杯的KF，里面有许多小KF，小KF的输出又再次在大KF中进行融合，应该算是一种stacking ensemble，每个小模型的输出再投入一个模型中进行集成。比较巧妙的设计就是IMM的第一步，它没有直接用上一轮迭代中的输出值作为新一轮迭代的初始状态而是添加了一个 模型转移概率，使得不同单一模型之间的交互更充分。归根到底，IMM也是一种贝叶斯方法，利用先验信息和后验估计来获取极大似然估计。若希望看到详细的数学推导，请自行查找相关的论文。
Original: https://blog.csdn.net/NeoZng/article/details/123488631
Author: HNU跃鹿战队
Title: 了解CV和RoboMaster视觉组（五）运动建模与预测
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/598280/
转载文章受原作者版权保护。转载请注明原作者出处！