一种基于最大相关熵的滤波定位方法、系统及滤波器与流程

本发明涉及滤波，尤其涉及一种基于最大相关熵的滤波定位方法、系统及滤波器。

背景技术：

1、最大相关熵（maximum correntropy，mc）是一种基于信息熵的新颖特征选择方法，通过最大化特征与目标变量直接的相关熵来选择最相关的特征，在数值仿真验证领域具有非常重要的地位。

2、目前，一些mc滤波定位方法主要采用的是固定点迭代的方式获取状态模型的后验状态进行定位，但这种方法较为耗时，尤其当出现较大野值时，迭代可能难以收敛，导致定位鲁棒性较差。还有些mc滤波定位方法采用经验的固定高斯核带宽；但在实际应用中，固定高斯核带宽难以适应实时变化的噪声和观测异常，无法保证定位精度，尤其是当出现较大的观测异常时，可能造成矩阵奇异，使数值不清楚或无法解答，导致定位无法正常运行。此外，还有些学者提出了自适应计算高斯核带宽的方法进行滤波定位，这种方法虽然取得了优于固定高斯核带宽的效果，但其主要是通过迭代方式搜索最优带宽参数以满足代价函数的最大相关熵，这种方法不仅效率较低而且成本较高。

3、因此，亟需一种既可以识别观测异常、避免由固定高斯核带宽造成矩阵奇异而引起数值不稳定问题，又可以保证定位精度、且计算效率高、具备较好的时效性和鲁棒性的滤波定位方法。

技术实现思路

1、本发明提供一种基于最大相关熵的定位滤波方法、系统及滤波器，用以同时解决现有滤波定位方法无法识别观测异常、定位精度低、计算效率低且成本高的问题。

2、第一方面，本发明提供一种基于最大相关熵的定位滤波方法，该方法包括：

3、获取载体的当前状态参数，得到动力学模型的imu状态方程和基于卡尔曼滤波的uwb观测方程；

4、基于所述imu状态方程和所述uwb观测方程建立imu-uwb混合定位模型；

5、基于最大相关熵准则求解所述imu-uwb混合定位模型，得到固定点的解析表达式，输出定位结果。

6、进一步的，所述imu-uwb混合定位模型为：

7、

8、其中，表示k-1时刻下的imu状态方程；表示k时刻下的预测uwb观测方程；表示k时刻下的预测imu状态参数；表示k时刻下的uwb观测方程；表示噪声项，，表示imu状态噪声向量；表示uwb观测噪声向量。

9、进一步的，所述噪声项满足：

10、；

11、其中，表示所述imu状态方程对应的状态协方差矩阵；

12、]，表示量测噪声矩阵；分别对和进行cholesky分解得到和；，表示增广变量。

13、进一步的，所述求解包括：

14、将所述imu-uwb混合定位模型的每一项乘，令，，，则得到：；

15、基于最大相关熵准则得到代价函数，并将关于预测imu状态参数的代价函数代入高斯核中，得到；其中，为关键矩阵，，表示高斯核函数，表示预测值与真实值之间的误差，为状态数，为观测数；

16、将求解得到的预测imu状态参数表示为固定点的解析表达式。

17、进一步的，所述固定点的解析表达式为：；

18、其中，表示k时刻下的预测imu状态参数；表示设计矩阵，令，为雅可比矩阵，为单位矩阵。

19、进一步的，所述方法还包括：

20、根据关键矩阵修正量测噪声矩阵，得到预测量测噪声矩阵；并将所述预测uwb观测方程所对应的协方差矩阵中的量测噪声矩阵替换为预测量测噪声矩阵，得到预测imu状态参数和与对应的预测状态协方差矩阵，输出定位结果；

21、其中，预测量测噪声矩阵为：；的计算方式为：

22、；；表示uwb观测方程的量测信息。

23、进一步的，基于所述最大相关熵准则，通过自适应计算得到高斯核带宽；所述高斯核带宽的计算方式为：；其中，表示常数，表示l2范数。

24、进一步的，所述动力学模型为四状态的九轴mems惯性动力学模型。

25、第二方面，本发明还提供了一种基于最大相关熵的定位滤波系统，该系统至少包括动力学模型、观测模型以及混合定位模型，用于执行上述任一项方法的步骤。

26、第三方面，本发明还提供了一种基于最大相关熵的定位滤波器，该滤波器至少包括动力学模型、观测模型以及混合定位模型，用于执行上述任一项方法的步骤。

27、总体而言，通过本发明所构思的技术方案，与现有技术相比能够取得下列有益效果：

28、（1）本发明提供一种基于最大相关熵的定位滤波方法、系统及滤波器，通过将imu状态方程和uwb观测方程进行紧组合，不仅结合的更为紧密，环境适应识别能力更好；而且利用最大相关熵来进行解算，可以在滤波过程中建立起imu-uwb混合定位模型，输出与期望响应之间的最大相似性，使得两个随机过程的相似性最大化，使定位结果具有更高的适用性及可靠性。

29、（2）本发明提供一种基于最大相关熵的定位滤波方法、系统及滤波器，由于imu状态方程和uwb观测方程的时间分辨率较高，本发明利用imu机械编排获取的状态预测值代替传统的，不需要进行耗时的固定点迭代，从而不用修正imu状态方程对应的状态协方差矩阵，只用修正量测噪声矩阵，在保证滤波精度的同时还提高了计算效率，降低了运算成本。

30、（3）本发明提供一种基于最大相关熵的定位滤波方法、系统及滤波器，通过利用简单有效的变分自适应高斯核贷款的计算方法，不仅较好地削弱了观测异常的负面影响，避免了传统mc固定核带宽因观测异常而造成的矩阵数值不稳定问题，具备较好鲁棒性；而且在定位精度和定位稳健性上均表现出最优性能。

技术特征：

1.一种基于最大相关熵的定位滤波方法，其特征在于，该方法包括：

2.如权利要求1所述的一种基于最大相关熵的定位滤波方法，其特征在于，所述imu-uwb混合定位模型为：

3.如权利要求2所述的一种基于最大相关熵的定位滤波方法，其特征在于，所述噪声项满足：

4.如权利要求3所述的一种基于最大相关熵的定位滤波方法，其特征在于，所述求解包括：

5.如权利要求4所述的一种基于最大相关熵的定位滤波方法，其特征在于，所述固定点的解析表达式为：；

6.如权利要求2所述的一种基于最大相关熵的定位滤波方法，其特征在于，所述方法还包括：

7.如权利要求1所述的一种基于最大相关熵的定位滤波方法，其特征在于，基于所述最大相关熵准则，通过自适应计算得到高斯核带宽；所述高斯核带宽的计算方式为：；其中，表示常数，表示l2范数。

8.如权利要求1所述的一种基于最大相关熵的定位滤波方法，其特征在于，所述动力学模型为四状态的九轴mems惯性动力学模型。

9.一种基于最大相关熵的定位滤波系统，其特征在于，该系统至少包括动力学模型、观测模型以及混合定位模型，用于执行权利要求1～8中任一项所述方法的步骤。

10.一种基于最大相关熵的定位滤波器，其特征在于，该滤波器至少包括动力学模型、观测模型以及混合定位模型，用于执行权利要求1～8中任一项所述方法的步骤。

技术总结
本发明提供一种基于最大相关熵的定位滤波方法、系统及滤波器，该方法包括：获取载体的当前状态参数，得到动力学模型的IMU状态方程和基于卡尔曼滤波的UWB观测方程；基于所述IMU状态方程和所述UWB观测方程建立IMU‑UWB混合定位模型；基于最大相关熵准则求解所述IMU‑UWB混合定位模型，得到固定点的解析表达式，输出定位结果。本发明不仅可以较好地削弱UWB观测异常影响，避免因观测异常造成的矩阵数值不稳定问题，具备较好鲁棒性；而且在定位精度和定位稳健性上均表现出最优性能。

技术研发人员：曹成度,王波,闵阳,李海亮,费亮,夏旺,李昭熹,童思奇,马龙,许诗旋
受保护的技术使用者：中铁第四勘察设计院集团有限公司
技术研发日：
技术公布日：2024/3/17