首页 > 代码库 > 卡尔曼滤波器实例:进一步讨论自由落体运动
卡尔曼滤波器实例:进一步讨论自由落体运动
上一篇中,我们使用向量[0 0 0]T作为系统的初始状态,并且使用$\infty$作为初始状态误差的方差,也就是假定物体的初始位置、初始速度、加速度都是未知的,所有的状态信息都通过Radar的测量结果估计出来。但实际上,物体的加速度是预先知道的(假定为标准重力加速度),因此,我们可以简化上一篇中的多项式,以得到更好的状态估计。
将物体的位置设为状态$x_1$:
$x_1=s_0-v_0t-\frac{1}{2}gt^2$
$\frac{dx_1}{dt}=x_2=-v_0t-gt$
$\frac{dx_2}{dt}=-g$
上一篇中,我们暂时“忘记了”g的数值,因此$\frac{dx_2}{dt}$仍然是一个需要估计的系统状态,需要继续微分。而此处,$\frac{dx_2}{dt}$是已知的,因此我们到此就可以写状态方程了:
$\left[ \begin{matrix} \frac{dx_1}{dt} \\ \frac{dx_2}{dt} \end{matrix} \right] = \left[ \begin{matrix} 0&1 \\ 0&0 \end{matrix} \right] \left[ \begin{matrix} x_1 \\ x_2 \end{matrix} \right] + \left[ \begin{matrix} 0\\-1 \end{matrix} \right]g$
将这个矩阵和如下标准状态矩阵对比:
$\mathbf{\dot{x}}=\mathbf{Fx}+\mathbf{Gu}+\mathbf{w}$
可得:
$\mathbf{F}=\left[ \begin{matrix} 0&1 \\ 0&0 \end{matrix} \right]$
$\mathbf{G}=\left[ \begin{matrix} 0 \\ -1 \end{matrix} \right]$
$\mathbf{u}=g$
将连续时间状态模型转换到离散时间:
$\boldsymbol{\Phi}_k=\left[ \begin{matrix} 1&T_s\\0&1 \end{matrix} \right]$
$\mathbf{G}_k=\int_0^{T_s}\boldsymbol{\Phi}(\tau)\mathbf{G}d\tau=\int_0^{T_s}\left[ \begin{matrix} 1&\tau \\ 0&1 \end{matrix} \right] \left[ \begin{matrix} 0\\-1 \end{matrix} \right]d\tau=\left[ \begin{matrix} -\frac{T_s^2}{2} \\ -T_s \end{matrix} \right]$
卡尔曼滤波器实例:进一步讨论自由落体运动