卡尔曼滤波算法和应用
1. 状态转移矩阵 从当前状态到下一个状态的转换. 例子: 假设跟踪车辆形式 车辆的状态矩阵, 位置变量 $p_t$; 速度变量 $v_t$. $$ x_t=\begin{bmatrix} p_t \\ v_t \end{bmatrix} $$ 假设车辆有加速度变量 $u_t$; 这里 $\Delta t$ 是单位时间; 这里就可以从上一个单位时间的 $p_{t-1}v_{t-1}$ 推导出当前的 $p_tv_t$. $$ \begin{aligned}p_t=&p_{t-1}+v_{t-1}\times\Delta t+u_t\times\frac{\Delta t^2}{2}\\v_t=&v_{t-1}+u_t\times\Delta t\end{aligned} $$ 这里可以发现输出变量是输入变量的线性组合, 所以卡尔曼滤波器是线性滤波器, 只能描述状态与状态之间的线性关系. 由于是线性关系, 所以可以将公式写成矩阵的形式. $$ \left[\begin{array}{c}p_t \\v_t\end{array}\right]=\left[\begin{array}{cc}1 & \Delta t \\0 & 1\end{array}\right]\left[\begin{array}{c}p_{t-1} \\v_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{\Delta t^2}{2} \\\Delta t\end{array}\right] u_t $$ 提取公式中带时间参数矩阵之后, 就可以简化公式获得卡尔曼滤波器中的状态预测公式. $$ \begin{aligned}&F_t=\left[\begin{array}{cc}1 & \Delta t \\0 & 1\end{array}\right], B_t=\left[\begin{array}{c}\frac{\Delta t^2}{2} \\\Delta t\end{array}\right]\\&\hat{x}_t^{-}=F_t \hat{x}_{t-1}+B_t u_t\end{aligned} \quad\quad \text{(1)} $$ 这里的 $\hat{x}_t^{-}$ 表示, 这个值有 hat 符号是预测值, 并不是真实值; 右上角减号表示这个值是从 t-1 时刻的参数估计而来, 还需要 t 时刻的观测值进行修正. ...