卡尔曼滤波理解

1. 根据实际情况确定预测公式及预测矩阵。
2. 把上一时刻最优估计值作为预测公式的输入得到当前时刻的预测值（这里提到的值并非数值，而是状态矩阵）
3. 根据上一时刻最优估计值的协方差矩阵和预测矩阵再加上预测噪声(协方差，因为系统预测不是完全准确)计算当前时刻预测值的协方差矩阵
4. 已知当前时刻的测量值和测量值的协方差矩阵
5. 根据当前时刻的预测值，预测值协方差矩阵，测量值，测量值协方差矩阵，根据两者的高斯分布相乘公式求得当前时刻的最优估计值及其协方差矩阵
6. 当前时刻最优估计值的协方差矩阵是根据当前时刻的预测值协方差矩阵和测量值协方差矩阵求得

卡尔曼增益参考：https://blog.csdn.net/qq_40526216/article/details/90722109

公式推导参考：https://www.cnblogs.com/polaris-shi/p/9562874.html

代码参考：https://www.cnblogs.com/kuangxionghui/p/10382124.html

源码解读

1. 在for循环之前加入初始时的预测值，最优估计值，测量值，预测值的协方差矩阵，最优估计值的协方差矩阵
2. 求预测值如上所述
3. 根据当前时刻的预测值的协方差矩阵求卡尔曼增益
4. 根据卡尔曼增益及预测值，测量值求最优估计值及最优估计值的协方差矩阵