一种抗差联邦卡尔曼滤波方法、设备与系统

1.本发明涉及涉及组合导航技术领域，尤其涉及一种抗差联邦卡尔曼滤波方法、设备与系统。


背景技术：

2.随着导航定位技术的发展，导航系统的种类越来越多，比如惯导系统、卫星导航系统、磁罗盘、里程仪/多普勒测速仪/空速计、气压高度表/雷达高度表、地标点/地图匹配等。这些导航系统各有特色，优缺点并存。在许多对导航性能要求苛刻的任务中，无论是精度要求还是可靠性要求高，单一的导航系统可能都无法满足要求，这就需要使用多种导航系统同时对运载体进行导航系统的测量，在对所有测量信息作综合处理(包括检测、结合、相关和估计)，从而得到更为准确和可靠的导航结果。
3.导航信息的综合处理技术一般都采用卡尔曼滤波。常规的卡尔曼滤波算法其实在实质上就是线性最小方差估计的一种，它只能应用于特定的数学模型，而且系统噪声还必须是已知其统计特性的均值为零的高斯白噪声，而当数学模型不确定或者出现观测野值时，滤波估计的可靠性会受干扰。因为船舶在情况复杂的海洋环境中航行，会受到各种各样的干扰，如天气的影响，海况的影响，比如船舶航行过程中，突然出现的船体侧滑，这时候测得的航行速度值就可能与船体的实际速度差距很大，这些值被称为观测野值，它会对滤波精度产生极大的影响，甚而可能使得滤波发散。
4.随着系统规模的不断增大，并且观测野值的存在，如何有效处理多个传感器测量信息的问题被提出并得到广泛的研究。因此，对卡尔曼滤波算法的改进研究，来提高滤波的鲁棒性就变得至关重要。


技术实现要素：

5.本发明的目的在于提供一种抗差联邦卡尔曼滤波方法、设备与系统来克服或至少减轻现有技术的上述缺陷中的至少一个。
6.为实现上述目的，本发明提供一种抗差联邦卡尔曼滤波系统，包括在载体上设置的惯性导航系统和n个其它导航系统，n为大于等于1的自然数，其中，所述惯性导航系统对应主滤波器，所述n个其它导航系统与n个局部滤波器一一对应；其中，
7.所述n个局部滤波器，用于接收各自对应的传感器的输入数据，根据下述式(1)-(5)计算系统状态量x的估计值：
[0008][0009]
其中，为k-1时刻载体状态的估计值，其初始值为k-1时刻接收到的输入数据；为k-1时刻对k时刻的载体状态的预测量；
[0010]
φ
k/k-1
＝i f(t
k-1
)ts，ts＝t
k-t
k-1
[0011]
其中，i为单位矩阵，tk为k时刻的时间，t
k-1
为k-1时刻的时间，f(t
k-1
)为t
k-1
时刻的
状态转移矩阵；
[0012][0013]
其中，p
k-1
为k-1时刻的状态估计均方差阵；p
k/k-1
为k-1时刻对k时刻的状态估计均方差阵的预测量；为φ
k/k-1
的转置矩阵；q为高斯白噪声方差阵；
[0014][0015]
其中，w
x
为量测残差的加权矩阵；hk为k时刻的量测矩阵；为k时刻的量测矩阵的转置矩阵；σ1为高斯噪声标准差；wy为状态预测残差的加权矩阵；
[0016][0017][0018]
其中，为k时刻状态量的估计值，zk为k时刻载体的量测量；
[0019]
所述主滤波器，用于融合所述n个局部滤波器得到的k时刻状态量的估计值，根据下式(6)-(7)获得k时刻系统状态全局最优估计和量测噪声方差阵
[0020][0021][0022]
其中，i表示第i个局部滤波器，
[0023][0024]
其中，(β(i))-1
为预设信息分配因子的倒数，所有信息分配因子的和为1。
[0025]
优选的，所述n个局部滤波器，用于使用计算得到的的值替换式(1)中的的值，迭代计算的值，直到前后两次计算得到的的值的差满足预设条件，或者迭代次数达到阈值。
[0026]
优选的，
[0027][0028]
其中，f
ins,9
×9为sins误差方程；t3×3为捷联矩阵，o为零矩阵。
[0029]
优选的，所述主滤波器还用于：将所述最优估计结果反馈至各局部滤波器。
[0030]
优选的，所述n个局部滤波器和所述主滤波器分别对应传感器，所述n个局部滤波器接收各自对应的传感器和所述主滤波器对应的传感器输入的数据。
[0031]
本发明实施例还提供一种抗差联邦卡尔曼滤波方法，应用于设置惯性导航系统和
n个其它导航系统的载体上，n为大于等于1的自然数，所述设备包括主滤波器和n个其它滤波器，其中，所述惯性导航系统对应主滤波器，所述n个其它导航系统与n个局部滤波器一一对应；所述方法包括：
[0032]
通过所述n个局部滤波器接收各自对应的传感器的输入数据，根据下述式(1)-(5)计算系统状态量x的估计值：
[0033][0034]
其中，为k-1时刻的状态量的预测值，其初始值为k-1时刻接收到的输入数据；为k-1时刻对k时刻的载体状态的预测量；
[0035]
φ
k/k-1
＝i f(t
k-1
)ts，ts＝t
k-t
k-1
[0036]
其中，i为单位矩阵，tk为k时刻的时间，t
k-1
为k-1时刻的时间，f(t
k-1
)为t
k-1
时刻的状态转移矩阵；
[0037][0038]
其中，p
k-1
为k-1时刻的状态估计均方差阵；p
k/k-1
为k-1时刻对k时刻的状态估计均方差阵的预测量；为φ
k/k-1
的转置矩阵；q为高斯白噪声方差阵；
[0039][0040]
其中，w
x
为量测残差的加权矩阵；hk为k时刻的量测矩阵；为k时刻的量测矩阵的转置矩阵；σ1为高斯噪声标准差；wy为状态预测残差的加权矩阵；
[0041][0042][0043]
其中，为k时刻状态量的估计值，zk为k时刻载体的量测量；
[0044]
通过所述主滤波器融合所述n个局部滤波器得到的k时刻状态量的估计值，根据下式(6)-(7)获得k时刻系统状态全局最优估计和量测噪声方差阵
[0045][0046][0047]
其中，i表示第i个局部滤波器，
[0048][0049]
其中，(β(i))-1
为预设信息分配因子的倒数，所有信息分配因子的和为1。
[0050]
优选的，所述n个局部滤波器，用于使用计算得到的的值替换式(1)中的的值，迭代计算的值，直到前后两次计算得到的的值的差满足预设条件，或者迭代次数达到阈值。
[0051]
优选的，
[0052][0053]
其中，f
ins,9
×9为sins误差方程；t3×3为捷联矩阵，o为零矩阵。
[0054]
优选的，所述主滤波器还用于：将所述最优估计结果反馈至各局部滤波器。
[0055]
本发明实施例还提供一种抗差联邦卡尔曼滤波设备，用于执行上述实施例及其任意优选实施方式中的抗差联邦卡尔曼滤波方法，该设备包括所述主滤波器和所述n个局部滤波器。
[0056]
本发明由于采取以上技术方案，其具有以下优点：
[0057]
引入联邦卡尔曼滤波结构，当量测信息增加或减少时，联邦滤波器仅通过调整局部滤波器数量即可完成融合模式的切换，与传统的集中式卡尔曼滤波算法相比，既具有灵活的数据融合结构，又兼具抗差能力，稳定性和鲁棒性优势明显。本发明中，通过扩大量测异常值的噪声方差，减小其在系统状态估计中的权重，进而降低量测异常值对状态估计精度的影响。
附图说明
[0058]
图1为本发明实施例提供的抗差联邦卡尔曼滤波系统的结构示意图。
[0059]
图2为本发明一示例提供的抗差联邦卡尔曼滤波系统的结构示意图。
[0060]
图3为本发明实施例提供的抗差联邦卡尔曼滤波方法的流程示意图。
[0061]
图4为本发明一示例提供的抗差联邦卡尔曼滤波系统的结构示意图。
[0062]
图5为本发明一示例提供的抗差联邦卡尔曼滤波方法的流程示意图。
具体实施方式
[0063]
在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。
[0064]
在本发明的描述中，术语“中心”、“纵向”、“横向”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制。
[0065]
在不冲突的情况下，本发明各实施例及各实施方式中的技术特征可以相互组合，并不局限于该技术特征所在的实施例或实施方式中。
[0066]
下面结合附图以及具体实施例对本发明做进一步的说明，需要指出的是，下面仅以一种最优化的技术方案对本发明的技术方案以及设计原理进行详细阐述，但本发明的保护范围并不仅限于此。
[0067]
本文涉及下列术语，为便于理解，对其含义说明如下。本领域技术人员应当理解，
下列术语也可能有其它名称，但在不脱离其含义的情形下，其它任何名称都应当被认为与本文所列术语一致。
[0068]
本发明实施例提出一种抗差联邦卡尔曼滤波系统，如图1所示，包括在载体上设置的惯性导航系统1和n个其它导航系统2，n为大于等于1的自然数，其中，所述惯性导航系统1对应主滤波器10，所述n个其它导航系统2与n个局部滤波器20一一对应；其中，
[0069]
所述n个局部滤波器20，用于接收各自对应的传感器的输入数据，根据下述式(1)-(5)计算系统状态量x的估计值：
[0070][0071]
其中，为k-1时刻的载体状态的估计值，其初始值为k-1时刻接收到的输入数据；为k-1时刻对k时刻的载体状态的预测量；
[0072]
φ
k/k-1
＝i f(t
k-1
)ts，ts＝t
k-t
k-1
[0073]
其中，i为单位矩阵，tk为k时刻的时间，t
k-1
为k-1时刻的时间，f(t
k-1
)为t
k-1
时刻的状态转移矩阵；
[0074][0075]
其中，p
k-1
为k-1时刻的状态估计均方差阵，由基本数理公式得到，数值等于p
k/k-1
为k-1时刻对k时刻的状态估计均方差阵的预测量；为φ
k/k-1
的转置矩阵；q为高斯白噪声方差阵，例如为均值为零且服从正态分布的向量序列；
[0076][0077]
其中，w
x
为量测残差的加权矩阵；hk为k时刻的量测矩阵，为已知的系统结构参数；为k时刻的量测矩阵的转置矩阵；σ1为高斯噪声标准差；wy为状态预测残差的加权矩阵；
[0078][0079][0080]
其中，为k时刻状态量的估计值，zk为k时刻载体的量测量；
[0081]
所述主滤波器10，用于融合所述n个局部滤波器得到的k时刻状态量的估计值，根据下式(6)-(7)获得k时刻系统状态全局最优估计和量测噪声方差阵
[0082][0083][0084]
其中，i表示第i个局部滤波器，
[0085][0086]
其中，(β(i))-1
为预设信息分配因子的倒数，所有信息分配因子的和为1。
[0087]
在一种实施方式中，所述n个局部滤波器，用于使用计算得到的的值替换式(1)中的的值，迭代计算的值，直到前后两次计算得到的的值的差满足预设条件，或者迭代次数达到阈值。
[0088]
优选的，
[0089][0090]
其中，f
ins,9
×9为sins误差方程，t3×3为捷联矩阵方向余弦矩阵，o为零矩阵。f
ins,9
×9可以表示为：
[0091][0092]
在一种实施方式中，所述主滤波器还用于：将所述最优估计结果反馈至各局部滤波器；
[0093]
所述n个局部滤波器，用于将接收到的的值作为k时刻载体的状态，用于预测下一时刻载体的状态量。
[0094]
在一种实施方式中，所述n个局部滤波器和所述主滤波器分别对应传感器，所述n个局部滤波器接收各自对应的传感器和所述主滤波器对应的传感器输入的数据。
[0095]
在一个具体示例中，本发明提出一种抗差联邦卡尔曼滤波系统。如图2所示，该系统包括：传感器模块和计算机模块。传感器模块包括sins(捷联导航系统)、dvl(多普勒测速仪)和usbl(超短基线定位系统)等传感器，用于提供航位推算所需相应sins数据、dvl数据及usbl数据；计算机模块包括主滤波器和局部滤波器，局部滤波器用于完成估计-粗差检测-量测降权-再估计循环，并计算出状态量抗差估计结果。主滤波器用于根据局部滤波器估计值计算出全局最优估计数据，并通过反馈回路将融合数据返回到局部滤波器，影响局部滤波器下个循环的估计。
[0096]
本发明实施例提供一种抗差联邦卡尔曼滤波方法，应用于设置惯性导航系统和n个其它导航系统的载体上，n为大于等于1的自然数，所述设备包括主滤波器和n个其它滤波
器，其中，所述惯性导航系统对应主滤波器，所述n个其它导航系统与n个局部滤波器一一对应；如图3所示，该方法包括：
[0097]
步骤31，通过n个局部滤波器接收各自对应的传感器的输入数据，根据下述式(1)-(5)计算系统状态量x的估计值：
[0098][0099]
其中，为k-1时刻载体状态的估计值，其初始值为k-1时刻接收到的输入数据；为k-1时刻对k时刻的载体状态的预测量；
[0100]
φ
k/k-1
＝i f(t
k-1
)ts，ts＝t
k-t
k-1
[0101]
其中，i为单位矩阵，tk为k时刻的时间，t
k-1
为k-1时刻的时间，f(t
k-1
)为t
k-1
时刻的状态转移矩阵；
[0102][0103]
其中，p
k-1
为k-1时刻的状态估计均方差阵；p
k/k-1
为k-1时刻对k时刻的状态估计均方差阵的预测量；为φ
k/k-1
的转置矩阵；q为高斯白噪声方差阵；
[0104][0105]
其中，w
x
为量测残差的加权矩阵；hk为k时刻的量测矩阵；为k时刻的量测矩阵的转置矩阵；σ1为高斯噪声标准差；wy为状态预测残差的加权矩阵；
[0106][0107][0108]
其中，为k时刻状态量的估计值，zk为k时刻载体的量测量。
[0109]
步骤32，通过主滤波器融合n个局部滤波器得到的k时刻状态量的估计值，根据下式(6)-(7)获得k时刻系统状态全局最优估计和量测噪声方差阵
[0110][0111][0112]
其中，i表示第i个局部滤波器，
[0113][0114]
其中，(β(i))-1
为预设信息分配因子的倒数，所有信息分配因子的和为1。
[0115]
在一种实施方式中，所述n个局部滤波器，用于使用计算得到的的值替换式(1)中的的值，迭代计算的值，直到前后两次计算得到的的值的差满足预设条件，或者迭代次数达到阈值。
[0116]
其中，
[0117][0118]
其中，f
ins,9
×9为sins误差方程；t3×3为捷联矩阵，o为零矩阵。
[0119]
在一种实施方式中，所述主滤波器还用于：将所述最优估计结果反馈至各局部滤波器；
[0120]
所述n个局部滤波器，用于将接收到的的值作为作为k时刻载体的状态，用于预测下一时刻载体的状态量。
[0121]
下面通过一个示例详细介绍本发明提供的抗差联邦卡尔曼滤波方法，在该方法中，引入联邦卡尔曼滤波结构，当量测信息增加或减少时，联邦滤波器仅通过调整局部滤波器数量即可完成融合模式的切换。如图4所示，包括参考系统(本示例中为惯导系统)和n个子系统(分别对应n个其它导航系统)。对子系统输出的信息进行有效性判断，当其对应的传感器有数据输入则认为有效，否则为无效。各局部滤波器将传感器输出的信息分别与同一个参考系统的传感器输出信息进行融合，构建n个局部滤波器。
[0122]
以东北天e-n-u地理坐标系为导航坐标系，以捷联惯性导航系统的位置误差[δl δλ δh]、速度误差[δv
e δv
n δvu]、姿态误差角[φ
e φ
n φu]、沿载体坐标系x-y-z轴上的陀螺漂移和加速度计零偏作为状态量，建立时刻t的载体状态x(t)的表达式：如图5所示，该方法包括：
[0123]
步骤51，根据惯导误差的时间传播特性，对局部滤波器进行时间更新。
[0124]
组合导航模型的载体状态方程如下：
[0125][0126]
其中，为x(t)的一阶导数；t表示当前时刻；f(t)为状态转移矩阵，g(t)为噪声分配矩阵，η(t)为系统噪声向量：
[0127][0128]
其中f
ins,9
×9为sin误差方程，用于对sins数据进行补偿，数值由固定公式计算，其表达式为：o为零矩阵；
[0129][0130]
其中t为捷联矩阵，为机体坐标系转换到惯性坐标系的转移矩阵，表达式为：
[0131]
η(t)＝[n
ax
,n
ay
,n
az
,n
wx
,n
wy
,n
wz
]
t
；
[0132]
其中ax、ay和az分别表示所述载体坐标系x-y-z轴上加速度计零均值高斯白噪声，wx、wy和wz分别表示所述载体坐标系x-y-z轴上陀螺仪零均值高斯白噪声。
[0133]
设真实状态值与一步预测关系方程如下:
[0134][0135]
式中xk为载体当前状态量，k为当前时刻，为k-1时刻对k时刻的载体状态的预测量，δxk为真实值与预测值之差；
[0136]
其中，为k-1时刻的状态量的预测值，其初始值为k-1时刻接收到的输入数据。
[0137][0138]
其中i为单位矩阵，tk为k时刻的时间。
[0139]
步骤52，对组合导航模型的量测方程进行重构，并根据新息向量的模进行粗差判定。
[0140]
组合导航模型的量测方程如下：
[0141]
yk＝hkxk vk[0142]
其中，yk为k时刻的量测量，hk为k时刻的量测矩阵，vk为k时刻的量测噪声；xk表示k时刻载体的状态。
[0143]
结合上述一步预测关系方程，对组合导航模型进行变换，得到
[0144][0145]
定义如下变量：
[0146][0147][0148]
[0149][0150]
其中：
[0151]
σ1为高斯噪声标准差，p
k-1
为k-1时刻的状态估计均方差阵，φ
k/k-1
为k-1时刻的状态方程结构参数，q为高斯白噪声方差阵。
[0152]
重构后的量测方程如下：
[0153]
zk＝mkxk ξk[0154]
定义新息向量元素：
[0155]
其中，zi(k)为k时刻第i个元素的量测向量，为式(4)的状态转移矩阵中k时刻第i个元素，x(k)为k时刻的状态向量。
[0156]
当|ei|小于设定阈值时，按常规卡尔曼滤波方法处理，当|ei|大于设定阈值时，局部滤波器确定量测存在粗差，对量测量进行抗差处理。
[0157]
步骤53，根据重构后的量测方程，定义代价函数：其中ei为状态量的残差。
[0158]
函数ρ满足：
[0159][0160]
其中设k＝r*ei/σ1，r为根据传感器精度改变的系数，例如取值为1.5。
[0161]
根据最小化代价函数求解，应满足：
[0162][0163]
其中为ρ函数的导数，zi为量测向量中的第i个元素，mi为重构后量测方程的状态转移矩阵中的第i个元素，x为状态向量。
[0164][0165]
定义
[0166]
[0167]
式子变成：
[0168]
向量化：m
tw·
e＝0
[0169]
其中e满足z＝mx e
[0170]
将e带入向量化的式子m
t
wz＝m
t
wmx
[0171]
求解得：
[0172]
其中，m
t
为m的转置，z为量测向量，w＝diag[w(ei)]。
[0173]
迭代上述过程，将求解出来的作为状态量替代继续上述操作。迭代次数例如可以设为3。
[0174]
其中设的初始值为
[0175]
基于上述步骤51-53的分析，局部滤波器基于传感器接收到的数据，通过下述式(1)-(5)进行计算：
[0176]
时间更新
[0177][0178][0179]
量测更新
[0180][0181][0182][0183]
其中w
x
代表量测残差的加权矩阵，wy代表状态预测残差的加权矩阵。
[0184]
从公式易得，该方法通过扩大量测异常值的噪声方差，减小其在系统状态估计中的权重，进而降低量测异常值对状态估计精度的影响，提高了系统的抗差性能。
[0185]
以式(5)得到的估计值作为的值代入式(1)，重复计算，直到前后两次计算得到的的值的差满足预设条件，或者迭代次数达到阈值，局部滤波器停止估计，并设定当前估计值为系统状态抗差估计结果。
[0186]
步骤54，主滤波器融合各局部滤波器得到的抗差估计结果，获得系统状态全局最优估计和量测噪声方差阵
[0187]
主滤波器融通过下述公式执行融合：
[0188][0189]
[0190]
式中，为k时刻的状态估计值，其方差阵为其中
[0191][0192]
所有信息分配因子的和应为1，即
[0193]
1＝β
(1)
β
(2)
... β
(n)
β
(m)
。
[0194]
步骤55，主滤波器将最优估计结果反馈至各局部滤波器。
[0195]
主滤波器还向各局部滤波器返回方差阵，方差阵由和信息分配因子倒数(β(i))-1
(i＝1,2,...,n,m)的乘积计算得到：局部滤波器将使用和预测下一时刻的状态量和方差。
[0196]
本发明实施例还提供一种抗差联邦卡尔曼滤波设备，用于执行上述实施例及其任意优选实施方式中的抗差联邦卡尔曼滤波方法，该设备包括上述主滤波器和n个局部滤波器。
[0197]
本发明实施例提供的方案中，引入联邦卡尔曼滤波结构，当量测信息增加或减少时，联邦滤波器仅通过调整局部滤波器数量即可完成融合模式的切换，与传统的集中式卡尔曼滤波算法相比，既具有灵活的数据融合结构，又兼具m估计的抗差能力，稳定性和鲁棒性优势明显。本发明中，通过扩大量测异常值的噪声方差，减小其在系统状态估计中的权重，进而降低量测异常值对状态估计精度的影响。
[0198]
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。