深入了解卡尔曼滤波器(3)
声明： 本文的图文均来自https://www.kalmanfilter.net如有侵权，请联系删除。
本文由微信公众号组成【DeepDriving】由于全文内容较多，分为三部分发出。关注微信官方账号【DeepDriving】，后台回复关键词卡尔曼滤波器可获得全文PDF。
多维卡尔曼滤波器
上面介绍了一维卡尔曼滤波器。我相信你对卡尔曼滤波器有一定的了解，但在实际应用中，我们通常需要处理具有多维状态数据的系统。例如，对于三维空间中的飞机，我们需要9维向量来描述其位置、速度和加速度：
[ x y z x ˙ y ˙ z ˙ x ¨ y ¨ z ¨ ] T \begin{bmatrix} x & y & z & \dot{x} & \dot{y} & \dot{z} & \ddot{x} & \ddot{y} & \ddot{z} \end{bmatrix}^{T} [xyzx˙y˙z˙x¨y¨z¨]T
假设我们采用恒加速度（constant acceleration）动态模型，那么在 n n n时刻飞机的状态可以写为
{ x n = x n − 1 + x ˙ n − 1 Δ t + 1 2 x ¨ n − 1 Δ t 2 y n = y n − 1 + y ˙ n − 1 Δ t + 1 2 y ¨ n − 1 Δ t 2 z n = z n − 1 + z ˙ n − 1 Δ t + 1 2 z ¨ n − 1 Δ t 2 x ˙ n = x ˙ n − 1 + x ¨ n − 1 Δ t y ˙ n = y ˙ n − 1 + y ¨ n − 1 Δ t z ˙ n = z ˙ n − 1 + z ¨ n − 1 Δ t x ¨ n = x ¨ n − 1 y ¨ n = y ¨ n − 1 z ¨ n = z ¨ n − 1 \begin{cases} x_{n} = x_{n-1} + \dot{x}_{n-1} \Delta t+ \frac{1}{2}\ddot{x}_{n-1} \Delta t^{2}\\ y_{n} = y_{n-1} + \dot{y}_{n-1} \Delta t+ \frac{1}{2}\ddot{y}_{n-1} \Delta t^{2}\\ z_{n} = z_{n-1} + \dot{z}_{n-1} \Delta t+ \frac{1}{2}\ddot{z}_{n-1} \Delta t^{2}\\ \dot{x}_{n} = \dot{x}_{n-1} + \ddot{x}_{n-1} \Delta t\\ \dot{y}_{n} = \dot{y}_{n-1} + \ddot{y}_{n-1} \Delta t\\ \dot{z}_{n} = \dot{z}_{n-1} + \ddot{z}_{n-1} \Delta t\\ \ddot{x}_{n} = \ddot{x}_{n-1}\\ \ddot{y}_{n} = \ddot{y}_{n-1}\\ \ddot{z}_{n} = \ddot{z}_{n-1}\\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧xn=xn−1+x˙n−1Δt+21x¨n−1Δt2yn=yn−1+y˙n−1Δt+21y¨n−1Δt2zn=zn−1+z˙n−1Δt+21z¨n−1Δt2x˙n=x˙n−1+x¨n−1Δty˙n=y˙n−1+y¨n−1Δtz˙n=z˙n−1+z¨n−1Δtx¨n=x¨n−1y¨n=y¨n−1z¨n=z¨n−1
在实际应用中，我们通常会使用矩阵的方式来描述多维数据处理的过程。接下来，我们将用矩阵的方式来介绍多维卡尔曼滤波器的几个方程。
状态外推方程
状态外推方程的作用是在当前时刻 n n n基于现有的知识去预测 n + 1 n+1 n+1时刻系统的状态，所以也叫状态预测方程或者状态转移方程，其矩阵形式的公式如下：
x ^ n + 1 , n = F x ^ n , n + G u n + w n \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+Gu_{n}+w_{n}} x^n+1,n=Fx^n,n+Gun+wn
其中， x ^ n + 1 , n \boldsymbol{\hat{x}_{n+1,n}} x^n+1,n是预测的 n + 1 n+1 n+1时刻的系统状态向量； x ^ n , n \boldsymbol{\hat{x}_{n,n}} x^n,n是估计的 n n n时刻的系统状态向量； u n \boldsymbol{u_{n}} un