CN116338573A - 一种封闭空间考虑噪声误差特性的无人系统定位方法 - Google Patents

Abstract

本发明公开了一种封闭空间考虑噪声误差特性的无人系统定位方法，包括步骤如下：利用状态扩维和量测差分方法白化有色厚尾过程噪声和量测噪声，用广义双曲分布建模厚尾非高斯噪声，将一步预测和似然概率密度函数用广义双曲分布的分层高斯形式表达，利用变分贝叶斯方法求解系统状态、未知的噪声参数以及辅助变量的后验分布联合求解；输出估计结果及对应的方差矩阵。本发明解决了过程噪声和量测噪声服从有色厚尾分布并且其先验信息未知下的状态估计问题，提高了无人系统在室内、井下、地下室等封闭空间中对设备的定位精度，保障系统正常运行，实现任务目标。

Description

一种封闭空间考虑噪声误差特性的无人系统定位方法

技术领域

本发明涉及无人系统领域，尤其涉及一种封闭空间考虑噪声误差特性的无人系统定位方法。

背景技术

在复杂的封闭空间中，信号传感器与人员或者机械设备上所带有的标签之间传播的无线信号可能会受到遮挡，出现无线信号发生反射、折射等情况，还会受到其他设备信号的干扰，这将导致测量值出现野值，同时由于被跟踪目标的频繁移动和传感器的频繁采样，使得目标跟踪状态模型中的过程和测量噪声为有色厚尾噪声，并且噪声先验信息未知，这些都会造成对目标的位置和速度的估计结果不精确。

公开号为CN114430525A的中国专利提出了一种封闭空间基于传感器网络的分布式定位方法，该方法利用高斯尺度混合分布建模噪声，利用状态演化模型改写量测方程，利用变分学习迭代求解状态估值和噪声参数，提高了封闭环境下定位精度。公开号为CN11462535A的中国专利提出了一种基于变分贝叶斯学习的井下传感器网络目标跟踪方法，该方法在利用椭球分布建模非高斯噪声基础上，采用变分贝叶斯方法，实现了复杂噪声环境下基于传感器网络的高精度目标跟踪。对于封闭空间中的目标跟踪提供了计算简单、鲁棒性强的估计方法。以上两种方法未考虑有色噪声下的目标跟踪定位问题，在此情况下，这两种方法的估计精度都会受到影响。

公开号为CN110940999A的中国专利提出了一种基于误差模型的自适应无迹卡尔曼滤波方法，该方法采用改进的基于Suge-Husa的自适应滤波方法解决噪声的先验统计特性未知的问题，用量测差分方法白化有色量测噪声，但是该方法仅适用于处理有色量测噪声。公开号为CN114024527A的中国专利提出了一种用于处理有色噪声的分布式传感器网络滤波方法，该方法利用训练集学习观测噪声参数，对目标状态和误差方差进行预测，但是该方法与卡尔曼滤波器相比较计算效率低，不适用于复杂环境下的目标跟踪。以上两方法都可以提高在有色噪声下的状态估计精确度，但是都未考虑过程噪声和量测噪声服从有色厚尾的情况，当应用在封闭空间如室内、井下、地下室时，这两种方法的性能都会受到影响，估计精确度降低。

发明内容

发明目的：本发明的目的是提供一种能够提高无人系统在室内、井下、地下室等封闭空间中对设备的定位精度，保障系统正常运行的封闭空间考虑噪声误差特性的无人系统定位方法。

技术方案：本发明封闭空间考虑噪声误差特性的无人系统定位方法，在井下封闭空间中，采用无人系统进行采矿，根据传感器测得与设备上的标签之间的位置信息，实现对无人系统的定位，详细步骤如下：

S1，根据目标运动模型和传感器测得与设备上的标签之间的距离和速度，建立过程方程和量测方程，构建目标定位跟踪系统模型；

S2，利用状态扩维和量测差分方法分别消除有色过程噪声和量测噪声的有色性，将带有有色厚尾过程和量测噪声的状态空间模型转换为带厚尾过程噪声和量测噪声的状态空间模型；

S3，根据过程方程估计目标定位跟踪系统状态变量一步预测值，同时计算相对应的一步预测误差协方差矩阵；

S4，根据变分贝叶斯方法对目标定位跟踪系统状态变量、尺度矩阵以及辅助变量的后验估计联合求解；

S5，输出估计结果及对应的方差矩阵。

进一步，步骤S1中，构建目标定位跟踪系统模型的实现步骤如下：

S11，构建带有色厚尾过程噪声和量测噪声的线性状态空间模型：

x_k＝Fx_k-1+w_k-1

z_k＝Hx_k+v_k

式中，k为离散时间；x_k∈Rⁿ为系统状态变量，n为系统状态变量的维数，Rⁿ为n维实数集；z_k∈R^m为量测变量，m为量测变量的维数，R^m为m维实数集；F_k∈R^n×n为状态转移矩阵，R^{n ×n}为n×n维实数集；H_k∈R^m×n为量测矩阵，R^m×n为m×n维实数集；w_k∈Rⁿ为过程噪声，v_k∈R^m为量测噪声；

S12，利用一阶自回归模型对过程噪声和量测噪声建模：

v_k＝φ_k-1v_k-1+η_k

式中，

φ_k分别是过程噪声和量测噪声的自回归参数，表示当前时刻噪声与上一时刻噪声的相关性，ζ_k、η_k分别是过程白色厚尾噪声、量测白色厚尾噪声；

S13，采用广义双曲分布的分层高斯形式对过程白色厚尾噪声和量测白色厚尾噪声建模：

p(ζ_k|ε_k)＝N(ζ_k；0,ε_kQ_k)

p(ε_k)＝GIG(ε_k；a₀,b₀,c₀)

p(η_k|ι_k)＝N(η_k；0,ι_kR_k)

p(ι_k)＝GIG(ι_k；d₀,e₀,f₀)

式中，N(·；·,·)表示高斯分布，ε_k、ι_k为辅助变量；Q_k、R_k分别为过程白色厚尾噪声矩阵和量测白色厚尾噪声矩阵，GIG(·；·,·,·)表示广义逆高斯分布；a₀、d₀为尺度参数，控制着分布的缩放；b₀、e₀为形状参数，决定分布的偏斜程度；c₀、f₀为指数参数，影响分布的形状和尾部。

进一步，步骤S2中，利用状态扩维和量测差分方法分别消除有色过程噪声和量测噪声的有色性，将带有有色厚尾过程噪声和量测噪声的状态空间模型转换为带厚尾过程噪声和量测噪声的状态空间模型，具体实现步骤如下：

S21，采用状态扩维的方法白化有色厚尾过程噪声，将过程噪声w_k加入到状态向量中，即得到：

式中，

为扩维状态变量；

S22，定义扩维估计误差协方差矩阵

为：

式中，P_k|k为估计误差协方差矩阵，

为互协方差矩阵，/>

为修正过程噪声尺度矩阵，(·)^T表示矩阵转置操作；

S23，根据扩维状态变量构造新的状态方程和量测方程分别为：

式中，

为扩维状态转移矩阵，I为单位矩阵，/>

为噪声驱动矩阵，/>

为扩维量测矩阵；

S24，采用量测差分法白化有色厚尾量测噪声，得到：

进一步，步骤S3中，对一步预测概率密度函数和量测似然概率密度函数进行建模，具体实现步骤如下：

S31，因量测白色厚尾噪声η_k服从广义双曲分布，同时根据步骤S24中得到的新量测方程，用广义双曲分布的分层形式建模量测似然概率密度函数：

p(ε_k)＝GIG(ε_k；a₀,b₀,c₀)；

S32，因过程白色厚尾噪声ζ_k服从广义双曲分布，一步预测概率密度函数近似为：

p(ι_k)＝GIG(ι_k；d₀,e₀,f₀)

式中，一步预测扩维状态变量均值

尺度矩阵

S33，建模正定矩阵Σ_k和R_k分别为逆威沙特分布：

p(Σ_k)＝IW(Σ_k；u₀,U_k)

p(R_k)＝IW(R_k；t₀,T₀)

式中，u₀与t₀为自由度参数，用于控制逆威沙特分布对尺度矩阵Σ_k和R_k的收缩程度，U_k与T₀为逆尺度矩阵，用于描述尺度矩阵Σ_k和R_k的先验信息。

进一步，步骤S4中，采用变分贝叶斯方法求得无人系统状态变量、噪声参数以及辅助变量的后验估计值，具体实现步骤如下：

S41，对变量进行初始化：

u₀＝n+1+ρ、/>

t₀＝m+1+ρ、T₀＝ρR_k、

式中，ρ为调节参数，E^(l)(·)表示求期望操作；

S42，对步骤S43-步骤S47进行N次循环；

S43，计算辅助矩阵

和/>

式中，l为迭代次数，

为扩维估计误差协方差矩阵；

S44，更新辅助变量ι_k和ε_k的后验分布q^(l+1)(ι_k)、q^(l+1)(ε_k)；

S45，更新一步预测概率密度函数的尺度矩阵：

式中，自由度参数

逆尺度矩阵/>

分别根据下式计算：

S46，更新量测噪声的尺度矩阵：

式中，自由度参数

逆尺度矩阵/>

分别根据下式计算：

S47，更新状态变量：

式中，状态变量均值

及其相应的扩维估计误差协方差矩阵/>

分别根据下式计算：

式中，

为修正卡尔曼增益，/>

为修正扩维一步预测误差协方差矩阵、

为修正量测噪声矩阵，通过下式计算：

式中，

分别为/>

的期望，其具体值可以根据广义逆高斯分布的性质得到。

进一步，步骤S5中，经过N次循环后，输出得到的状态变量估计值

及其对应的估计误差协方差矩阵/>

本发明与现有技术相比，其显著效果如下：

本发明通过状态扩维和量测差分方法白化有色厚尾过程噪声和量测噪声，采用变分贝叶斯方法根据新得到的带有厚尾过程噪声和量测噪声的状态空间模型对系统状态变量进行估计，得到目标的最优估计位置；计算简单、估计精度高，能够提高无人系统在室内、井下、地下室等封闭空间中对设备的定位精度，保障系统正常运行，实现任务目标。

附图说明

图1为本发明的流程图；

图2中的(a)为本发明与基于学生分布的变分贝叶斯卡尔曼滤波方法、基于最大相关熵的卡尔曼滤波方法以及基于Huber函数的卡尔曼滤波方法在位置上的均方根误差值，(b)为本发明与基于学生t分布的变分贝叶斯卡尔曼滤波方法、基于最大相关熵的卡尔曼滤波方法以及基于Huber函数的卡尔曼滤波方法在速度上的均方根误差值；

图3为本发明在封闭空间中的实际应用场景示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，一种封闭空间考虑噪声误差特性的无人系统定位方法，具体包括以下步骤：

步骤1，根据目标运动模型和传感器测得与设备上的标签之间的距离和速度，建立过程方程和量测方程，构建目标定位跟踪系统模型；

构建系统状态空间模型，由于过程和量测噪声为有色厚尾噪声，采用一阶自回归模型对过程噪声和量测噪声建模，对厚尾噪声部分采用广义双曲分布建模，考虑噪声先验信息的不精确性，对一步预测概率密度函数和量测噪声的尺度矩阵采用逆威沙特分布建模，具体方法为：

步骤11，构建带有色厚尾过程噪声和量测噪声的线性状态空间模型：

x_k＝Fx_k-1+w_k-1 (1)

z_k＝Hx_k+v_k (2)

式中，k为离散时间，x_k∈Rⁿ为系统状态变量，n为系统状态变量的维数，Rⁿ为n维实数集；z_k∈R^m为量测变量，m为量测变量的维数，R^m为m维实数集；F_k∈R^n×n为状态转移矩阵，R^{n ×n}为n×n维实数集；H_k∈R^m×n为量测矩阵，R^m×n为m×n维实数集；w_k∈Rⁿ为过程噪声，v_k∈R^m为量测噪声；

步骤12，利用一阶自回归模型对过程噪声和量测噪声建模：

v_k＝φ_k-1v_k-1+η_k (4)

式中，

φ_k分别是过程噪声和量测噪声的自回归参数，表示当前时刻噪声与上一时刻噪声的相关性，ζ_k、η_k分别是过程白色厚尾噪声和量测白色厚尾噪声；

步骤13，采用广义双曲分布的分层高斯形式对过程白色厚尾噪声和量测白色厚尾噪声建模，即：

p(ζ_k|ε_k)＝N(ζ_k；0,ε_kQ_k) (5)

p(ε_k)＝GIG(ε_k；a₀,b₀,c₀) (6)

p(η_k|ι_k)＝N(η_k；0,ι_kR_k) (7)

p(ι_k)＝GIG(ι_k；d₀,e₀,f₀) (8)

式中，N(·；·,·)表示高斯分布，ε_k、ι_k为辅助变量；Q_k、R_k分别为过程白色厚尾噪声矩阵和量测白色厚尾噪声矩阵，GIG(·；·,·,·)表示广义逆高斯分布，a₀、d₀称为尺度参数，它控制着分布的缩放，b₀、e₀称为形状参数，它决定了分布的偏斜程度，c₀、f₀称为指数参数，它对分布的形状和尾部有很大的影响。

步骤2，利用状态扩维和量测差分方法分别消除过程噪声和量测噪声的有色性，将带有有色厚尾过程噪声和量测噪声的状态空间模型转换为带厚尾过程噪声和量测噪声的状态空间模型；

采用状态扩维和量测相减的方法分别去除过程噪声和量测噪声的有色性，将带有有色厚尾过程噪声和量测噪声的状态空间模型转换为带厚尾过程噪声和量测噪声的状态空间模型，具体方法为：

步骤21，采用状态扩维的方法白化有色厚尾过程噪声，将过程噪声w_k加入到状态向量中，即得到：

式中，

为扩维状态变量。

步骤22，定义扩维估计误差协方差矩阵

为：

式中，P_k|k为估计误差协方差矩阵，

为互协方差矩阵，/>

为修正过程噪声尺度矩阵，(·)^T表示矩阵转置操作；

步骤23，根据扩维状态变量构造新的状态方程和量测方程分别为：

式中，

为扩维状态转移矩阵，I为单位矩阵，/>

为噪声驱动矩阵，/>

为扩维量测矩阵；

步骤24，采用量测差分法白化有色厚尾量测噪声，可以得到：

步骤3，根据过程方程估计系统状态变量一步预测值，同时计算相对应的一步预测误差协方差矩阵；

为得到后验概率密度函数的近似封闭形式的解析解，需要对一步预测概率密度函数和量测似然概率密度函数进行建模，具体方法为：

步骤31，考虑量测白色厚尾噪声η_k服从广义双曲分布，同时结合公式(13)，用广义双曲分布的分层形式建模量测似然概率密度函数：

p(ε_k)＝GIG(ε_k；a₀,b₀,c₀) (15)；

步骤32，考虑过程白色厚尾噪声ζ_k服从广义双曲分布，一步预测概率密度函数近似为：

p(ι_k)＝GIG(ι_k；d₀,e₀,f₀) (17)

式中，一步扩维预测状态变量均值

尺度矩阵

步骤33，建模正定矩阵Σ_k和R_k分别为逆威沙特分布：

p(Σ_k)＝IW(Σ_k；u₀,U_k) (18)

p(R_k)＝IW(R_k；t₀,T₀) (19)

式中，u₀与t₀为自由度参数，用于控制逆威沙特分布对尺度矩阵Σ_k和R_k的收缩程度；U_k与T₀为逆尺度矩阵，用于描述尺度矩阵Σ_k和R_k的先验信息。

步骤4，根据变分贝叶斯方法对系统状态变量、尺度矩阵以及辅助变量的后验估计联合求解；

采用变分贝叶斯方法求得系统状态变量、噪声参数以及辅助变量的后验估计值，由于变量之间是耦合的，选择固定点迭代的方法进行解耦，具体方法为：

步骤41，对变量进行初始化：

u₀＝n+1+ρ、/>

t₀＝m+1+ρ、T₀＝ρR_k、

式中，ρ为调节参数，E^(l)(·)表示求期望操作；

步骤42，对步骤43-步骤47进行N次循环；

步骤43，计算辅助矩阵

和/>

式中,l为迭代次数，

为扩维估计误差协方差矩阵；

步骤44，更新辅助变量ι_k和ε_k的后验分布q^(l+1)(ι_k)、q^(l+1)(ε_k)，其具体分布根据选定的先验分布通过变分贝叶斯方法确定；

步骤45，更新一步预测概率密度函数的尺度矩阵：

式中，自由度参数

逆尺度矩阵/>

分别根据下式计算：

步骤46，更新量测噪声的尺度矩阵：

式中，自由度参数

逆尺度矩阵/>

分别根据下式计算：

步骤47，更新状态变量：

式中，状态变量均值

及其相应的扩维估计误差协方差矩阵/>

分别根据下式计算：

式中，

为修正卡尔曼增益，/>

为修正扩维一步预测误差协方差矩阵、/>

为修正量测噪声矩阵，通过下式计算：

式中，

分别为/>

的期望，其具体值可以根据广义逆高斯分布的性质得到。

步骤5，结束循环，输出估计结果及对应的方差矩阵。

经过N次循环后，输出得到的扩维状态变量估计值

及其对应的扩维估计误差协方差矩阵/>

以下对本发明的效果做进一步的验证：

本实施例的实际应用场景如图3所示，在井下封闭空间中，采用无人系统进行采矿，传感器测得与设备上的标签之间的距离、速度等位置信息，然后利用本发明提出的方法对数据进行处理，实现对采矿设备的精确定位。

选用具有不精确的尺度矩阵的有色厚尾过程和量测噪声的二维平面目标跟踪定位模型来验证本发明提出的方法的性能。

选择状态转移矩阵F_k-1：

式中，Δt为采样时间间隔，I₂为二维单位矩阵；

量测矩阵H_k为：

H_k＝[I₂ 0] (35)；

选择系统状态变量为：

式中,x_k、y_k表示笛卡尔坐标系下目标的位置坐标，

表示笛卡尔坐标系下目标的速度；

选用均方根误差作为评价指数来衡量算法的性能，其计算公式为：

式中，

分别为在第i次蒙特卡洛实验的真实和估计位置，M_C＝1000为蒙特卡洛实验总次数，T＝100为仿真时间。速度的均方根误差(RMSE_vel)计算方法类似。

过程噪声和量测噪声均利用一阶自回归模型建模，即采用公式(3)和公式(4)，式(3)和(4)中已知相关参数

φ_k＝diag(0.5 0.5)；

过程白色厚尾噪声ζ_k和量测白色厚尾噪声η_k分别利用高斯混合分布建模：

式中，w.p.为事件发生的概率，

和/>

通过下式得到：

式中，Q_k、R_k为过程白色厚尾噪声ζ_k和量测白色厚尾噪声η_k的名义尺度矩阵，分别为：

R_k＝rI₂ (41)

式中，过程噪声参数q＝1m²/s³，量测噪声参数r＝100m²；

图2中的(a)图、(b)图分别为本发明与基于学生t分布的变分贝叶斯卡尔曼滤波方法、基于最大相关熵的卡尔曼滤波方法以及基于Huber函数的卡尔曼滤波方法在位置和速度上的均方根误差值，通过比较显示，本发明提出的方法的性能优于现有方法。

显然，本领域的技术人员可以对本发明进行各种改动和变形而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变形属于本发明权力要求及其等同技术的范围之内，则本发明也意图包含这些改动和变形在内。

Claims (6)

1.一种封闭空间考虑噪声误差特性的无人系统定位方法，其特征在于，在井下封闭空间中，采用无人系统进行采矿，根据传感器测得与设备上的标签之间的位置信息，实现对无人系统的定位，详细步骤如下：

S1，根据目标运动模型和传感器测得与设备上的标签之间的距离和速度，建立过程方程和量测方程，构建目标定位跟踪系统模型；

S2，利用状态扩维和量测差分方法分别消除有色过程噪声和量测噪声的有色性，将带有有色厚尾过程和量测噪声的状态空间模型转换为带厚尾过程噪声和量测噪声的状态空间模型；

S3，根据过程方程估计目标定位跟踪系统状态变量一步预测值，同时计算相对应的一步预测误差协方差矩阵；

S4，根据变分贝叶斯方法对目标定位跟踪系统状态变量、尺度矩阵以及辅助变量的后验估计联合求解；

S5，输出估计结果及对应的方差矩阵。

2.根据权利要求1所述封闭空间考虑噪声误差特性的无人系统定位方法，其特征在于，步骤S1中，构建目标定位跟踪系统模型的实现步骤如下：

S11，构建带有色厚尾过程噪声和量测噪声的线性状态空间模型：

x_k＝Fx_k-1+w_k-1

z_k＝Hx_k+v_k

式中，k为离散时间；x_k∈Rⁿ为系统状态变量，n为系统状态变量的维数，Rⁿ为n维实数集；z_k∈R^m为量测变量，m为量测变量的维数，R^m为m维实数集；F_k∈R^n×n为状态转移矩阵，R^n×n为n×n维实数集；H_k∈R^m×n为量测矩阵，R^m×n为m×n维实数集；w_k∈Rⁿ为过程噪声，v_k∈R^m为量测噪声；

S12，利用一阶自回归模型对过程噪声和量测噪声建模：

v_k＝φ_k-1v_k-1+η_k

式中，

φ_k分别是过程噪声和量测噪声的自回归参数，表示当前时刻噪声与上一时刻噪声的相关性，ζ_k、η_k分别是过程白色厚尾噪声、量测白色厚尾噪声；

S13，采用广义双曲分布的分层高斯形式对过程白色厚尾噪声和量测白色厚尾噪声建模：

p(ζ_k|ε_k)＝N(ζ_k；0,ε_kQ_k)

p(ε_k)＝GIG(ε_k；a₀,b₀,c₀)

p(η_k|ι_k)＝N(η_k；0,ι_kR_k)

p(ι_k)＝GIG(ι_k；d₀,e₀,f₀)

式中，N(·；·,·)表示高斯分布，ε_k、ι_k为辅助变量；Q_k、R_k分别为过程白色厚尾噪声矩阵和量测白色厚尾噪声矩阵，GIG(·；·,·,·)表示广义逆高斯分布，a₀、d₀为尺度参数，控制分布的缩放；b₀、e₀为形状参数，决定分布的偏斜程度；c₀、f₀为指数参数，影响分布的形状和尾部。

3.根据权利要求2所述封闭空间考虑噪声误差特性的无人系统定位方法，其特征在于，步骤S2中，利用状态扩维和量测差分方法分别消除有色过程噪声和量测噪声的有色性，将带有有色厚尾过程噪声和量测噪声的状态空间模型转换为带厚尾过程噪声和量测噪声的状态空间模型，具体实现步骤如下：

S21，采用状态扩维的方法白化有色厚尾过程噪声，将过程噪声w_k加入到状态向量中，即得到：

式中，

为扩维状态变量；

S22，定义扩维估计误差协方差矩阵

为：

式中，P_k|k为估计误差协方差矩阵，

为互协方差矩阵，/>

为修正过程噪声尺度矩阵，(·)^T表示矩阵转置操作；

S23，根据扩维状态变量构造新的状态方程和量测方程分别为：

式中，

为扩维状态转移矩阵，I为单位矩阵，/>

为噪声驱动矩阵，/>

为扩维量测矩阵；

S24，采用量测差分法白化有色厚尾量测噪声，得到：

4.根据权利要求2所述封闭空间考虑噪声误差特性的无人系统定位方法，其特征在于，步骤S3中，对一步预测概率密度函数和量测似然概率密度函数进行建模，具体实现步骤如下：

S31，因量测白色厚尾噪声η_k服从广义双曲分布，同时根据步骤S24中得到的新量测方程，用广义双曲分布的分层形式建模量测似然概率密度函数：

p(ε_k)＝GIG(ε_k；a₀,b₀,c₀)；

S32，因过程白色厚尾噪声ζ_k服从广义双曲分布，一步预测概率密度函数近似为：

p(ι_k)＝GIG(ι_k；d₀,e₀,f₀)

式中，一步预测扩维状态变量均值

尺度矩阵

S33，建模正定矩阵Σ_k和R_k分别为逆威沙特分布：

p(Σ_k)＝IW(Σ_k；u₀,U_k)

p(R_k)＝IW(R_k；t₀,T₀)

式中，u₀与t₀为自由度参数，用于控制逆威沙特分布对尺度矩阵Σ_k和R_k的收缩程度；U_k与T₀为逆尺度矩阵，用于描述尺度矩阵Σ_k和R_k的先验信息。

5.根据权利要求2所述封闭空间考虑噪声误差特性的无人系统定位方法，其特征在于，步骤S4中，采用变分贝叶斯方法求得无人系统状态变量、噪声参数以及辅助变量的后验估计值，具体实现步骤如下：

S41，对变量进行初始化：

u₀＝n+1+ρ、/>

t₀＝m+1+ρ、T₀＝ρR_k、

式中，ρ为调节参数，E^(l)(·)表示求期望操作；

S42，对步骤S43-步骤S47进行N次循环；

S43，计算辅助矩阵

和/>

式中，l为迭代次数，

为扩维估计误差协方差矩阵；

S44，更新辅助变量ι_k和ε_k的后验分布q^(l+1)(ι_k)、q^(l+1)(ε_k)；

S45，更新一步预测概率密度函数的尺度矩阵：

式中，自由度参数

逆尺度矩阵/>

分别根据下式计算：

S46，更新量测噪声的尺度矩阵：

式中，自由度参数

逆尺度矩阵/>

分别根据下式计算：

S47，更新状态变量：

式中，状态变量均值

及其相应的扩维估计误差协方差矩阵/>

分别根据下式计算：

式中，

为修正卡尔曼增益，/>

为修正扩维一步预测误差协方差矩阵、/>

为修正量测噪声矩阵，通过下式计算：

式中，

分别为/>

的期望，其具体值可以根据广义逆高斯分布的性质得到。

6.根据权利要求5所述封闭空间考虑噪声误差特性的无人系统定位方法，其特征在于，步骤S5中，经过N次循环后，输出得到的状态变量估计值

及其对应的估计误差协方差矩阵/>

Images