卡尔曼跟踪

卡尔曼预测更新

predict：
$$x_i^{-}=A{x}_{i-1}+B{u}_{i-1}$$

correction:
$$x_i=x_i^{-}+k(y_i-H{x}_i^{-})$$

其中，$k$为kalman gain matrix; $y_i$观测值; $A$状态转移矩阵; $H$测量矩阵; B控制矩阵。

计算预测值、预测值和真实值之间的误差协防差矩阵

$$x_k^{-}=A{x}_{k-1}^{-}+B{u}_{k-1}$$

$$P_k^{-}=A{P}_{k-1}{A}^T+Q$$

$Q$系统误差

计算卡尔曼增益$k$，再得到估计值

$$K_k=P_k^{-}{H}^T(H{P}_K^{-}{H}^T+R{)}^{-1}$$

$$x_k=A{x}_k^{-}+K_k(Z_k-H{x}_k^{-})$$
$R$测量误差

计算估计值和真实值之间的误差协防差矩阵

$$P_k=(1-K_{k}H)P_k^{-}$$

卡尔曼转换关系

估计值==》预测值==》估计值==》$\dots$估计值==》预测值==》估计值==》$\dots$
核心在于计算卡尔慢增益$k$