原理

假设小车第k时刻状态为

$$ \boldsymbol{x}_k=\left[ \begin{array}{c} p_k\\ v_k\\ \end{array} \right] $$

$p_k$为k时刻的位置，$v_k$为k时刻的速度。
如果没有给小车加速减速，预测小车的下一状态为

$$ \begin{cases} p_k=p_{k-1}+\varDelta t\cdot v_{k-1}\\ v_k=v_{k-1}\\ \end{cases} $$

换句话说，也即下列公式

$$ \boldsymbol{\hat{x}}_k=\left[ \begin{matrix} 1& \varDelta t\\ 0& 1\\ \end{matrix} \right] \boldsymbol{\hat{x}}_{k-1}=F_k\boldsymbol{\hat{x}}_{k-1} $$

$F_k$ 也称状态转移矩阵,

$$ \boldsymbol{\hat{x}}_k=\left[ \begin{matrix} 1& \varDelta t\\ 0& 1\\ \end{matrix} \right] \boldsymbol{\hat{x}}_{k-1}=F_k\boldsymbol{\hat{x}}_{k-1} $$

基本知识

状态外推方程（预测方程）
估计不准确度外推方程
卡尔曼增益更新方程
状态更新方程
估计不确定度更新方程