kalman算法
kalman滤波器论文《A New Approach to Linear Filtering and Prediction Problems》，有时间可以看一下。
kalman滤波的定义：
卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应用(如雷达、计算机视觉)中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。例如,对于雷达来说，人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。
我目前应用到的是对一个时间观测序列进行平滑。也就是对过去位置的估计。用于目标物体轨迹的跟踪。
因为最基础的kalman filter是线性滤波器，并且使用的参数有些提前的设置，所以kalman filter是有使用条件的。还有一些kalman filter的变种。我只用到了基础的就可以满足我的应用场景。
kalman的最中心的思想是，使用系统内部预测值，和实际观测值来计算出一个最优的值（这些值在已有的参数设置下是最少噪声的，因为难以做到完全去除。）
那么如何使用两个已经有的值去计算最优值，一般想到的就是这两个值得加权平均。这个权，表示哪个更加重要，哪个貌似噪音更少，就对最优结果更有贡献力。
下面是kalman的5个公式，可以分为两部分：
看不下去没关系，可以看完分析后再回头看这个就清楚很多
类似于隐马尔科夫，系统的预测值是由系统状态推导出来。因为kalman
的线性性，最简单的就是与矩阵内积。所以转换矩阵A其实是这个kalman filter的一个性质，在初始化时设置好的常数矩阵。所以使用A与上一次得到的最优结果（系统状态）相乘。
由于这一项无法人为对预测进行控制，所以后面的B系数表示人为对系统的控制，一般用不到。
这样得出来的预测值，是先验的预测值。也就是仅仅只根据系统的过去去预测。但是我们还有当前时间对应的观测值，虽然有噪声，但是也是有参考价值的。
所以使用了上文说的先验预测值和观测值的加权平均。这个权就是卡尔曼增益。
这个值怎么得到的呢?这个得结合kalman filter最优化得目标。一个序列如果目标是想要它越平滑，那么最终后验的预测值（加权平均后的）要越接近于没有噪声的真实值Xk（这个真实值我们无法知晓）。
编辑公式不方便，所以用文字描述。上文说的最接近，用公式表示就是后验预测的值，与真实值的后验估计误差协方差是最小的。就我自己理解，这个协方差的大小表示了两个随机变量的离散程度。如果两个随机变量的分布越接近的话，协方差越小。
协方差表示的是两个随机变量的总体的误差，这与只表示一个变量误差的方差不同。 如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值。 如果两个变量的变化趋势相反，即其中一个大于自身的期望值，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。
计算这个卡尔曼增益，就是带入协方差均值公式，对K求导获取的k值。
这个可以参考：链接
在计算出卡尔曼增益后对预测值进行更新，并且计算出最优值对应的协方差。为下次迭代做准备。对于第一次计算没有上一次的值，所以需要对需要上次值的变量进行初始化，包括第0个最优值，第0个最优值对应的协方差误差。
如果需要平滑的轨迹非常陡峭（噪声比较多），可以调整预测的噪声和测量的噪声来平滑。