算法-卡尔曼滤波之多维卡尔曼滤波的状态外推方程

在学了一维卡尔曼滤波后，但是在实际情况下，很多动态过程都是多维的；

例如如图所示，该飞机是一个全姿态的，我们考虑它的动态过程，例如三维空间下飞机xyz的位置，xyz的速度，xyz的加速度等， 它们满足的关系是，我们使用矩阵来表示；

假设飞机是以恒定加速度进行运动的，此时飞机的状态外推方程为如下9个方程，它们都是在时间点n时刻，飞机的系统状态，但是对于更多更复杂的模型，我们使用列方程将会很复杂，通常做法是使用矩阵来描述多维过程；

 

状态外推方程，就是预测-已知当前时刻状态，利用方程推的下一时刻的状态；

其中对应：

F-状态转移矩阵；

G-输入控制矩阵；

Wn-过程噪声；（一般不用，我们通常放在协方差外推方程中）；

Un是输入变量/控制变量；

 

 

一般来说状态变量的个数，不小于 独立储能元件的个数；

那么哪些变量是属于状态变量，哪些是属于系统输入，是需要我们单独分析的；

 下面给出几个例子：

弹簧系统，此时状态变量有：施加给弹簧的力F，弹簧的位移；

其中施加给弹簧的力F就是系统输入，弹簧的位移是系统状态；

 

 

对应转换为状态转移矩阵F如下图： 

 引入输入控制：

下面给出几个例子：

在自由落体下：

 

这就是多维卡尔曼的状态外推方程，当前时刻的估计值=状态转移矩阵*上一时刻的估计值+输入控制矩阵*输入控制+过程噪声（通常这个噪声会存在与协方差外推方程中）；

我们通过把多维复杂的外推方程使用状态转移矩阵来表示，把输入控制使用输入控制矩阵来表示，得到了简单美观的矩阵形式的状态外推方程；