理解卡尔曼滤波器

核心要点

1. 假设系统噪声和测量噪声符合高斯分布，协方差代表不确定度。
2. 两个高斯分布的乘积依然是高斯分布，代表了两个分布的融合
3. 高斯分布的线性变换依然是高斯分布，因此经典Kalman filter 仅适用于线性系统模型
   
4. 所以kalman的核心思想即：综合通过前一时刻得到的预测信息（第一个高斯分布）和当期传感器测量的信息（第二个高斯分布），融合两个高斯分布得到当期系统状态的最佳估计。
   （1） 根据上一时刻系统最佳估计和系统状态方程预测当前时刻的系统状态，得到一个预测值的高斯分布prediction;
   （2） 根据传感器测量值及其噪声，得到测量值的高斯分布measurement
   （3）N1与N2相乘，即得到新的高斯分布，即当前时刻的最佳估计，包括状态估计和不确定度估计。由于传感器测量值与系统状态的尺度不一定相同，所以引入H矩阵变换到同一尺度。

公式理解

分为两个阶段，第一个阶段，状态预测阶段，通过上一个时刻最佳预测和系统方程，得到当前时刻的估计，即得到第一个高斯分布。
阶段1.状态预测<