卡尔曼公式理解

状态方程：
$x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}$
$p(w) \sim N(0, Q)$
测量方程：
$y_k = Hx_k + v_k$
$p(v) \sim N(0,R)$

预测值：
$\hat x_k^- = A\hat x_{k-1} + Bu_{k-1} \quad(1)$
预测值与真实值之间的误差协方差：
$P_k^- = AP_{k-1}A^T + Q \quad(2)$
由(1)(2)得到卡尔曼增益：
$K_k = P_k^-H^T(HP_k^-H^T + R)^{-1} \quad(3)$
由(1)(3)和测量值z就得到估计值：
$\hat x_k = \hat x_k^- + K_k(z_k - H\hat x_k^-)\quad(4)$
最后还要计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备：
$P_k=(I-K_kH)P_k^- \quad(5)$

所以只要给定初值$P_0$、$x_0$算法就能递归下去