状态方程离散化

由于使用卡尔曼滤波需要离散化的系统状态方程，即系统方程和量测方程都需要是离散型的，而在实际的物理系统中我们得到的是连续系统的状态方程，动力学特性用连续微分方程描述，所以对它们需要进行离散化处理。在这里我就不放离散化的具体推导，毕竟很多人也只是用这个公式，而不必去真正了解过程，能用就可以了。如果有大佬想了解具体推导过程，可以去看看秦永元等主编的《卡尔曼滤波与组合导航原理》，里面介绍的还是很详细的。

描述物理系统动力学特性的系统方程为：

其中系统的驱动源 为白噪声过程，即

为 的方差强度阵。

根据线性系统理论，系统方程的离散化形式为：

其中一步转移阵

当滤波周期 较短时， 可近似看作常阵

此时

即

一般计算到二阶即可。

令

则离散后的方程形式为：

其中

 

当滤波周期k 较短时， 在该周期内变化不大，则

令

计算一步转移阵和等效离散系统噪声方差阵如下图所示：

 

若滤波周期较长，采样频率较高，且系统矩阵 随时间变化较剧烈，而 在一个滤波周期内能得到N个采样值，则计算一步转移阵和等效离散系统噪声方差阵时可利用这些采样值来简化算法。

设滤波周期为 ，在 内每隔 就能得到系统阵的采样值：

一步转移阵 和等效离散系统噪声方差阵 可按如下方法简化计算：

考察上式可看出，计算一步转移阵 所需矩阵运算仅需作N步矩阵求和，计算等效离散系统噪声方差阵所需的矩阵运算为作N步矩阵求和后作一步矩阵相乘，与之前的公式相比计算量大为减小，对工程实现十分有利。此外，与未简化的算法仅用到 时刻系统的动态信息相比，简化算法利用了 内N次采样获得的系统动态信息，这在一定程度上弥补了级数过早截断引起的精度损失。

也可以利用MATLAB直接进行状态方程离散化

在matlab中，[G,H]=c2d(A,B,Ts)，Ts是采样周期

得到离散化状态空间等式：x(k+1)=Gx(k) + Hu(k)