本申请涉及位姿估计的计算领域，尤其涉及一种误差状态估计量的计算方法、装置、设备及存储介质，应用于误差状态滤波器，所述方法包括：获取上一时刻更新后的第一误差状态估计量和对应的第一协方差矩阵；根据第一误差状态估计量和第一协方差矩阵构建对应的传播点，以得到第一传播点组；对每一传播点进行非线性系统变换，以得到变换后的第二传播点组；根据第二传播点组，确定误差状态预测估计量以及对应的第二协方差矩阵；获取当前时刻的观测参数，并根据观测参数确定观测值；根据观测值和误差状态预测估计量更新，以得到当前时刻的第二误差状态估计量。本申请可以实现确保误差状态估计的准确性的有益效果。
【技术实现步骤摘要】
本申请涉及位姿估计的计算领域，尤其涉及一种误差状态估计量的计算方法、装置、设备及存储介质。
技术介绍
1、目前误差状态滤波器（eskf）在无人机、机器人位姿估计领域已经得到了广泛应用。其主要原理是将真实状态分解为名义状态（nominal-state）和误差状态（error-state），并对误差状态进行估计，最后用更新后的误差状态对名义状态进行修正，并对误差状态进行重置。相比传统的扩展卡尔曼滤波器（ekf），最显著的优势是在处理包含姿态估计这样的旋转量上。由于误差状态通常比较小，工作点在原地附近，这样远离奇异点。因此线性化带来的误差或是二阶以上变量带来的误差可以忽略。
2、误差状态滤波器主要作用在于估计误差状态，利用imu以外的传感器对误差状态进行更新从而修补imu偏置以及噪声和模型不完美带来的影响。但是由于每次更新完后，误差状态被重置为0。同时演化方程被线性化后进行预测。因此误差状态的预测值一直为0，预测过程仅仅对误差状态的协方差进行了预测。尽管误差状态为小量，一般情况下线性化演化方程带来的误差可以忽略。但在误差状态的协方差比较大的情况下（例如协方差矩阵的对角元素是很大正数时，意味着误差状态的空间分布比较大），尽管工作点在原点附近，真实值也可能远离原点并处在强非线性区域。在此情况下，直接采用线性化动力学方程则会对误差状态的估计产生较大偏离，进而导致误差状态估计的准确性下降。
3、针对于上述情况可知，如何确保误差状态估计的准确性，是个亟待解决的技术问题。
技术实现思路</p>1、为了克服现有技术的不足，本申请提供了一种误差状态估计量的计算方法、装置、设备及存储介质，以实现确保误差状态估计的准确性的有益效果。
2、本申请解决其技术问题所采用的技术方案具体包括：
3、本申请第一方面提供了一种误差状态估计量的计算方法，应用于误差状态滤波器，所述方法包括：
4、步骤s1，获取上一时刻更新后的第一误差状态估计量和对应的第一协方差矩阵；
5、步骤s2，根据所述第一误差状态估计量和所述第一协方差矩阵构建对应的传播点，以得到第一传播点组；
6、步骤s3，对所述第一传播点组中每一传播点进行非线性系统变换，以得到变换后的第二传播点组；
7、步骤s4，根据所述第二传播点组，确定误差状态预测估计量以及对应的第二协方差矩阵；
8、步骤s5，获取当前时刻的观测参数，并根据所述观测参数确定观测值；
9、步骤s6，根据所述观测值和所述误差状态预测估计量更新，以得到当前时刻的第二误差状态估计量。
10、可选的，所述根据所述第一误差状态估计量和所述第一协方差矩阵构建对应的传播点的步骤包括：
11、使用无迹变换对所述第一误差状态估计量和所述第一协方差矩阵进行构建，以得到用于表征概率分布的所述第一传播点组，所述第一传播点组的构建采用的公式如下：
12、；
13、；
14、；
15、其中，为系统状态的维度，为第个点对应的权重，是一个可调参数，为对应的矩阵的平方根矩阵的第列。
16、可选的，所述对所述第一传播点组中每一传播点进行非线性系统变换的步骤中，采用如下误差状态的动态系统方程：
17、；
18、其中，为真实状态的名义状态表示，为误差状态量。
19、可选的，所述根据所述第二传播点组，确定误差状态预测估计量以及对应的第二协方差矩阵的步骤中，所述误差状态预测估计量表示为：
20、；
21、所述第二协方差矩阵表示为：
22、；
23、其中，的下标表示变换后的传播点。
24、可选的，所述获取当前时刻的观测参数，并根据所述观测参数确定观测值的步骤中，采用如下观测方程：
25、；
26、其中为状态量，为观测噪声，为观测值，为对应观测系统的参数。
27、可选的，所述根据所述观测值和所述误差状态预测估计量更新的步骤中，采用如下更新方程：
28、；
29、其中为状态量的预测值，所述状态量的预测值根据名义状态量的值和误差状态量预测估计量采用如下公式确定：
30、；
31、其中，表示四元数乘积。
32、可选的，所述根据所述第二传播点组，确定误差状态预测估计量以及对应的第二协方差矩阵的步骤之后，所述方法包括：
33、采用如下判别式计算哈曼顿距离：
34、；
35、其中，为曼哈顿距离，表示第二协方差矩阵的逆矩阵，表示向量的转置；
36、判断所述曼哈顿距离和预设的期望值之间的数值差是否大于预设的强非线性阈值；
37、若所述数值差大于所述强非线性阈值，则执行步骤s5至步骤s6；
38、否则，将所述误差状态预测估计量赋0，并根据赋0后的误差状态预测估计量进行观测更新。
39、本申请第二方面提供了一种误差状态估计量的计算装置，包括：
40、前置信息获取模块，用于获取上一时刻更新后的第一误差状态估计量和对应的第一协方差矩阵；
41、传播点构建模块，用于根据所述第一误差状态估计量和所述第一协方差矩阵构建对应的传播点，以得到第一传播点组；
42、传播点变换模块，用于对所述第一传播点组中每一传播点进行非线性系统变换，以得到变换后的第二传播点组；
43、传播点信息提取模块，用于根据所述第二传播点组，确定误差状态预测估计量以及对应的第二协方差矩阵；
44、观测值获取模块，用于获取当前时刻的观测参数，并根据所述观测参数确定观测值；
45、误差状态估计量更新模块，用于根据所述观测值和所述误差状态预测估计量更新，以得到当前时刻的第二误差状态估计量。
46、本申请第三方面提供了一种电子设备，包括：至少一个处理器以及至少一个存储器，其中，所述存储器上存储有计算机可读指令；
47、所述计算机可读指令被一个或多个所述处理器执行，使得电子设备实现上述的误差状态估计量的计算方法。
48、本申请第四方面提供了一种存储介质，其上存储有计算机可读指令，所述计算机可读指令被处理器执行，以实现上述的误差状态估计量的计算方法。
49、本申请的有益效果是：获取上一时刻更新后的第一误差状态估计量和对应的第一协方差矩阵，通过无迹变换等非线性变换方式，基于这两个量构建表征概率分布的第一传播点组，再将所有的传播点对传播点表征的状态量进行系统演化，得到误差状态预测估计量及其对应的第二协方差矩阵；在此情况下根据第二协方差矩阵构建基于误差状态量的曼哈顿距离，以根据曼哈顿距离判断误差状态的协方差是否过大，若是，则结合误差状态预测估计量及第二协方差矩阵，对当前时刻的第二误差状态估计量进行观测更新，从而避免由于线性化误差状态的动力学方程，以及更新过的误差状态被置本文档来自技高网
...
【技术保护点】
1.一种误差状态估计量的计算方法，其特征在于，应用于误差状态滤波器，所述方法包括：
2.根据权利要求1所述的误差状态估计量的计算方法，其特征在于，所述根据所述第一误差状态估计量和所述第一协方差矩阵构建对应的传播点的步骤包括：
3.根据权利要求2所述的误差状态估计量的计算方法，其特征在于，所述对所述第一传播点组中每一传播点进行非线性系统变换的步骤中，采用如下误差状态的动态系统方程：
4.根据权利要求3所述的误差状态估计量的计算方法，其特征在于，所述根据所述第二传播点组，确定误差状态预测估计量以及对应的第二协方差矩阵的步骤中，所述误差状态预测估计量表示为：
5.根据权利要求4所述的误差状态估计量的计算方法，其特征在于，所述获取当前时刻的观测参数，并根据所述观测参数确定观测值的步骤中，采用如下观测方程：
6.根据权利要求5所述的误差状态估计量的计算方法，其特征在于，所述根据所述观测值和所述误差状态预测估计量更新的步骤中，采用如下更新方程：
7.根据权利要求4所述的误差状态估计量的计算方法，其特征在于，所述根据所述第二传播点组，确定误差状态预测估计量以及对应的第二协方差矩阵的步骤之后，所述方法包括：
8.一种误差状态估计量的计算装置，其特征在于，包括：
9.一种电子设备，其特征在于，包括：至少一个处理器以及至少一个存储器，其中，所述存储器上存储有计算机可读指令；
10.一种存储介质，其上存储有计算机可读指令，其特征在于，所述计算机可读指令被处理器执行，以实现如权利要求1至7中任一项所述的误差状态估计量的计算方法。
...
【技术特征摘要】
1.一种误差状态估计量的计算方法，其特征在于，应用于误差状态滤波器，所述方法包括：
2.根据权利要求1所述的误差状态估计量的计算方法，其特征在于，所述根据所述第一误差状态估计量和所述第一协方差矩阵构建对应的传播点的步骤包括：
3.根据权利要求2所述的误差状态估计量的计算方法，其特征在于，所述对所述第一传播点组中每一传播点进行非线性系统变换的步骤中，采用如下误差状态的动态系统方程：
4.根据权利要求3所述的误差状态估计量的计算方法，其特征在于，所述根据所述第二传播点组，确定误差状态预测估计量以及对应的第二协方差矩阵的步骤中，所述误差状态预测估计量表示为：
5.根据权利要求4所述的误差状态估计量的计算方法，其特征在于，所述获取当前时刻的观测参数，并根据所述观测参数确...
【专利技术属性】
技术研发人员：孙涛，张建国，丁宁，黄东岳，
申请(专利权)人：深圳市人工智能与机器人研究院，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。