2.2　非线性高斯系统状态估计的扩展卡尔曼滤波器设计

前面所讲的卡尔曼滤波要求系统状态方程和观测方程都是线性的。然而，许多工程系统往往不能用简单的线性系统来描述。例如，导弹控制问题、测轨问题和惯性导航问题的系统状态方程往往不是线性的。因此，有必要研究非线性滤波问题。对于非线性模型的滤波问题，理论上还没有严格的滤波公式。一般情况下，先将非线性方程线性化，然后利用线性系统的卡尔曼滤波基本方程进行处理。本节就给出非线性系统的卡尔曼滤波问题的处理方法[1-4]。

2.2.1　非线性系统模型

一类离散非线性系统的状态方程和观测方程为

x(k+1)=f(x(k),w(k),k)

（2.2.1）

y(k+1)=h(x(k+1),v(k+1),k+1)

（2.2.2）

其中，x(k)∈Rn×1是n维状态向量；y(k)∈Rm×1是m维测量向量；w(k) 和v(k+1) 分别是状态建模误差噪声向量和测量误差噪声向量；f(x(k),w(k),k)∈Rn×1是以x(k),w(k),k为共同作用的n维状态转换非线性函数模型；h(x(k+1),v(k+1),k+1)∈Rm×1是以x(k+1),v(k+1), k+1 为共同作用的m维观测非线性函数模型。

为了简化，这里仅讨论下列情况下的非线性模型：

其中，w(k) 和v(k+1) 都是均值为零的白噪声序列，并满足如下统计特性。

在式（2.2.3）和式（2.2.4）中

进一步，在式（2.2.4）中

另外，已知初始状态x(0) 的统计特性：

并假设初始状态x(0) 、w(k) 、v(k+1) 之间是统计独立的，即

下面介绍两种线性化滤波方法：围绕标称轨迹的线性化滤波方法和围绕滤波值的线性化滤波方法。这两种方法都是先把非线性模型线性化，然后应用线性系统的卡尔曼滤波基本公式。

2.2.2　围绕标称轨迹的线性化滤波方法

这里以式（2.2.3）和式（2.2.4）表示的非线性动态系统为背景，介绍围绕标称轨迹的线性化滤波方法。以初始状态x(0) 为起点，基于状态模型即式（2.2.3），求取系统状态x(k) 的标称轨迹：

状态向量x(k) 与标称状态向量x*(k) 之间的误差向量

称为状态偏差向量。

1．非线性状态模型的线性化表示

把状态模型即式（2.2.3）中的非线性函数f(x(k),k) 围绕标称状态向量x*(k) 进行泰勒展开：

略去二次项及高阶项，可得

令

为向量函数f(⋅) 的雅克比矩阵。

式（2.2.8）经移项整理后，可得

基于式（2.2.6），可得状态方程的近似线性化模型：

2．非线性观测模型的线性化表示

以式（2.2.7）得到的状态x(k) 的标称轨迹x*(k) 为基础，得到系统标称状态对应的标称观测值序列：

类似于非线性函数f(x(k),k) 围绕标称状态向量x*(k) 进行泰勒展开，这里将式（2.2.2）中的非线性函数h(x(k+1),k+1) 围绕标称状态向量x*(k+1) 进行泰勒展开：

略去二次项及高阶项后，可得

其中

若令

则可得非线性观测方程即式（2.2.2）的线性化方程：

3．围绕标称轨迹的线性卡尔曼滤波器设计

由式（2.2.3）和式（2.2.4）描述的非线性系统，经过以上线性化变换后，已转换成符合卡尔曼滤波器设计要求的线性系统，即式（2.2.5）和式（2.2.6）。因此，可以利用2.1节中建立的卡尔曼滤波基本方程，设计出围绕标称轨迹的线性卡尔曼滤波器。

首先，假设已知线性系统状态模型即式（2.2.9）中初始状态δx(0) 的统计特性：

假设已获得从1时刻至k时刻的观测值：

并假设已获得k时刻的状态偏差δx(k) 的估计值和估计误差协方差矩阵：

下面将设计出求解状态偏差δx(k+1) 估计值和估计误差协方差矩阵δP(k+1|k+1) 的卡尔曼滤波器递推方程组。

（1）状态偏差δx(k+1) 的一步预测估计。基于式（2.2.10），有

再将式（2.2.10）与式（2.2.17）相结合，得到状态偏差δx(k+1) 的预测估计误差：

以及状态偏差δx(k+1) 的预测估计误差的协方差矩阵：

（2）观测偏差向量δy(k+1) 的一步预测估计。基于线性观测模型即式（2.2.15），有

再将式（2.2.15）与式（2.2.20）相结合，得到δy(k+1) 的一步预测估计误差：

称为k+1 时刻的观测信息。

（3）求解状态偏差向量δy(k+1) 估计值的卡尔曼滤波器设计：

其中，卡尔曼滤波器的增益矩阵δK(k+1) 为

（4）求解状态偏差向量δy(k+1) 估计误差的协方差矩阵：

（5）式（2.2.10）和式（2.2.15）围绕标称轨迹的线性卡尔曼滤波估计为

这种线性化滤波方法通常只在能够得到标称轨迹，并且状态偏差δx(k) 较小时使用。

2.2.3　围绕滤波值的线性化滤波方法

围绕滤波值的线性化滤波方法是将式（2.2.3）中的非线性函数f(x(k),k) 围绕滤波值展开成泰勒级数，略去二次项及高阶项后，得到非线性系统的线性化模型。

考虑非线性滤波方程即式（2.2.3）和式（2.2.4），假设已知初始值信息

并且已知观测信息y(1),y(2),⋯,y(k) ，可以获得状态向量x(k) 的估计值的估计误差协方差矩阵：

1．非线性状态模型的线性化表示

将式（2.2.3）中的非线性函数f(x(k),k) 围绕标称状态进行泰勒展开：

略去二次项及高阶项，可得式（2.2.3）的近似线性化方程：

其中

并得到状态x(k+1) 的一步预测估计值：

2．非线性观测模型的线性化表示

下面把式（2.2.4）中的非线性函数h(x(k+1),k+1) 围绕状态x(k+1) 的一步预测估计值进行泰勒展开：

略去式（2.2.29）中的二次项及高阶项，可得非线性观测方程的线性形式：

其中

至此，由式（2.2.1）～式（2.2.4）描述的一类非线性系统，通过一阶泰勒展开，被近似简化为由式（2.2.27）～式（2.2.30）描述的线性系统，已符合标准线性卡尔曼滤波器的设计条件。

3．围绕滤波值的线性卡尔曼滤波器设计

（1）状态x(k+1) 的一步预测估计。基于式（2.2.1）和式（2.2.4），有

再将式（2.2.32）与式（2.2.26）相结合，得到状态x(k+1) 的一步预测估计误差：

进一步得到预测估计误差的方差矩阵：

（2）观测向量y(k+1) 的一步预测估计。基于由式（2.2.33）得到的状态x(k+1) 的一步预测估计值和式（2.2.31），得到观测向量y(k+1) 的预测估计值：

再将式（2.2.35）与式（2.2.31）相结合，得到观测向量y(k+1) 的预测估计误差：

称为k+1 时刻获得的观测信息。

（3）围绕滤波值线性化的递归卡尔曼滤波器设计。基于上述步骤，设计由式（2.2.3）和式（2.2.4）描述的非线性系统关于状态变量x(k+1) 估计值的扩展卡尔曼滤波器：

即

（4）扩展卡尔曼滤波器的增益矩阵K(k+1) 求解。

首先，计算状态变量x(k+1) 的估计误差向量，由式（2.2.37）可得

还有观测向量的一个分解表达式：

由式（2.2.39）和式（2.2.40），利用下列原理

可以求出扩展卡尔曼滤波器的增益矩阵K(k+1) 。

（5）计算状态变量x(k+1) 的估计误差协方差矩阵。由式（2.2.39），可得

至此，就设计出由式（2.2.26）～式（2.2.42）组成的围绕滤波值线性化的卡尔曼滤波器。其中，式（2.2.37）就是具体的卡尔曼滤波器。

4．非线性系统状态估计的扩展卡尔曼滤波器性能分析

在式（2.2.7）～式（（2.2.10）的线性化简化过程中，非线性函数f(x(k),k) 围绕标称轨迹x*(k) 的泰勒展开式中，所有非线性项为

在式（2.2.11）～式（2.2.15）的线性化简化过程中，非线性函数h(x(k+1),k+1) 围绕标称轨迹x*(k+1) 的泰勒展开式中，所有非线性项为

在式（2.2.26）～式（2.2.28）的线性化简化过程中，非线性函数f(x(k),k) 围绕滤波估计值的泰勒展开式中，所有非线性项为

在式（2.2.29）～式（2.2.31）的线性化简化过程中，非线性函数h(x(k+1),k+1) 围绕滤波估计值的泰勒展开式中，所有非线性项为

以上非线性项都被舍去，随着系统中非线性函数f(x(k),k) 和h(x(k+1),k+1) 的非线性程度不断增加，系统在简化过程中也将丢失更多的信息，这必然会导致由非线性系统所设计出的扩展线性卡尔曼滤波器的性能也随着系统非线性程度的增加而逐渐下降，甚至导致滤波器发散。因此，如何设计出对非线性函数f(x(k),k) 和h(x(k+1),k+1) 具有更强逼近能力的线性卡尔曼滤波器，一直是人们感兴趣的研究课题。

后面两节将要介绍的滤波器对非线性函数f(x(k),k) 和h(x(k+1),k+1) 具有更强逼近能力，所以相对于本节所设计的滤波器在性能上有较大提升。