卡尔曼滤波算法-Kalman Filter Algorithm

1、简介

1.1 滤波是什么

所谓了滤波，就是从混合在一起的诸多信号中提取出所需要的信号。

1.2 信号的分类：

（1）确定性信号：可以表示为确定的时间函数，可确定其在任何时刻的量值。（具有确定的频谱）

（2）随机信号：不能用确定的数学关系来描述的，不能预测其任何瞬时值，其变化服从统计规律。（频率不确定，功率谱确定）

确定性信号的滤波：

可采用低通、高通、带通和带阻等模拟滤波器或者计算机通过算法实现--常规滤波。

随机信号的滤波：

根据有用信号和干扰信号的功率谱设计滤波器--维纳滤波(Wiener Filtering) 或者卡尔曼滤波(Kalman Filter)

随机信号的滤波也可以看做是估计问题。

1.3 kalman filter是什么

简单的说卡尔曼滤波器是一个“optical recursive data processing algorithm(最优化回归数据处理算法）”。从形式上，卡尔曼滤波器是5个公式。

对于解决大部分问题卡尔曼滤波器是最优、效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航、控制、传感器数据融合，甚至在雷达系统及导弹追踪等方面，近年来更被用于计算机图像处理，例如头像识别，头像分割、图像边缘检测等等。

1.4 卡尔曼滤波器的特点：

（1）卡尔曼滤波器处理的对象是随机信号；

（2）被处理的信号无有用和干扰之分，滤波的目的是要估计出所有被处理的信号（区别于维纳滤波）；

（3）系统的白噪声激励和测量噪声并不是需要滤除的对象，它们的统计特性是估计过程中需要利用的信息；（区别于最小二乘法）

（4）算法是递推的，且使用状态空间法在时域内设计滤波器，适用于对多维随机过程；

（5）被估计量既可以是平稳的，也可以是非平稳的；

（6）估计过程中，只需要考虑过程噪声和测量噪声及当前时刻系统状态的统计特性。（花费计算机计算资源少）

1.5 卡尔曼滤波分类：

现行系统的卡尔曼滤波方程；

1）线性离散系统 2）线性连续系统

非线性系统的卡尔曼滤波方程

1）扩展卡尔曼滤波器EKF 2）无迹卡尔曼滤波器UKF

1.6 卡尔曼滤波的基本思想：

卡尔曼滤波就是把统计学应用到了滤波算法上。其算法的核心思想是,根据当前的仪器"测量值" 和上一刻的 "预测量" 和 "误差",计算得到当前的最优量. 再预测下一刻的量,

里面比较突出的是观点是. 把误差纳入计算, 而且分为预测误差和测量误差两种.通称为噪声.

还有一个非常大的特点是,误差独立存在, 始终不受测量数据的影响.

在海图作业中，假设航海长需要知道当前时刻船的位置，航海长通常以上一时刻的船位基准，根据航向、航速和海流等一系列因素推算当前刻的船位置，观测和推算这两个船位一般不重合，无论是通过航向航速等因素计算出来的结果和当前时刻传感器对船位的测量结果都是不准确的，都会有误差，航海长需要通过分析和判断选择一个可靠的船位，作为船舰的下一个位置。当前准确位置变成一个根据计算估计出来的位置和传感器测量出来的位置获取真实位置的最优估计问题。

上面这种场景便是一个典型的卡尔曼滤波的应用场景，卡尔曼滤波的基本思想是：

以k-1时刻的最优估计 $x_{k-1}$ 为准，预测k时刻的状态变化量 $\widehat{x}_{k/k-1}$ ,同时又对该状态进行观测，得到观测变量 $Z_{k}$ ,再在预测和观测之间进行分析，或者说以观测量对预测量进行修正，从而得到k时刻的最优状态估计 $x_{k}$

2、卡尔曼滤波器的基本方程：

2.1、无控制离散型卡尔曼滤波器的基本方程：

系统的状态方程： $x_{k} = \phi _{k,k-1} * x_{k-1}+\Gamma _{k-1}w_{k-1}$

系统的测量方程： $Z_{k}=H_{k}*x_{k} + v_{k}$

这里：

$w_{k-1}$ 为过程白噪声；

$v_{k}$ 位测量噪声；

$\Gamma$ 为噪声驱动矩阵；

$Z_{k}$ 系统测量方程的输出量（即观测量），是可以实际测量的量；

如果 $w_{k-1}$ 和 $v_{k}$ 满足

$E\left [ w_{k} \right ] = 0,Cov\left [ w_{k},w_{j} \right ] = Q_{k}\delta _{kj};$

$E\left [ v_{k} \right ] = 0,Cov\left [ v_{k},v_{j} \right ] =R_{k}\delta _{kj};$

$Cov\left [ w_{k},v_{j} \right ]=0$

$Q_{k}$ 位过程噪声的协方差矩阵，表示 $w$ 向量元素之间的相关关系，其为非负定阵；

$R_{k}$ 为测量噪声的协方差，表示 $v$ 向量元素之间的相关关系，其为正定阵；

在实际应用中 $w_{k-1}$ 和 $v_{k}$ 常假设为满足高斯分布，过程噪声 $w_{k-1}$ 和测量噪声 $v_{k}$ 假设是相互独立的（之间没有关系，无相互影响）

意思是这些噪声在离散的状态上是没有关系的（互相独立的，每个时刻的噪声都是独立的）且服从高斯分布：

$p(w_{k-1})$ ~ $N(0,Q)$

$p(v_{k})$ ~ $N(0,R)$

$Q_{k}=ww^{T}$

$R_{k}=vv^{T}$

$E\left [ w_{k} \right ] = 0$

$E\left [ v_{k} \right ] = 0$

则无控制离散型卡尔曼滤波的基本方程为；

(1)对k时刻状态的预测方程：

${\widehat{x}_{k/k-1} =\phi _{ k,k-1}*x_{k-1}$ (1-1)

(2)对k时刻预测的均方误差的预测方程：

${\widehat{P}_{k/k-1} =\phi _{ k,k-1}*P_{k-1}*\phi _{ k,k-1}^{T} + \Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^{T}$ (1-2)

(3)滤波增益方程（权重）：

$K_{k}={\widehat{P}_{k/k-1}} * H_{k}^{T}\left [ H_{k} * {\widehat{P}_{k/k-1}} * H_{k}^{T} + R_{k} \right ]^{-1}$ (1-3)

(4)滤波估计方程（k时刻的最优估计值）：

${x}_{k} ={\widehat{x}_{k/k-1}} + K_{k}\left [ Z_{k} -H_{k}* {\widehat{x}_{k/k-1}\right ]$ (1-4)

(5)滤波均方误差更新矩阵（k时刻最优均方误差）：

${P}_{k} =\left [ I - K_{k}*H_{k} \right ]*\widehat{P}_{k/k-1}$ (1-5)

参数解析：

1、 ${x}_{k-1}$ 和 ${x}_{k}$ : 分别表示 k-1 时刻和 k 时刻的后验状态估计值，是滤波的结果之一，即更新后的结果，也叫最优估计（估计的状态，根据理论，我们不可能知道每时刻状态的确切结果所以叫估计）。

2、 ${\widehat{x}_{k/k-1}$ : k 时刻的先验状态估计值，是滤波的中间计算结果，即根据上一时刻（k-1时刻）的最优估计预测的k时刻的结果，是预测方程的结果。

3、 ${P}_{k-1}$ 和 ${P}_{k}$ ：分别表示 k-1 时刻和 k 时刻的后验估计协方差（即的协方差，表示状态的不确定度），是滤波的结果之一。

4、 ${\widehat{P}_{k/k-1}$ ：k 时刻的先验估计协方差（ ${\widehat{x}_{k/k-1}$ 的协方差），是滤波的中间计算结果。

5、 $H$ ：是状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系，卡尔曼滤波里为线性关系，它负责将 m 维的测量值转换到 n 维，使之符合状态变量的数学形式，是滤波的前提条件之一。

6、 $Z_{k}$ ：测量值（观测值），是滤波的输入。

7， $K_{k}$ ：滤波增益矩阵，是滤波的中间计算结果，卡尔曼增益，或卡尔曼系数。

8、 $\phi _{ k,k-1}$ ：状态转移矩阵，实际上是对目标状态转换的一种猜想模型。例如在机动目标跟踪中，状态转移矩阵常常用来对目标的运动建模，其模型可能为匀速直线运动或者匀加速运动。当状态转移矩阵不符合目标的状态转换模型时，滤波会很快发散。

9、 $Q_{k}$ ：过程激励噪声 $w$ 的协方差（系统过程的协方差），表示 $w$ 向量元素之间的相关关系。该参数被用来表示状态转换矩阵与实际过程之间的误差。因为我们无法直接观测到过程信号，所以 Q 的取值是很难确定的。是卡尔曼滤波器用于估计离散时间过程的状态变量，也叫预测模型本身带来的噪声。状态转移协方差矩阵

10、 $R_{k}$ ：测量噪声 $v$ 的协方差，表示 $v$ 向量元素之间的相关关系。滤波器实际实现时，测量噪声协方差 R一般可以观测得到，是滤波器的已知条件。

11、 $Z_{k} -H_{k}* {\widehat{x}_{k/k-1}$ ：实际观测和预测观测的残差，和卡尔曼增益一起修正先验（预测），得到后验。

2.2、带有控制的离散型卡尔曼滤波基本方程

系统的状态方程： $x_{k} = \phi _{k,k-1}* x_{k-1}+\Gamma _{k-1}w_{k-1} + B_{k-1}u_{k-1}$

系统的测量方程： $Z_{k}=H_{k}*x_{k} + v_{k}$

参数解析：

1、 $u_{k-1}$ 表示系统的控制输入；

2、 $B_{k-1}$ 是系统参数，表示系统控制输入k-1时刻到k时刻，系统输入转化为状态的转换矩阵；

如果 $w_{k-1}$ 和 $v_{k}$ 满足

$E\left [ w_{k} \right ] = 0,Cov\left [ w_{k},w_{j} \right ] = Q_{k}\delta _{kj};$

$E\left [ v_{k} \right ] = 0,Cov\left [ v_{k},v_{j} \right ] =R_{k}\delta _{kj};$

$Cov\left [ w_{k},v_{j} \right ]=0$

$Q_{k}$ 位过程噪声的协方差，其为非负定阵；

$R_{k}$ 为测量噪声的协方差，其为正定阵；

则带控制离散型卡尔曼滤波的基本方程为；

(1)对k时刻状态的预测方程：

${\widehat{x}_{k/k-1}} =\phi _{ k,k-1}*x_{k-1} +B_{k-1}u_{k-1}$ (2-1)

(2)对k时刻预测的均方误差的预测方程：

${\widehat{P}_{k/k-1}} =\phi _{ k,k-1}*P_{k-1}*\phi _{ k,k-1}^{T} + \Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^{T}$ (2-2)

(3)滤波增益方程（权重）：

$K_{k}={\widehat{P}_{k/k-1}} * H_{k}^{T}\left [ H_{k} * {\widehat{P}_{k/k-1}} * H_{k}^{T}+R_{k} \right ]^{-1}$ (2-3)

(4)滤波估计方程（k时刻的最有估计值）：

${x}_{k} ={\widehat{x}_{k/k-1}}+ K_{k}\left [ Z_{k} - H_{k}*\widehat{x}_{k/k-1}\right ]$ (2-4)

(5)滤波均方误差更新矩阵（k时刻最优均方误差）：

${P}_{k} =\left [ I - K_{k}*H_{k} \right ]*\widehat{P}_{k/k-1}$ (2-5)

2.3、线性离散型卡尔曼滤波方程的一般形式：

系统的状态方程： $x_{k} = \phi _{k,k-1}* x_{k-1}+\Gamma _{k-1}w(k-1) + B_{k-1}u_{k-1}$

系统的测量方程： $Z_{k}=H_{k}*x_{k} + v_{k}$

如果 $w_{k-1}$ 和 $v_{k}$ 满足

$E\left [ w_{k} \right ] = 0,Cov\left [ w_{k},w_{j} \right ] = Q_{k}\delta _{kj};$

$E\left [ v_{k} \right ] = 0,Cov\left [ v_{k},v_{j} \right ] =R_{k}\delta _{kj};$

$Cov\left [ w_{k},v_{j} \right ]=S_{k}\delta _{kj}$

$Q_{k}$ 位过程噪声的协方差，其为非负定阵；

$R_{k}$ 为测量噪声的协方差，其为正定阵；

引入矩阵 $J_{k}=\Gamma _{k-1}S_{k}R_{k}^{-1}$ ，对状态方程进行等效变换：

$x_{k} =\phi_{k,k-1}^{*}x_{k-1} +B_{k-1}u_{k-1} + J_{k-1}Z_{k-1}+w_{k-1}^{*};$

$Z_{k}=H_{k}x_{k}+v_{k}$

其中：

$\phi_{k,k-1}^{*} = \phi_{k,k-1} -J_{k-1}H_{k-1};$

$w_{k-1}^{*}=\Gamma _{k-1}w_{k-1}-J_{k-1}v_{k-1}$

$E\left [ w_{k}^{*} \right ] = 0,Cov\left [ w_{k}^{*},w_{j} \right ] = Q_{k}\delta _{kj};$

$E\left [ v_{k} \right ] = 0,Cov\left [ v_{k},v_{j} \right ] =R_{k}\delta _{kj};$

$Cov\left [ w_{k}^{*},v_{j} \right ]=0$

$Q_{k}$ 位过程噪声的协方差，其为非负定阵；

$R_{k}$ 为测量噪声的协方差，其为正定阵；

则线性离散型卡尔曼滤波器的一般形式基本方程为；

(1)对k时刻状态的预测方程：

${\widehat{x}_{k/k-1} =\phi _{ k,k-1}^{*}*x_{k-1} +B_{k-1}u_{k-1}+ J_{k-1}Z_{k-1}$ (3-1)

(2)对k时刻协方差的预测方程：

${\widehat{P}_{k/k-1} =\phi _{ k,k-1}^{*}*P_{k-1}*(\phi _{ k,k-1}^{*})^{T} + \Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^{T} - J_{k-1}S_{k-1}^{T}\Gamma_{k-1}^{T}$ (3-2)

(3)滤波增益方程（权重）：

$K_{k}={\widehat{P}_{k/k-1}} * H_{k}^{T}\left [ H_{k} * {\widehat{P}_{k/k-1}} * H_{k}^{T} + R_{k} \right ]^{-1}$ (3-3)

(4)滤波估计方程（k时刻的最有估计值）：

${x}_{k} ={\widehat{x}_{k/k-1}}+B _{k-1}*u_{k-1} +J_{k-1}Z_{k-1}+ H_{k}\left [ Z_{k} -H_{k}* {\widehat{x}_{k/k-1} - H_{k}B_{k-1}u_{k-1}\right ]$ (3-4)

(5)滤波均方误差更新矩阵（k时刻最优均方误差）：

${P}_{k} =\left [ I - K_{k}*H_{k} \right ]*\widehat{P}_{k/k-1}$ (3-5)

Kalman滤波是一种递归过程，主要有两个更新过程：时间更新和观测更新，其中时间更新主要包括状态预测和协方差预测，主要是对系统的预测，而观测更新主要包括计算卡尔曼增益、状态更新和协方差更新，因此整个递归过程上面五个方面的计算：1）状态预测；2）协方差预测；3）卡尔曼增益；4）状态更新；5）协方差更新；

1、式(3-1)即线性随机差分方程(Linear Stochastic Difference equation)，其利用k-1时刻系统状态最优估计 $x_{k-1}$ 给出当前k时刻系统状态的估计值 ${\widehat{x}_{k/k-1}}$ ， $x_{k-1}$ 和 ${\widehat{x}_{k/k-1}}$ 都是n*1矩阵， $\phi _{ k,k-1}^{*}$ 是k-1时刻到k时刻的系统转换矩阵，大小位n*n矩阵， $u_{k-1}}$ 是系统输入，大小为m*1， $B_{k-1}}$ 位将输入转化为系统状态的矩阵，大小为n*m，线性随机差分方程假设上一状态与当前状态有某种线性关系，比如恒温环境的温度，匀速运动的速度等，但是因为现实环境的复杂，这种线性关系不是完全平滑的，也就是会有一些扰动，式中用 $w_{k-1} = J_{k-1}Z_{k-1}$ 表示扰动，该扰动是系统过程噪声，主要是从上一状态进入到当前状态时外界因素的干扰，通常假定服从高斯白噪声分布。这一公式的主要含义是： $x_{k-1}$ 是上一状态的最优结果， ${\widehat{x}_{k/k-1}$ 是利用上一状态预测得到的结果， $Z_{k-1}$ 是上一时刻的观测结果，这公式主要预测当前时刻系统状态结果。

2、式(3-2)

协方差预测：

$P_{k-1}$ 是上一状态 $x_{k-1}$ 对应的协方差， ${\widehat{P}_{k/k-1}$ 是 $x_{k}$ 时刻对应的协方差的预测值， $Q_{k-1}$ 是系统过程噪声（假设为高斯白噪声）的协方差矩阵；

这个公式主要含义：对系统结果对应的协方差进行更新；

3、式(3-3)

$H_{k}$ 为观测矩阵， $R_{k}$ 为测量噪声（同样假设为高斯白噪声）对应的协方差矩阵；主要含义：用于状态更新和协方差更新中，因为前两个预测我们得到了当前状态的预测结果，然后再得到当前状态的观测值，通过预测值和观测值，获得卡尔曼增益 $K_{k}$

4、式(3-4)

主要含义：利用当前状态的观测值和预测值，及增益，计算k时刻下的当前状态的最优状态估计 ${x}_{k}$ ，也就是状态更新。

5、式(3-5)

其中， $I$ 为单位矩阵；

主要含义：前面已经更新了状态，为了保证递推下去，还要更新当前状态对应的协方差矩阵，用于下一轮计算的公式（3-2）中的 $P_{k-1}$ ；

2.4、离散型卡尔曼滤波基本方程使用要点：

（1）滤波初值的选择

卡尔曼滤波是一种递推算法，启动时必须先给初值 $\widehat{x}_{0},P_{0}$ ;

情况一：一般情况下，取 $\widehat{x}_{0} = E[x_{0}], P_{0}=Cov\left [ x_{0} \right ]$ ，卡尔曼滤波是无偏的，即滤波稳定，但是实际上这样的初值很难得到；

情况二：如果系统是一致完全随机可控和一致完全随机可观测的，则卡尔曼滤波器一定是一致渐进稳定的，此时随意的选取滤波初值不影响最终估计值（大多数情况下）。

（2）估计均方误差的等价形式及选用

$P_{k}=(I-K_{k}H_{k})\widehat{P}_{k/k-1}$ ; (1)

$P_{k}=(I-K_{k}H_{k})\widehat{P}_{k/k-1}(I-K_{k}H_{k}) + K_{k}R_{k}K_{k}^{T};$ (2)

$P_{k}=(\widehat{P}_{k/k-1})^{-1}+H_{k}^{T}R_{k}^{-1}H_{K};$ (3)

公式（1）形式简单，计算量小，但是累积误差容易使协方差矩阵失去非负正定性甚至对称性，所以实际中常使用公式（2）；

如果在滤波初值对估计量的统计特性缺乏了解，选取滤波初值随意，则宜采用公式（3）.

（3）卡尔曼滤波的基本方程只适用于系统方程和测量方程均为离散的情况，但实际的物理系统一般都是连续的，动力学特性用连续微分方程来描述，所以在使用基本方程之前，需要对系统方程和测量方程进行离散化处理。连续系统的离散化处理包括对过程白噪声的等效离散化处理。

3、连续系统的卡尔曼滤波基本方程

通过对实际的物理系统进行分析后得到的系统模型一般为连续型的。连续型卡尔曼滤波方程可在离散型卡尔曼滤波器基本方程基础上推导出来，基本思路：将连续系统离散化，应用离散型卡尔曼滤波器的基本方程和导数概念推导出连续型滤波方程。

采用递推算法是离散型卡尔曼滤波的最大优点，算法可由计算机执行，不必存储时间过程中的大量测量信息，连续型卡尔曼滤波则根据连续时间过程中的测量值，采用求解矩阵微分方程的方法估计系统状态变量的时间连续值，因此算法失去了递推性。

连续系统的状态空间表达式为：

$x(t)=A(t)x(t)+B(t)u(t)+G(t)w(t)$

$z(t)=H(t)x(t)+v(t)$

其中：

$E[w(t)]=0,Cov[w(t),w(\gamma )]=Q(t)\delta (t-\gamma );$

$E[v(t)]=0,Cov[v(t),v(\gamma )]=R(t)\delta (t-\gamma );$

$Cov[w(t),v(\gamma )]=S(t)\delta (t-\gamma );$

Q(t)为非负定矩阵；R(t)为正定矩阵

与连续系统模型等效的离散系统的数学模型：

$x(t+\Delta t)=\Phi (t+\Delta t,t)x(t)+\Psi (t+\Delta t,t)u(t)+\Gamma (t+\Delta t,t)W(t);$

$z(t+\Delta t)=H(t+\Delta t)x(t+\Delta t)+V(t+\Delta t);$

其中： $\Phi (t+\Delta t,t)=I+A(t)\Delta t+o(\Delta t);$

$\Psi (t+\Delta t,t)=B(t)\Delta t+o(\Delta t);$

$\Gamma (t+\Delta t,t)=G(t)\Delta t+o(\Delta t);$

$W(t),V(t)$ 是零均值分段常值白噪声过程，其协方差为：

$Cov[W(t),W(\gamma )]=\frac{Q(t)}{\Delta t}\delta _{kj};Cov[V(t),V(\gamma )]=\frac{R(t)}{\Delta t}\delta _{ij};$

$Cov[W(t),V(\gamma )]=\frac{S(t)}{\Delta t}\delta _{ij};t=t_{0}+k\Delta t,\gamma =t_{0}+j\Delta t;k,j=0,1,2$

进入矩阵J来除去过程噪声与测量噪声的相关性

$J(t)=\Gamma (t+\Delta t/t)S(t)R^{-1}(t)$

(1)状态的一步预测方程：

$\widehat{x}(t+\Delta t/t)= \Phi (t+\Delta t, t)x(t)+\Psi (t + \Delta t,t)u(t)+J(t)(z(t)-H(t)x(t))$

(2)协方差预测方程：

$\widehat{P}(t+\Delta t/t)=[\Phi (t+\Delta t,t)-J(t)H(t)]P(t)[\Phi (t+\Delta t,t)-J(t)H(t)]^{T} + \Gamma (t+\Delta t,t)\frac{Q(t)}{\Delta t}\Gamma ^{T}(t+\Delta t,t)-J(t)\frac{R(t)}{\Delta t}J^{T}(t)$

(3)滤波增益方程（权重）：

$K(t+\Delta t)=\widehat{P}(t+\Delta t /t)H^{T}(t+\Delta t)[H(t+\Delta t/t)\widehat{P}(t+\Delta t/t)H^{T}(t+\Delta t)+\frac{R(t)}{\Delta t}]^{-1}$

(4)滤波估计方程（K时刻的最优值）：

$x(t+\Delta t)=x(t+\Delta t/t) + K(t+\Delta t)z(t+\Delta t/t)=x(t)+\left \{A(t)x(t)+B(t)u(t)+G(t)S(t)R^{-1}(t)[z(t)-H(t)x(t)] \right \}\Delta t + H(t+\Delta t) \left \{ z(t+\Delta t)-H(t+\Delta t)[x(t)+[A(t)x(t)+B(t)u(t)+B(t)u(t)+G(t)S(t)R^{-1}(t)(z(t)-H(t)x(t))]\Delta t] \right \}$

将其变形求极限

$x(t)=A(t)x(t)+B(t)u(t)+K_{s}(t)[z(t)-K(t)x(t)]$

$K_{s}=K(t)+G(t)S(t)R^{-1}(t)$

(5)滤波协方差更新矩阵（K时刻的最优协方差误差）：

$P(t+\Delta t)=[I-K(t+\Delta t)H(t+ \Delta t)]P(t+\Delta t/t)$

$P(t)=A(t)P(t)+P(t)A^{T}(t)+G(t)Q(t)G^{T}(t)-K_{s}(t)R^{-1}(t)K^{T}_{s}(t)$

普通卡尔曼滤波是在线性高斯情况下利用最小均方误差获得目标的动态估计，适用于过程和测量都属于线性系统，且误差符合高斯分布的系统，但是实际上很多系统都存在一定的非线性，表现在过程方程（状态方程）是非线性的，或者观测与状态之间的关系（测量方程）是非线性的。这种情况下就不能使用一般的卡尔曼滤波了，解决的方法是将非线性关系线性近似，将其转化为线性问题。

对于非线性问题线性化常用的两个途径：

（1）将非线性环节线性化，对高阶项采用忽略或者逼近措施；（EKF）

（2）用采样方法近似非线性分布；（UKF）

4、扩展卡尔曼滤波器（EKF）

非线性系统模型： $x(t)=f(x(t),t)+g(x(t),t)w(t)$

$z(t)=H(x(t),t)+v(t)$

其中：

$E[w(t)]=0,Cov[w(t),w(\gamma )]=Q(t)\delta (t-\gamma );$

$E[v(t)]=0,Cov[v(t),v(\gamma )]=R(t)\delta (t-\gamma );$

$Cov[w(t),v(\gamma )]=0$

假设在t时刻已经获得系统状态x的滤波估计 $\widehat{x}(t)$ ，将 $f(x(t),t)$ 和 $H(x(t),t)$ 在 $\widehat{x}(t)$ 附近线性化，即非线性系统将随时在新估计的结果附近进行线性化。

将 $f(x(t),t)$ 和 $H(x(t),t)$ 在 $x(t)=\widehat{x}(t)$ 附近展开成泰勒级数，忽略二阶以上的高阶项，则获得线性化方程为:

$x(t)=f(\widehat{x}(t),t)+\frac{\partial f(\widehat{x}(t),t)}{\partial \widehat{x}(t)}[x(t)-\widehat{x}(t)]+g(\widehat{x}(t),t)w(t)$

$z(t)=h(\widehat{x}(t),t)+\frac{H(\widehat{x}(t),t)}{\partial \widehat{x}(t)}[x(t)-\widehat{x}(t)] +v(t)$

将其变形，取 $F(t)=\frac{\partial f(\widehat{x}(t),t)}{\partial \widehat{x}(t)};K(t)=\frac{\partial H(\widehat{x}(t),t)}{\partial \widehat{x}(t)};G(t)=g(\widehat{x}(t),t)$