基于非理想条件下非线性网络系统的高斯滤波方法

摘要

基于非理想条件下非线性网络系统的高斯滤波方法，本发明涉及非线性网络系统的高斯滤波方法。本发明的目的是为了解决现有方法未考虑非线性网络系统中可能出现的相关噪声、一步随机延迟测量和数据丢包的问题，以及基于模型线性近似或者忽略延迟量测可能导致滤波器估计精度下降甚至发散的问题。基于非理想条件下非线性网络系统的高斯滤波方法过程为：步骤一、建立系统模型及传感器量测模型；步骤二、给出假设和引理；步骤三、基于步骤二设计高斯滤波器；步骤四、基于三阶球径容积法则，对步骤三中的高斯加权积分进行近似，得到设计滤波器的的数值形式。本发明可以应用于航天器及飞行器导航技术领域。

权利要求

1.基于非理想条件下非线性网络系统的高斯滤波方法，其特征在于：所述方法具体过程为：

步骤一、建立系统模型及传感器量测模型；

步骤二、给出假设和引理；

步骤三、基于步骤二设计高斯滤波器；

步骤四、基于三阶球径容积法则，对步骤三中的高斯加权积分进行近似，得到设计滤波器的的数值形式；

所述步骤一中建立系统模型及传感器量测模型；具体过程为：建立带有相关噪声的非线性离散时间系统模型：xk+1＝f(xk)+ωk                          (7)建立非线性量测模型：

zk＝h(xk)+υk                           (8)n n

式中，xk+1为k+1时刻的系统状态，xk为k时刻的系统状态，xk,xk+1∈R ，R为n维实数空间；

m m

zk是k时刻传感器模型，zk∈R ，R为m维实数空间；f(·)和h(·)为已知的非线性函数；ωk∈n m

R和υk∈R是相关的零均值高斯白噪声并且协方差为式中，δkl是Kronecker delta函数，Qk和Rk分别为过程噪声和量测噪声协方差，Sk为互协n m

方差，l为l时刻，ωl∈R和υl∈R是相关的零均值高斯白噪声；

考虑通信带宽、延迟测量及数据丢包，非线性量测模型进一步建立为：式中，zk是k时刻传感器模型；zk‑1是k‑1时刻传感器模型；zk|k‑1是当zk丢失时的补偿量，为k时刻的量测值一步预测；γk和ηk是不相关伯努利分布变量并且满足P为概率； 为中间变量；yk是存在量测时滞和数据包丢失的k时刻传感器模型；

所述步骤二中给出假设和引理；具体过程为：假设1.假设ωk,υk,γk和ηk与x0不相关，且x0满足T

式中，x0为初值， 为初值的估计值，E[ ]为求期望，(·) 为 T为转置， 为初值对应协方差；

引理1.A＝[aij]n×n是实值矩阵，B＝diag{b1,…,bn}和C＝diag{c1,…,cn}是对角随机矩阵，定义

式中，aij为A矩阵的第i行第j列元素，[aij]n×n为第i行第j列元素为aij的n×n矩阵，diag表示将其后面的元素排列为对角阵，b1为矩阵对角线第1个元素，bn为矩阵对角线第n个元T T

素，c1为矩阵对角线第1个元素，cn为矩阵对角线第n个元素，E{BAC}为先对矩阵BAC做乘法运算然后求期望，⊙是Hadamard积；

增广系统为 然后，有

式中，Xk+1为增广系统， φ和 均为中间变量；

其中， 且

式中，I为单位矩阵，0为零矩阵， 为φ的期望，E[φ]为对φ求期望， 为 的期望，为对 求期望；

所述步骤三中设计高斯滤波器；具体过程为：一步预测均值和协方差矩阵分别给出如下Xk+1|k＝Xk+1|k‑1+Kkεk                         (1)式中，Xk+1|k‑1为两步预测， 为两步预测协方差矩阵，εk为新息， 为新息协方差矩阵，T为转置，Kk为增益矩阵，定义如下式中， 为在k‑1时刻量测条件下Xk+1与εk的互协方差矩阵；

量测修正后的均值和协方差矩阵如下：Xk+1|k+1＝Xk+1|k+Mk+1εk+1                        (4)式中， 为新息协方差矩阵，εk+1为新息，Mk+1为增益矩阵；

式中， 为在k时刻量测条件下Xk+1与εk+1的互协方差矩阵；

所述步骤四中基于三阶球径容积法则，对步骤三中的高斯加权积分进行近似，得到设计滤波器的的数值形式；具体过程为：预测:

1、分解

式中， 为k时刻状态的一步预测协方差矩阵，Mk|k‑1为中间变量， 为k时刻增广系统 的一步预测协方差矩阵， 为中间变量；

在N1中，令

式中，N1为一个高斯分布，Γk,k‑1|k‑1为中间变量，是构造的状态xk与噪声υk‑1基于k‑1时刻量测的估计，xk|k‑1为k时刻状态的一步预测，υk‑1|k‑1为k‑1时刻量测噪声的估计值，Πk,k‑1|k‑1为中间变量， 为k时刻状态一步预测协方差矩阵， 为状态xk与噪声υk‑1基于k‑1时刻量测的互协方差矩阵， 为k‑1时刻量测噪声的协方差矩阵，Mk,k‑1|k‑1为中间变量；

2、计算容积点

xi,k|k‑1＝Mk|k‑1ξi+xk|k‑1,i＝1,…2n                    (88)式中，xi,k|k‑1为k时刻关于一步预测量的容积点，ξi、ζi、为sigma点，且维数分别为2n、

4n和2(n+m)； 为联合估计中关于分量xk|k‑1的容积点， 为联合估计中关于分量xk‑1|k‑1的容积点，Xi,k|k‑1为k时刻增广系统联合状态 的一步预测容积点，δi,k,k‑1|k‑1为对应Γk,k‑1|k‑1中xk|k‑1部分的第i个容积点，γi,k,k‑1|k‑1为对应Γk,k‑1|k‑1中υk‑1|k‑1部分的第i个容积点，Mk,k‑1|k‑1为中间变量，Γk,k‑1|k‑1为中间变量，n为系统状态维数，m为量测噪声维数；

3、计算传播的容积点

式中， 为xi,k|k‑1经系统模型传播后的容积点， 为xi,k|k‑1经量测模型传播后的容积点，h(·)为已知的非线性函数， 为 经系统模型传播后的容积点，f(·)为已知的非线性函数， 为 经量测模型传播后的容积点， 为 经系统模型传播后的容积点；

定义

式中，l1、l2、l3、l4、l5、l6、l7、l8、l9、l10、l11、l12、l13、l14、l15为中间变量；

4、计算式1、式2中相应的量

其中，

将(103)‑(104)带入 得对于在上一时刻获得的εk和 带入(1)‑(2)，直接获得Xk+1|k和式中，xk|k‑1为k时刻状态的一步预测均值， 为k时刻状态的一步预测协方差，为在k‑1时刻量测条件下状态xk+1与k时刻新息的互协方差矩阵， 为在k‑1时刻量测条件下状态xk与k时刻新息的互协方差矩阵，Qk为k时刻过程噪声；

修正:

1、分解

式中， 为k+1时刻状态一步预测协方差矩阵，Mk+1|k为中间变量， 为k时刻状态估计协方差矩阵，Mk|k为中间变量， 为k+1时刻增广系统联合状态 的一步预测协方差矩阵， 为中间变量；

在N2中，令

式中，N2为一个高斯分布，Γk+1,k|k为中间变量，Πk+1,k|k为Γk+1,k|k对应的协方差矩阵，为k+1时刻一步预测互协方差矩阵， 为在k时刻量测条件下状态xk+1与量测噪声υk的互协方差矩阵， 为k时刻量测噪声的后验协方差矩阵；

2、计算容积点

xi,k+1|k＝Mk+1|kξi+xk+1|k,i＝1,…2n                  (109)xi,k|k＝Mk|kξi+xk|k,i＝1,…2n                     (110)式中，xi,k+1|k为k+1时刻状态量一步预测容积点，xk+1|k为k+1时刻状态量一步预测，xi,k|k为k时刻状态量估计值对应积点，xk|k为k时刻状态量估计值，Xi,k+1|k为k+1时刻增广系统联合状态量 一步预测容积点， 为k+1时刻增广系统联合状态量对应的xk+1|k部分的容积点， 为k+1时刻增广系统联合状态量对应的xk|k部分的容积点， 为中间变量，Xk+1|k为k+1时刻联合状态量一步预测，δi,k+1,k|k为联合量 对应的xk+1|k部分的容积点，γi,k+1,k|k为联合量 对应的υk|k部分的容积点，Mk+1,k|k为中间变量，Γk+1,k|k为中间变量；

3、计算传播的容积点

式中， 为xi,k+1|k经系统模型传播的容积点， 为xi,k|k经系统模型传播的容积点，为xi,k+1|k经量测模型传播的容积点， 为xi,k|k经量测模型传播的容积点， 为经量测模型传播的容积点， 为 经量测模型传播的容积点， 为δi,k+1,k|k经量测模型传播的容积点；

定义

式中，l′1、l′2、l′3、l′4、l′5、l′6、l′7、l′8、l′9、l′10、l′11、l′12、l′13、l′14、l′15、l′16、l′17为中间变量；

4、计算式4、式5、式6中相应量式中，yk+1为存在量测时滞和数据包丢失的k+1时刻传感器模型；

式中， 为k+1时刻新息协方差矩阵，Φ为中间变量，⊙为Hadamard积，Ψ为中间变量，Ξ为中间变量， 为在k时刻量测下状态xk与k+1时刻新息的互协方差矩阵， 为在k时刻量测下k+1时刻状态与k+1时刻新息的互协方差矩阵，Sk为k时刻互相关噪声，ωk|k为k时刻量测过程噪声ωk的估计值；

将(127)‑(128)带入 得由(125)‑(126)，得εk+1和将εk+1和 带入(43)‑(44)，获得Xk+1|k+1和Xk+1|k+1＝Xk+1|k+Mk+1εk+1                      (43)

说明书

基于非理想条件下非线性网络系统的高斯滤波方法

技术领域

[0001] 本发明涉及非线性网络系统的高斯滤波方法，具体涉及一种针对带有相关噪声、一步随机延迟测量和数据丢包的非线性系统的状态估计方法。

背景技术

[0002] 近几年，网络系统的估计问题引起了广泛的关注[1‑3]([1]L.Schenato，“Optimal estimation in networked control systems subject to random delay and packet 
drop，”IEEE transactions on automatic control，vol.53，no.5，pp.1311，2008.[2]
W.A.Zhang，L.Yu，G.Feng，“Optimal linear estimation for networked systems with 
communication constraints，”Automatica，vol.47，no.9，pp.1992‑2000，2011.[3]
R.Caballero‑ A.Hermoso‑Carazo，J.Linares‑Pérez，“Optimal state estimation 
for networked systems with random parameter matrices，correlated noises and 
delayed measurements，”International Journal of General Systems，vol.44，no.2，
[4]
pp.142‑154，2015.)。卡尔曼滤波器(KF) (R.E.Kalman，“A new approach to linear 
filtering and prediction problems，”Journal of basic Engineering，vol.82，no.1，
pp.35‑45，1960.)是解决状态估计问题的有效方法。但是，它只适用于理想的线性系统。实
际上，非线性，相关噪声，随机延迟和数据包丢失无处不在。在本文中，考虑具有同步相关噪
声和一步随机延迟测量和多包丢失的非线性网络系统的估计问题。

[0003] 对于非线性系统，有许多估计方法。对于具有弱非线性的系统，基于一阶泰勒级数[5]
展开，扩展的KF(EKF)可以达到可接受的精度 (Y.Bar‑Shalom，X.R.Li，T.Kirubarajan，
“Estimation with applications to tracking and navigation:theory algorithms 
and software，”John Wiley&Sons，2004.)。与EKF算法需要计算雅可比矩阵不同，差分滤波
[6]
器(DDF) (M. N.K.Poulsen，O.Ravn，“New developments in state estimation 
for nonlinear systems，”Automatica，vol.36，no.11，pp.1627‑1638，2000.)使用斯特林
插值逼近非线性函数，克服了EKF算法可能陷入局部线性化的问题。基于近似概率分布比近
[7]
似任意非线性函数或变换更容易的事实，提出了一系列sigma点滤波器，如粒子滤波器(PF)
(A.Doucet，S.Godsill，C.Andrieu，“On sequential Monte Carlo sampling methods for 
Bayesian filtering，”Statistics and computing，vol.10，no.3，pp.197‑208，2000.)，无
[8]
迹KF(UKF) (S.J.Julier，J.K.Uhlmann，“Unscented filtering and nonlinear 
estimation，”Proceedings of the IEEE，vol.92，no.3，pp.401‑422，2004.)和容积KF
[9]
(CKF) (I.Arasaratnam，S.Haykin，“Cubature kalman filters，”IEEE Transactions on 
automatic control，vol.54，no.6，pp.1254‑1269，2009.)。同时，为了适应不同的应用场
景，提出了上述算法的许多改进版本。例如，基于二阶泰勒展开的EKF，无迹PF，具有二阶斯
特林插值的DDF，自适应UKF，高阶CKF，平方根CKF等。由于UKF和CKF具有更简洁的形式且CKF
算法的数值稳定性远高于UKF，因此CKF已广泛应用于实际工程领域，如目标跟踪和移动终
端定位。

[0004] 对于具有相关噪声的系统，在[10](X.X.Wang，Y.Liang，Q.Pan，et al.，“A Gaussian approximation recursive filter for nonlinear systems with correlated 
noises，”Automatica，vol.48，no.9，pp.2290‑2297，September，2012.)中，重新构造了一个
[11]
新的伪过程方程，其中过程噪声与观测噪声不相关。在 (X.X.Wang，Y.Liang，Q.Pan，et 
al.，“Design and implementation of Gaussian filter for nonlinear system with 
randomly delayed measurements and correlated noises，”Applied Mathematics and 
Computation，vol.232，pp.1011‑1024，2014.)中，基于投影定理，提出了一种非线性系统的
GASF。随后，[11]中的方法用于设计具有随机延迟测量和相关噪声的非线性系统的高斯滤
[12]
波器 (G.B.Chang，“Comments on“A Gaussian approximation recursive filter for 
nonlinear systems with correlated noises，”Automatica，vol.50，no.2，pp.655‑656，
[13]
February，2014.)。在 (G.B.Chang，“Alternative formulation of the Kalman filter 
for correlated process and observation noise，”IET Science，Measurement&
Technology，vol.8，no.5，pp.310‑318，September，2014.)中，[13]和[14]中的这两个滤波
[14]
算法在线性系统中被证明在理论上是等价的。在 (H.Yu，X.J.Zhang，S.Wang，et al.，
“Alternative framework of the Gaussian filter for non‑linear systems with 
synchronously correlated noises，”IET Science，Measurement&Technology，vol.10，
no.4，pp.306‑315，July，2016.)中，基于状态增强和条件高斯分布，开发了两个替代框架，
并证明了[14]和[10‑11]中算法的等价性。然后，在[15](K.Zhao，S.M.Song，“Alternative 
framework of the Gaussian filter for non‑linear systems with randomly delayed 
measurements and correlated noises，”IET Science，Measurement&Technology，
vol.12，no.2，pp.161‑168，2017.)和[16](S.L.Sun，L.Xie，W.Xiao，et al.，“Optimal 
linear estimation for systems with multiple packet dropouts，”Automatica，
vol.44，no.5，pp.1333‑1342，May，2008.)中，条件高斯分布的替代框架被扩展到具有相关
噪声的非线性系统和具有相关噪声和一步延迟测量的非线性系统。

[0005] 对于具有一步随机延迟测量的系统，通常使用伯努利分布随机变量来描述延迟。对于具有多数据包丢失的系统，分别提出了零输入补偿，保持输入补偿和预测补偿。对于从
传感器发送到滤波器的观测数据包的问题，在一个采样周期内到达滤波器的一个和多个数
据包分别在[17](S.L.Sun“,Optimal linear filters for discrete‑time systems with 
randomly delayed and lost measurements with/without time stamps,”IEEE 
Transactions on Automatic Control,vol.58,no.6,pp.1551‑1556,June,2013.)和[18]
(S.L.Sun,G.H.Wang,“Modeling and estimation for networked systems with 
multiple random transmission delays and packet losses,”Systems&Control 
Letters,vol.73,pp.6‑16,November,2014.)中讨论。在[19](S.L.Sun,J.Ma“, Linear 
estimation for networked control systems with random transmission delays and 
packet dropouts,”Information Sciences,vol.269,pp.349‑365,June,2014.)中，考虑了
更一般的情况，其中数据包分别从传感器和控制器传输到滤波器和执行器。在上述情况下，
当发生丢包时，测量无效。为了避免这种情况，开发了保持输入补偿机制。在[20](Y.Liang,
T.W.Chen,Q.Pan,“Optimal linear state estimator with multiple packet 
dropouts,”IEEE Transactions on Automatic Control,vol.55,no.6,pp.1428‑1433,
June,2010.)中，在存在多个数据包丢失的情况下，保持输入补偿用于最佳线性估计问题。
在[21](J.Ma,S.L.Sun,“Information fusion estimators for systems with multiple 
sensors of different packet dropout rates,”Information Fusion,vol.12,no.3,
pp.213‑222,July,2011.)中，在线性最小方差(LMV)意义上的集中式和分布式融合估计问
题中考虑了相同的方法。在[22](S.L.Sun,L.Xie,W.Xiao,et al.,“Optimal linear 
estimation for systems with multiple packet dropouts,”Automatica,vol.44,no.5,
pp.1333‑1342,May,2008.)中，基于一种新颖的数据包丢失模型，开发了最优线性估计器。
但是，保持输入补偿不考虑系统的最新信息，因此提出了一种新的补偿机制，其中最新观测
的预测值被用作补偿。在[23](J.Ding,S.L.Sun,J.Ma,N.Li“, Fusion Estimation for 
Multi‑Sensor Networked Systems with Packet Loss Compensation,”Information 
Fusion,vol.45,pp.138‑149,January,2019.)中，基于新的补偿机制，设计了集中式和分布
式融合估计器。在[24](E.I.Silva,M.A.Solis“,An alternative look at the constant‑
gain Kalman filter for state estimation over erasure channels,”IEEE 
Transactions on Automatic Control,vol.58,no.12,pp.3259‑3265,December,2013.)
中，考虑了一类切换估计器，其中缺失数据被最佳估计所取代。

[0006] 值得一提的是，在[25‑27]([25]J.Ma,S.L.Sun,“Linear estimators for networked systems with one‑step random delay and multiple packet dropouts 
based on prediction compensation,”IET Signal Processing,vol.11,no.2,pp.197‑
204,April,2017.[26]C.Zhu,Y.Xia,L.Xie,et al.“, Optimal linear estimation for 
systems with transmission delays and packet dropouts,”IET signal Processing,
vol.7,no.9,pp.814‑823,December,2013.[27]S.L.Sun“, Optimal linear estimators 
for discrete‑time systems with one‑step random delays and multiple packet 
dropouts,”Acta Automatica Sinica,vol.38,no.3,pp.349‑354,March,2012.)中，在三种
不同的模型中同时考虑了一步随机延迟测量和多个数据包丢失。在[27]中，通过引入两个
不相关的伯努利随机变量来描述可能的数据包丢失，延迟测量及补偿值，其确保系统可以
随时接收上述三个量中的一个作为测量值，以便保持滤波算法的准确性。然而，在[27]中，
它没有考虑延迟测量和实时测量同步到达的情况。在[26]中，测量模型进行了改变，因为相
邻测量可能同时到达，其中在数据处理中心的最近时刻接收的测量被用作当前时期的补偿
值，并与[27]相比，[26]中算法的优越性已在[26]中显示。此外，在[25]中，通过改变补偿机
制，其中当前时期的测量而不是数据处理中心接收的最新测量的一步预测被用作补偿器，
提出了新的测量模型。已经证实，与[26]相比，[25]中提出的算法具有更高的估计精度和更
小的计算负担，因为[25]中的模型总是使用最新的测量信息。

[0007] 由于现有方法没有考虑到在非线性系统中数据处理中心可以同时接收两个相邻测量，导致实际系统处理此类问题时可能出现信息损失，从而影响系统的估计精度，甚至有
可能导致滤波器发散。

发明内容

[0008] 本发明的目的是为了解决现有方法未考虑非线性网络系统中可能出现的相关噪声、一步随机延迟测量和数据丢包的问题，以及基于模型线性近似或者忽略延迟量测可能
导致滤波器估计精度下降甚至发散的问题，而提出基于非理想条件下非线性网络系统的高
斯滤波方法。

[0009] 基于非理想条件下非线性网络系统的高斯滤波方法具体过程为：

[0010] 步骤一、建立系统模型及传感器量测模型；

[0011] 步骤二、给出假设和引理；

[0012] 步骤三、基于步骤二设计高斯滤波器；

[0013] 步骤四、基于三阶球径容积法则，对步骤三中的高斯加权积分进行近似，得到设计滤波器的的数值形式。

[0014] 本发明的有益效果为：

[0015] 本发明提出了一种具有同步相关噪声、一步随机延迟和多个数据包丢失的非线性网络系统的状态估计方法，考虑系统为一般非线性系统，针对系统中可能出现的同步相关
噪声、一步随机延迟测量和数据包丢失，设计了高斯递归滤波算法，可以确保系统获得高精
度的估计值，避免了系统的发散，确保了系统的稳定。

附图说明

[0016] 图1为本发明算法与文献[23]中算法关于UNGM模型中状态的估计情况图；

[0017] 图2为本发明算法与文献[23]中算法关于UNGM模型中状态的估计均方根误差情况图；

[0018] 图3为本发明算法与文献[23]中算法关于UNGM模型中状态的估计误差情况图；

[0019] 图4为本发明算法与文献[23]中算法关于一个强非线性模型中状态的估计均方根误差情况图；

[0020] 图5为本发明算法与文献[23]中算法关于一个强非线性模型中状态的估计误差情况图。

具体实施方式

[0021] 具体实施方式一：本实施方式基于非理想条件下非线性网络系统的高斯滤波方法具体过程为：

[0022] 步骤一、建立系统模型及传感器量测模型；

[0023] 步骤二、给出假设和引理；

[0024] 步骤三、基于步骤二设计高斯滤波器；

[0025] 步骤四、基于三阶球径容积法则，对步骤三中的高斯加权积分进行近似，得到设计滤波器的的数值形式。

[0026] 具体实施方式二：本实施方式与具体实施方式一不同的是，所述步骤一中建立系统模型及传感器量测模型；具体过程为：

[0027] 建立带有相关噪声的非线性离散时间系统模型：

[0028] xk+1＝f(xk)+ωk  (7)

[0029] 建立一般的非线性量测模型：

[0030] zk＝h(xk)+υk  (8)

[0031] 式中，xk+1为k+1时刻的系统状态，xk为k时刻的系统状态，xk,xk+1∈Rn，Rn为n维实数m m
空间；zk是k时刻传感器模型，zk∈R ，R 为m维实数空间；f(·)和h(·)为已知的非线性函
n m
数；ωk∈R和υk∈R是相关的零均值高斯白噪声并且协方差为

[0032]

[0033] 式中，δkl是Kronecker delta函数，Qk和Rk分别为过程噪声和量测噪声协方差，Sk为n m
互协方差，l为l时刻，ωl∈R和υl∈R是相关的零均值高斯白噪声；

[0034] 在本发明中，考虑以下情况：在数据传输期间可能发生一步随机延迟和数据包丢失；数据包只发送一次；估算器可以同时接收最多两个测量数据。也就是说，估计器可以接
收零，一个或两个数据包。实际上，可以通过以下模型描述测量方程。考虑通信带宽、延迟测
量及数据丢包，一般的非线性量测模型进一步建立为：

[0035]

[0036] 式中，zk是k时刻传感器模型；zk‑1是k时刻传感器模型；zkk‑1是当zk丢失时的补偿量，为k时刻的量测值一步预测；γk和ηk是不相关伯努利分布变量并且满足
P为概率； 为中间变量；yk是存在量测时滞和数据
包丢失的k时刻传感器模型。

[0037] 其它步骤及参数与具体实施方式一相同。

[0038] 具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述步骤二中给出假设和引理；具体过程为：

[0039] 给出相应的假设和引理，假设是滤波器设计的前提，引理是为了方便滤波器推导；

[0040] 假设1.假设ωk,υk,γk和ηk与x0不相关，且x0满足

[0041]

[0042] 式中，x0为初值， 为初值的估计值，E[]为求期望，(·)T为 T为转置，为初值对应协方差；

[0043] 引理1.A＝[aij]n×n是实值矩阵，B＝diag{b1,…,bn}和C＝diag{c1,…,cn}是对角随机矩阵，定义

[0044]

[0045] 式中，aij为A矩阵的第i行第j列元素，[aij]n×n为第i行第j列元素为aij的n×n矩阵，diag表示将其后面的元素排列为对角阵，b1为矩阵对角线第1个元素，bn为矩阵对角线第n个
T T
元素，c1为矩阵对角线第1个元素，cn为矩阵对角线第n个元素，E{BAC}为先对矩阵BAC 做乘
法运算然后求期望，⊙是Hadamard积，对任意的维数相同的矩阵M和N，定义[M⊙N]ij＝Mij·
Nij，显然，对应公式(12)中，即使A包含不确定性，(12)依然满足只要A和B，C不相关，但是A需
要被E[A]代替；

[0046] 为了后文推导方便，增广系统为 然后，有

[0047]

[0048]

[0049] 式中，Xk+1为增广系统， φ和 均为中间变量；

[0050] 其中， 且

[0051]

[0052]

[0053] 式中，I为单位矩阵，0为零矩阵， 为φ的期望，E[φ]为对φ求期望， 为 的期望， 为对 求期望；

[0054] a⊥b意味T
着a不相关于b。(a)(·) 中·代表与a相同的量。T代表转秩当作为上标时。Yk＝L{yk,
yk‑1,…,y1}，这里L{·}代表由·张成的线性空间。0和I代表合适维度的零矩阵和单位矩
阵。E[·]中E代表数学期望。E[a|b]代表a在b条件下的条件期望。P代表协方差矩阵。N(·)
代表分布函数。∫代表积分运算。

[0055] 其它步骤及参数与具体实施方式一或二相同。

[0056] 具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述步骤三中设计高斯滤波器；具体过程为：

[0057] 一步预测均值和协方差矩阵分别给出如下

[0058] Xk+1|k＝Xk+1|k‑1+Kkεk  (1)

[0059]

[0060] 式中，Xk+1|k‑1为两步预测， 为两步预测协方差矩阵，εk为新息， 为新息协方差矩阵，T为转置，Kk为增益矩阵，定义如下

[0061]

[0062] 式中， 为在k‑1时刻量测条件下Xk+1与εk的互协方差矩阵；

[0063] 量测修正后的均值和协方差矩阵如下：

[0064] Xk+1|k+1＝Xk+1|k+Mk+1εk+1  (4)

[0065]

[0066] 式中， 为新息协方差矩阵，εk+1为新息，Mk+1为增益矩阵；

[0067]

[0068] 式中， 为在k时刻量测条件下Xk+1与εk+1的互协方差矩阵；

[0069] A.预测

[0070] 定理1.对于系统(13)‑(14)，一步预测平均值和协方差如下

[0071] Xk+1|k＝Xk+1|k‑1+Kkεk  (17)

[0072]

[0073] Xk+1|k‑1和 在(20)和(24)中给出；

[0074] 新息εk和协方差 在上一时刻已经给出；

[0075] 证明:

[0076] 证明过程分为两部分：

[0077] 1)计算两步预测均值和协方差。

[0078] 根据GASF，有

[0079] Xk+1|k＝Xk+1|k‑1+Kkεk  (19)

[0080] 因为ωk⊥L{yk‑1,…,y1},因此,E[ωk|Yk‑1]＝0,所以

[0081]

[0082] 将(20)带入(19)，可得(17)；

[0083] 接下来计算 由(19)可得

[0084]

[0085] 基于协方差的定义和(21)，有

[0086]

[0087] 重写Xk+1|k‑1并且与Xk+1相减,可得

[0088]

[0089] 根据协方差的定义，有

[0090]

[0091] 这里

[0092]

[0093]

[0094] 将(24)代入(22)，得到(18)。

[0095] 2)计算增益矩阵。

[0096] 增益矩阵定义为

[0097]

[0098] 这里，

[0099]

[0100] 接下来计算 和

[0101] 首先，计算 使用(46)中εk+1的定义，对于εk，有

[0102]

[0103] 这里

[0104]

[0105]

[0106]

[0107]

[0108] 在(31)中，N1如下

[0109]

[0110] 在N1中,除 外所有量是已知的， 计算如下，

[0111]

[0112] 这里，ωk‑1|k‑1和υk‑1|k‑1计算如下.

[0113] yk‑1和ωk‑1的联合分布可得如下

[0114]

[0115] 根据条件高斯分布引理，得到

[0116]

[0117]

[0118] 对于υk‑1,同上可得

[0119]

[0120]

[0121] 值得注意的是εk‑1＝yk‑1‑yk‑1|k‑2和 已经在k‑1时刻计算了；

[0122] 然后计算

[0123]

[0124] 这里，

[0125]

[0126]

[0127]

[0128]

[0129] 将(27)和(38)带入(26)并且将(26)带入(25)，可得Kk；

[0130] 证明结束.

[0131] B.修正

[0132] 定理2.对于系统(13)‑(14)，高斯滤波器的均值和协方差的状态校正如下：

[0133] Xk+1|k+1＝Xk+1|k+Mk+1εk+1                   (43)

[0134]

[0135] 其中，

[0136]

[0137]

[0138] 式中，yk+1为存在量测时滞和数据包丢失的k+1时刻传感器模型，N(·)为分布函数，xk+1|k为k+1时刻状态xk+1的一步预测值， 为状态xk+1的一步预测协方差，xkk为k时刻
状态xk的估计值， 为k时刻状态xk的协方差矩阵，υk|k为k时刻量测噪声υk的估计值；

[0139] 证明过程分为两部分：

[0140] 1)计算新息εk+1和协方差

[0141] 根据新息εk+1的定义，有

[0142] εk+1＝yk+1‑yk+1|k  (47)

[0143] 这里

[0144]

[0145] 在(48)中，xk+1|k, xkk和 已经获得过.将(48)带入(47)，可得(46).

[0146] 重写yk+1|k,有

[0147]

[0148] 这里υk|k计算过程同(36)中的υk‑1|k‑1。然后，

[0149]

[0150] 显然有

[0151]因此

[0152]

[0153] 为了计算 根据引理1，定义

[0154] E[φAφT]＝Φ⊙E[A]  (52)

[0155]

[0156]

[0157]

[0158] 令 然后

[0159]

[0160]

[0161]

[0162]

[0163] 通过一系列代数运算，得到了

[0164]

[0165] 在下文中，计算(51)右侧的项。

[0166] Item 1.

[0167]

[0168] 这里，

[0169]

[0170]

[0171]

[0172]

[0173] Item 2.

[0174]

[0175] 这里， 且 可得 and

[0176]

[0177] 这里N2与N1形式相同,但N1中的k需要被替换为k+1，

[0178] Item 3.

[0179]

[0180] Item 4.

[0181]

[0182] Item 5.

[0183]

[0184] Item 6.

[0185]

[0186] Item 7.

[0187]

[0188] Item 8.

[0189]

[0190] Item 9.

[0191]

[0192] Item 10.

[0193] 因为υk+1与υk不相关，可得

[0194]

[0195] Item 11.

[0196]

[0197] Item 12.

[0198]

[0199] Item 13.

[0200]

[0201] 将(61),(66),(68)‑(78)带入(51),可得

[0202] 2)计算增益矩阵Mk+1.

[0203] 由(45)，只需计算

[0204]

[0205] 这里， 可以类似 求得.

[0206]

[0207] 这里，

[0208]

[0209]

[0210]

[0211]

[0212] 将(82)‑(84)带入(81)，可得 将 和 带入(79)，可得

[0213] 证明完成。

[0214] 其它步骤及参数与具体实施方式一至三之一相同。

[0215] 具体实施方式五：本实施方式与具体实施方式一至四之一不同的是，所述步骤四中基于三阶球径容积法则，对步骤三中的高斯加权积分进行近似，得到设计滤波器的的数
值形式；具体过程为：

[0216] 基于球径容积法则算法，本发明提出的算法的数值实现如下所示。

[0217] 预测:

[0218] 1、分解

[0219]

[0220]

[0221] 式中， 为k时刻状态的一步预测协方差矩阵，Mk|k‑1为中间变量， 为k时刻增广系统 的一步预测协方差矩阵， 为中间变量；

[0222] 在N1中，令

[0223]

[0224]

[0225]

[0226] 式中，N1为一个高斯分布，Γk,k‑1|k‑1为中间变量，是构造的状态xk与噪声υk‑1基于k‑1时刻量测的估计，xk|k‑1为k时刻状态的一步预测，υk‑1|k‑1为k‑1时刻量测噪声的估计值，
Πk,k‑1|k‑1为中间变量， 为k时刻状态一步预测协方差矩阵， 为状态xk与噪声υk‑1
基于k‑1时刻量测的互协方差矩阵， 为k‑1时刻量测噪声的协方差矩阵，Mk,k‑1|k‑1为中
间变量；

[0227] 2、计算容积点

[0228] xi,k|k‑1＝Mk|k‑1ξi+xk|k‑1,i＝1,…2n               (88)

[0229]

[0230]

[0231] 式中，xi,k|k‑1为k时刻关于一步预测量的容积点，ξi、ζi、 为sigma点，且维数分别为2n、4n和2(n+m)； 为联合估计中关于分量xk|k‑1的容积点， 为联合估计中关于
分量xk‑1|k‑1的容积点，Xi,k|k‑1为k时刻增广系统联合状态 的一步预测容积
点，δi,k,k‑1|k‑1为对应Γk,k‑1|k‑1中xk|k‑1部分的第i个容积点，γi,k,k‑1|k‑1为对应Γk,k‑1|k‑1中
υk‑1|k‑1部分的第i个容积点，Mk,k‑1|k‑1为中间变量，Γk,k‑1|k‑1为中间变量，n为系统状态维数，
m为量测噪声维数；

[0232] 3、计算传播的容积点

[0233]

[0234]

[0235]

[0236] 式中， 为xi,k|k‑1经系统模型传播后的容积点， 为xi,kk‑1经量测模型传播后的容积点，h(·)为已知的非线性函数， 经系统模型传播后的容积点，f
(·)为已知的非线性函数， 为 经量测模型传播后的容积点， 为
δi,k,k‑1|k‑1经系统模型传播后的容积点；

[0237] 定义

[0238]

[0239]

[0240]

[0241]

[0242]

[0243]

[0244]

[0245] 式中，l1、l2、l3、l4、l5、l6、l7、l8、l9、l10、l11、l12、l13、l14、l15为中间变量；

[0246] 4、计算式1、式2中相应的量

[0247]

[0248]

[0249] 其中，

[0250]

[0251]

[0252] 这里ωk‑1|k‑1和υk‑1|k‑1在(34)和(36)中给出；

[0253] 将(103)‑(104)带入 得

[0254] 对于在上一时刻获得的εk和 带入(1)‑(2)，直接获得Xk+1|k和

[0255] 式中，xk|k‑1为k时刻状态的一步预测均值， 为k时刻状态的一步预测协方差，为在k‑1时刻量测条件下状态xk+1与k时刻新息的互协方差矩阵， 为在k‑1时刻量
测条件下状态xk与k时刻新息的互协方差矩阵，Qk为k时刻过程噪声；

[0256] 修正:

[0257] 1、分解

[0258]

[0259]

[0260]

[0261] 式中， 为k+1时刻状态一步预测协方差矩阵，Mk+1|k为中间变量， 为k时刻状态估计协方差矩阵，Mk|k为中间变量， 为k+1时刻增广系统联合状态
的一步预测协方差矩阵， 为中间变量；

[0262] 在N2中，令

[0263]

[0264]

[0265] 式中，N2为一个高斯分布，Γk+1,k|k为中间变量，Πk+1,k|k为Γk+1,k|k对应的协方差矩阵， 为k+1时刻一步预测互协方差矩阵， 为在k时刻量测条件下状态xk+1与量测噪
声υk的互协方差矩阵， 为k时刻量测噪声的后验协方差矩阵；

[0266] 2、计算容积点

[0267] xi,k+1|k＝Mk+1|kξi+xk+1|k,i＝1,…2n     (109)

[0268] xi,k|k＝Mk|kξi+xk|k,i＝1,…2n     (110)

[0269]

[0270]

[0271] 式中，xi,k+1|k为k+1时刻状态量一步预测容积点，xk+1|k为k+1时刻状态量一步预测，xi,k|k为k时刻状态量估计值对应积点，xk|k为k时刻状态量估计值，Xi,k+1|k为k+1时刻增广系
统联合状态量 一步预测容积点， 为k+1时刻增广系统联合状态量
对应的xk+1|k部分的容积点， 为k+1时刻增广系统联合状态量
对应的xk|k部分的容积点， 为中间变量，Xk+1|k为k+1时刻联合状态
量 一步预测，δi,k+1,k|k为联合量 对应的xk+1|k部分的
容积点，γi,k+1,k|k为联合量 对应的υk|k部分的容积点，Mk+1,k|k为中间
变量，Γk+1,k|k为中间变量；

[0272] 3、计算传播的容积点

[0273]

[0274]

[0275]

[0276]

[0277] 式中， 为xi,k+1|k经系统模型传播的容积点， 为xi，k|k经系统模型传播的容积点， 为xi,k+1|k经量测模型传播的容积点， 为xi,k|k经量测模型传播的容积点，
为 经量测模型传播的容积点， 为 经量测模型传播的容积点，
为δi,k+1,k|k经量测模型传播的容积点；

[0278] 定义

[0279]

[0280]

[0281]

[0282]

[0283]

[0284]

[0285]

[0286]

[0287] 式中，l′1、l′2、l′3、l′4、l′5、l′6、l′7、l′8、l′9、l′10、l′11、l′12、l′13、l′14、l′15、l′16、l′17为中间变量；

[0288] 4、计算式4、式5、式6中相应量

[0289]

[0290] 式中，yk+1为存在量测时滞和数据包丢失的k+1时刻传感器模型；

[0291]

[0292]

[0293]

[0294] 令 则有

[0295]

[0296]

[0297]

[0298]

[0299] 式中， 为k+1时刻新息协方差矩阵，Φ为中间变量，⊙为Hadamard积，Ψ为中间变量，Ξ为中间变量， 为在k时刻量测下状态xk与k+1时刻新息的互协方差矩阵， 为
在k时刻量测下k+1时刻状态与k+1时刻新息的互协方差矩阵，Sk为k时刻互相关噪声，ωk|k
为k时刻量测过程噪声ωk的估计值；

[0300] 将(127)‑(128)带入 得

[0301] 由(125)‑(126)，得εk+1和

[0302] 将εk+1和 带入(43)‑(44)，获得Xk+1|k+1和

[0303] Xk+1|k+1＝Xk+1|k+Mk+1εk+1                   (43)

[0304]

[0305] 其它步骤及参数与具体实施方式一至四之一相同。

[0306] 实施例一：

[0307] 数值仿真分析

[0308] 为了验证本发明所设计控制算法的有效性，使用两个非线性模型来测试所提算法的有效性。其中，‘original’,‘this paper’和'[23]'代表参考信号，本文提出算法的估计
结果和[23]中的算法基于EKF扩展到非线性系统的估计结果。

[0309] 1)单变量非静态增长模型(UNGM)

[0310] UNGM表示如下:

[0311]

[0312]

[0313] 这里ωk和υk是零均值高斯白噪声，且协方差满足

[0314]

[0315] 初值为x0＝‑0.3.滤波器初值为x0|0＝0和P0|0＝1.伯努利分布的随机变量满足概率 和 次独立的蒙特卡洛仿真被执行。并且k时刻误差(Error)和均方根误
差(RMSE)定义如下

[0316]

[0317]

[0318] 这里 和 表示k时刻nth次蒙特卡洛方法的真值和估计值.N是总的仿真次数.图1‑图3为对应的仿真结果，其中图1说明本文估计结果更接近真值，效果优于文献[23]的
算法，图2和图3为本文算法与文献[23]中算法的估计误差及估计均方根误差，进一步说明
了本文算法的优越性，且算法波动比较小，说明算法鲁棒性好。

[0319] 2)强非线性模型

[0320] 强非线性模型给出如下:

[0321]

[0322] zk＝cos(x1,k)+x2,kx3,k+υk  (135)

[0323] 这里，xk和zk是系统状态和量测量，ωk和υk是零均值高斯白噪声，且协方差满足

[0324]T T

[0325] 初始状态为x0＝[‑0.7 1 1]。滤波初值为x0|0＝[0 0 0] 和P0|0＝I3。和 同前所述。

[0326] 图4‑图5为对应的仿真结果，其中图4和图5分别为本文算法与文献[23]中算法的估计误差及估计均方根误差，仿真结果体现出即使针对强非线性系统，提出的算法依然有
较强的处理能力，而文献[23]中的算法几乎获得不到比较满意的真值，说明了本文算法应
用于实际的系统具有更强的处理问题能力。

[0327] 本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于
本发明所附的权利要求的保护范围。