扩展卡尔曼滤波(EKF)之非线性系统的线性化

涅索斯衬衫

不要问，问就是不会

引言

——为什么要线性化？

上篇主要介绍了卡尔曼滤波的推导过程，由前篇的描述可知，卡尔曼滤波的应用前提是系统需为线性系统，且系统的噪声（过程噪声、测量噪声）均为高斯白噪声。但实际上，几乎所有的系统是非线性的（倒立摆、机器人系统），即系统状态变量的差分方程是非线性的。既然如此，是不是意味着卡尔曼滤波无法应用了呢？当然不是。我们知道，对于非线性系统的分析复杂，虽然有许多方法对其分析（相平面、描述函数法等），但很多情况下依然会选择线性化处理，使其等效为一个线性系统。经过线性化话处理后，卡尔曼滤波的适用条件便可满足，这也就是扩展卡尔曼滤波（ Extern Kalman Filter, EKF ）的由来。

本篇主要介绍在操作点（ an operator point ）处对非线性系统的雅克比线性化（ Jacobian Linearization ）的过程以及实例分析。

一、控制系统的数学描述

设一动态系统的数学模型为：

\dot{x}=f(x,u)

z=h(x,u)

其中该系统的状态变量向量 x=(x_1,x_2,...,x_n)\in{R^{n}} ,控制输入向量 u=(u_1,u_2,...,u_m)\in{R^{m}} ,系统观测向量 z=(z_1,z_2,...,z_p)\in{R^{p}} 。其中，状态向量的差分方程及观测方程如下所示：

\left\{\begin{matrix} \dot{x_1}=f_1(x_1,x_2,...,x_n,u_1,...,u_m)\\ \dot{x_2}=f_2(x_1,x_2,...,x_n,u_1,...,u_m)\\ \vdots \\ \dot{x_n}=f_n(x_1,x_2,...,x_n,u_1,...,u_m) \end{matrix}\right.

\left\{\begin{matrix} {z_1}=y_1(x_1,x_2,...,x_n,u_1,...,u_m)\\ {z_2}=y_2(x_1,x_2,...,x_n,u_1,...,u_m)\\ \vdots \\ {z_p}=z_p(x_1,x_2,...,x_n,u_1,...,u_m) \end{matrix}\right.

为便于对上述模型的理解，给出两个实例。

实例1：线性系统 \rightarrow 速度方程 \dot{p}=v

\left\{\begin{matrix} \dot{x_1}=x_2\\ z=x_1\\ \end{matrix}\right.

实例2：非线性系统 \rightarrow 二轮车，其中状态变量 X=(x,y,\theta,v) [1]。

\left\{\begin{matrix} \dot{x}=vcos\theta \\ \dot{y}=vsin\theta\\ \dot{\theta}=u_1\\ \dot{v}=u_2\\ \end{matrix}\right.

二、线性系统与非线性系统的差别

通过上述两个实例了解了系统模型的描述形式，但这两个系统之间有何差别？即线性系统与非线性系统的区别在哪呢？了解控制对象的基本属性是我们设计控制算法的前提。

（1）齐次性

线性系统可以应用线性叠加原理，也就是满足齐次性，而描述非线性系统的数学模型为非线性微分方程，因此叠加原理不可用。故 能否应用叠加原理是这两类系统的本质区别 。

（2）平衡态

什么是平衡态（an equilibrium point）

平衡态的定义 ：在无外力作用且系统输出的各阶导数等于零时，系统处于平衡态，而此时系统的状态点即为平衡点( x^{*} )，即 f(x^{*},0)=0 。

显然，对于线性系统，只有一个平衡状态 \dot{x}=A*x 。 线性系统的稳定性 即为平衡状态的稳定性，而且 只取决于系统本系统的结构和参数，与外作用力和初始条件无关 [1]。而非线性系统可能存在多个平衡状态，各平衡状态可能是稳定的，也可能是不稳定的（如实例2），且平衡状态的稳定性不仅与系统的结构与参数有关，还与系统的初始条件有直接的关系。

通过上述两个实例不难分析出非线性系统与线性系统之间的这两个特性。这一部分参考胡寿松版《自动控制原理》第八章即可。

三、非线性系统的线性化

——如何线性化？

若上述系统为一非线性系统，则该系统在一个操作点 (x_0,u_0) ( the operating point )处的“ 标称轨迹 ”满足：

\dot{x_0}=f(x_0,u_0)

{z_0}=h(x_0,u_0)

若该非线性系统的运动在标称系统轨迹(nominal system trajectory)的邻域内，即 x^{*} = {x_0+\delta{x}} ， u^{*} = {u_0+\delta{u}} ， z^{*} = {z_0+\delta{z}} ，则有：

\frac{d(x_0+\delta{x})}{dt}=f(x_0+\delta{x},u_0+\delta{u})

z+\delta{z}=h(x+\delta{x_0},u_0+\delta{u})

此时 \delta 表示一个小增量(a small quantity)，对其进行 Taylor公式 展开，即有：

\dot{x_0}+\delta{\dot{x}}=f(x_0,u_0)+\frac{\partial {f} }{\partial x}\mid_{(x_0,u_0)}\delta{x}+\frac{\partial {f} }{\partial u}\mid_{(x_0,u_0)}\delta{u}+higer-order-terms

z+\delta{z}=h(x_0,u_0)+\frac{\partial {h} }{\partial x}\mid_{(x_0,u_0)}\delta{x}+\frac{\partial {h} }{\partial u}\mid_{(x_0,u_0)}\delta{u}+higer-order-terms

忽略高阶项并化简，可知在操作点 (x_0,u_0) 处 非线性系统线性化模型 如下所示：

————————————————————

\delta{\dot{x}}=\frac{\partial {f} }{\partial x}\mid_{(x_0,u_0)}\delta{x}+\frac{\partial {f} }{\partial u}\mid_{(x_0,u_0)}\delta{u} （1）

\delta{z}=\frac{\partial {h} }{\partial x}\mid_{(x_0,u_0)}\delta{x}+\frac{\partial {h} }{\partial u}\mid_{(x_0,u_0)}\delta{u} （2）

————————————————————

其中：

\frac{\partial {f} }{\partial x}=(\frac{\partial {f1} }{\partial x},...,\frac{\partial {fn} }{\partial x})^{T} ， \frac{\partial {f} }{\partial u}=(\frac{\partial {f1} }{\partial u},...,\frac{\partial {fn} }{\partial u})^{T}

\frac{\partial {h} }{\partial x}=(\frac{\partial {h1} }{\partial x},...,\frac{\partial {hn} }{\partial x})^{T} ， \frac{\partial {h} }{\partial u}=(\frac{\partial {h1} }{\partial u},...,\frac{\partial {hn} }{\partial u})^{T}

\frac{\partial {f} }{\partial x} ， \frac{\partial {f} }{\partial u} 为状态差分方程 f[\bullet] 对状态向量 x 和控制向量 u 的雅克比矩阵； \frac{\partial {h} }{\partial x} ， \frac{\partial {h} }{\partial u} 为观测方程 z[\bullet] 对状态向量 x 和控制量 u 的雅克比矩阵。其描述形式如下：

\left\{\begin{matrix} \frac{\partial {f1} }{\partial x}\approx(\frac{\partial{f_1}}{\partial{x_1}}, \frac{\partial{f_1}}{\partial{x_2}}, ..., \frac{\partial{f_1}}{\partial{x_n}})\\ \frac{\partial {f2} }{\partial x}\approx(\frac{\partial{f_2}}{\partial{x_1}}, \frac{\partial{f_2}}{\partial{x_2}}, ..., \frac{\partial{f_2}}{\partial{x_n}})\\ \vdots\\ \frac{\partial {fn} }{\partial x}\approx(\frac{\partial{f_n}}{\partial{x_1}}, \frac{\partial{f_n}}{\partial{x_2}}, ..., \frac{\partial{f_n}}{\partial{x_n}})\\ \end{matrix}\right.

\left\{\begin{matrix} \frac{\partial {h1} }{\partial x}\approx(\frac{\partial{h_1}}{\partial{x_1}}, \frac{\partial{h_1}}{\partial{x_2}}, ..., \frac{\partial{h_1}}{\partial{x_n}})\\ \frac{\partial {h2} }{\partial x}\approx(\frac{\partial{f_2}}{\partial{x_1}}, \frac{\partial{h_2}}{\partial{x_2}}, ..., \frac{\partial{h_2}}{\partial{x_n}})\\ \vdots\\ \frac{\partial {hn} }{\partial x}\approx(\frac{\partial{h_n}}{\partial{x_1}}, \frac{\partial{h_2}}{\partial{x_2}}, ..., \frac{\partial{h_n}}{\partial{x_n}})\\ \end{matrix}\right.

因此，对比式(1)(2)可知，在标称轨迹 (x_0,u_0) 处，有 A = \frac{\partial {f} }{\partial x}\mid_{(x_0,u_0)} ， B = \frac{\partial {f} }{\partial u}\mid_{(x_0,u_0)} ， H = \frac{\partial {h} }{\partial x}\mid_{(x_0,u_0)} ， J = \frac{\partial {h} }{\partial u}\mid_{(x_0,u_0)} 。其中， A\in{R^{n\times{n}}} , G\in{R^{m\times{1}}} , H\in{R^{p\times{1}}} ， J\in{R^{p\times{1}}} ， 线性化方程 表示如下所示：

——————————————

\dot{\delta{x}}=A*\delta{x}+B*{\delta{u}} (3)

\delta{z}=H*{\delta{x}}+J*{\delta{u}} (4)

——————————————

不难得知，上述线性化方程式已成为Kalman滤波所需的状态方程和观测方程形式，根据前篇的Kalman滤波基本方程（黄金五条），即可得 基于状态偏差的Kalman滤波递推方程 的最优估计（PX4飞控基于此）。

四、实例分析

倒立摆 (A pendulum)

倒立摆的动态模型如上所示，系统的平衡方程[5]：

I\frac{d^{2}{\theta}}{d^{2}{t}}+Mglsin{\theta}=u

y={\theta}

令状态变量 (x_1,x_2)=(\theta,\dot{\theta}) ，则状态方程：

\dot{x_1}=x_2

\dot{x_2}=-\frac{Mgl}{I}sin{x_1}+\frac{u}{I}

y=x_1

在平衡点 (x_1^{\ast}, x_2^{\ast}) = (\pi, 0) 处的线性化方程如下所示：

\begin{bmatrix} \delta{\dot{x_1}}\\ \delta{\dot{x_2}}\\ \end{bmatrix}=\begin{bmatrix} 0\quad 1\\ \frac{Mgl}{I}\quad 0\\ \end{bmatrix}\begin{bmatrix} \delta{{x_1}}\\ \delta{{x_2}}\\ \end{bmatrix}+\begin{bmatrix} 0\\ \frac{1}{I}\\ \end{bmatrix}{\delta{u}}

\delta{y}=\begin{bmatrix} 0\\ 1\\ \end{bmatrix}\begin{bmatrix} \delta{{x_1}}\\ \delta{{x_2}}\\ \end{bmatrix}=\delta{x_1}

MATLAB code实现如下

syms x1 x2 u;
syms M g l I;
x = [x1 x2];
f1 = x2;
f2 = -(M*g*l/I)*sin(x1)+u/I;