基于矩阵形式函数迭代的惯导解算方法及系统

1.本发明涉及传感器技术领域，具体地，涉及基于矩阵形式函数迭代的惯导解算方法及系统，更为具体地，涉及基于矩阵形式函数迭代的高精度高效率惯导解算方法。


背景技术：

2.惯性导航是根据陀螺与加速度计的输出(如角速度/加速度、角增量/速度增量等)，以航迹推算的方式积分求解载体的姿态、速度和位置。惯性导航的误差源主要包括传感器误差、地球重力误差和解算误差。通常人们认为基于传统圆锥补偿的多子样惯导算法精度已经足够高。但是，随着超高精度冷原子惯导器件以及高动态大机动应用的出现，惯导解算的精度需要进一步提升，以充分发挥惯性器件的潜能。
3.最近提出的基于切比雪夫多项式函数迭代的高精度惯导解算方法，可以将惯性导航解算中的误差降低到计算机截断误差水平，彻底消除了传统惯导算法中的不可交换性误差。但是，函数迭代方法相对于传统惯导解算方法计算量偏大，限制了在低成本处理器上的应用和推广。因此，如何在不损失计算精度的条件下，提高函数迭代算法的执行效率是一个需要解决的问题。
4.专利文献cn106643726a(申请号：201611051763.4)公开了一种统一惯性导航解算方法，步骤包括：1、更新计算地固坐标系到导航坐标系的转换矩阵2、更新计算固联坐标系到导航坐标系的转换矩阵3、更新计算载体在地固坐标系下的速度矢量4、根据载体在地固坐标系下的速度矢量，更新计算载体在地固坐标系下的位置矢量；并根据所述速度矢量更新载体在地理坐标系下的位置坐标；5、重复步骤1～4，在设定的惯性测量时段内，迭代计算得到每个时刻的载体在地固坐标系下的速度矢量、载体在地固坐标系下的位置矢量，以及载体在地理坐标系下的位置坐标。


技术实现要素：

5.针对现有技术中的缺陷，本发明的目的是提供一种基于矩阵形式函数迭代的惯导解算方法及系统。
6.根据本发明提供的一种基于矩阵形式函数迭代的惯导解算方法，其特征在于，包括：
7.步骤s1：在导航参考坐标系下构建惯性导航方程，基于构建的惯性导航方程得到惯导姿态积分方程、惯导速度积分方程以及惯导位置积分方程；
8.步骤s2：对陀螺和加速度计的观测值进行插值或拟合，以获得区间内角速度与比力的切比雪夫多项式近似表达；
9.步骤s3：基于区间内角速度与比力的切比雪夫多项式近似表达，利用矩阵形式的函数迭代方法对惯导姿态积分方程进行求解，从而精确地解算出姿态；
10.步骤s4：根据计算出的姿态值利用矩阵形式的函数迭代方法对惯导速度积分方程进行求解，从而精确地解算出速度；
11.步骤s5：基于计算出的速度值利用矩阵形式的函数迭代方法对惯导位置积分方程进行求解，从而精确地解算出位置；
12.所述导航参考坐标系包括地球坐标系。
13.优选地，所述步骤s1采用：
14.惯性导航方程在地球坐标系下的表达式为：
[0015][0016][0017][0018]
其中，表示惯导姿态微分方程；上标/下标b表示载体坐标系；e表示地球坐标系；i表示惯性坐标系；n表示导航坐标系；分别表示e系相对于b系的姿态四元数和姿态旋转矩阵；表示b系相对于e系的姿态旋转矩阵；表示陀螺测量的角速度；表示地球自转角速度；表示惯导速度微分方程；ve表示载体相对于地球的速度表示在地球坐标系下；fb表示加速度计测量的比力；ge与pe分别表示地球坐标系下的地球重力加速度向量以及载体在地球坐标系下的位置；
×
表示向量叉乘；表示四元数的乘法；表示惯导位置微分方程；
[0019]
考虑在当前采样时间窗口[0t]内，对惯性导航方程两边积分，得积分形式的惯性导航方程：
[0020][0021][0022][0023]
其中，q(0)、ve(0)、pe(0)分别表示初始姿态四元数、初始速度以及初始位置；q表示姿态四元数。
[0024]
优选地，所述步骤s2采用：
[0025]
假设在当前采样时间窗口t∈[0tn]内，有陀螺测量的角速度或者角增量加速度计测量的比力或者速度增量其中tk(k＝1,2,...,n),n为采样的个数；使用仿射变换将时间窗口映射到τ∈[-1 1]
[0026][0027]
对陀螺和加速度计的观测值进行插值或拟合，以获得区间内角速度与比力fb的切比雪夫多项式近似表达：
[0028]
[0029][0030]
其中，nw，nf分别表示角速度和比力的多项式拟合最大阶数；ci表示角速度多项式近似表达中第i阶切比雪夫多项式的系数，di表示比力多项式近似表达中第i阶切比雪夫多项式的系数；fi表示第一类切比雪夫多项式，其循环定义如下
[0031][0032]
优选地，所述步骤s3采用：
[0033]
由积分形式的惯导姿态方程构建函数迭代形式：
[0034][0035]
其中，下标l表示第l次迭代结果；使用切比雪夫多项式将其近似为其中，mq表示切比雪夫多项式拟合的最大阶数；α
l,i
表示切比雪夫系数；下标l表示第l次迭代，下标i对应第i阶切比雪夫多项式的系数；将切比雪夫系数写成矩阵的形式：
[0036][0037]
已知第l次迭代姿态四元数q
l
以及多项式插值获得的角速度则：
[0038][0039]
z＝diag([1/2,1,
…
,1])
ꢀꢀꢀ
(14)
[0040][0041][0042]
其中，下标m＝mq，σk为切比雪夫零点，定义为
[0043][0044]
对于n
×
m维的矩阵a＝[a1,a2,
…
,an]
t
,diag(a)定义为：
[0045][0046]
将第l 1次迭代的姿态四元数表示为切比雪夫多项式
[0047][0048]
将第l 1次迭代的姿态四元数在切比雪夫零点处的值表示成矩阵形式，
[0049][0050]
其中，b
l 1,i
表示第l 1次迭代的姿态切比雪夫系数；下标i对应第i阶切比雪夫多项式；在求得上述α
l
之后，第l 1次迭代姿态四元数的切比雪夫系数矩阵b
l 1
由下式计算得到：
[0051][0052]
其中，
[0053]
χ0＝[q(0),0,
…
,0]
t
ꢀꢀꢀ
(22)
[0054][0055]
[0056]
矩阵d的第1行第j列元素为因此，基于上一次迭代得到的姿态矩阵q
l
，通过矩阵计算的形式得到下一次迭代过程的姿态切比雪夫多项式系数矩阵b
l 1
，进而得到下一次迭代过程的新姿态矩阵q
l 1
；
[0057]
按照上述迭代过程，直到相邻两次迭代姿态切比雪夫系数误差小于给定误差或达到最大迭代次数时终止迭代，其中相邻两次迭代的误差计算如下
[0058][0059]
优选地，所述步骤s4采用：
[0060]
由积分形式的惯导速度方程构建函数迭代形式：
[0061][0062][0063]
其中，q
*
表示四元数q的共轭四元数；使用切比雪夫多项式近似为其中，mv表示切比雪夫多项式拟合的最大阶数；β
l,i
表示切比雪夫系数；将切比雪夫系数写成矩阵的形式
[0064][0065]
已知当前区间四元数迭代结果q,多项式插值获得的比力fb以及第l次的速度位置迭代结果，β
l
由下式计算
[0066][0067][0068][0069]
将第l 1次速度迭代结果使用切比雪夫多项式表示
[0070][0071]
将第l 1次速度在切比雪夫零点处的值写成矩阵形式
[0072][0073]
其中，s
l 1,i
表示第l 1次迭代的速度切比雪夫系数，下标i对应第i阶切比雪夫多项式；在求得β
l
之后，第l 1次迭代速度切比雪夫系数由下式计算：
[0074][0075]
其中，m＝mv，η0＝[ve(0),0,
…
,0]
t
；因此，基于先前已完成的姿态解算结果，利用上一次迭代得到的速度矩阵通过矩阵计算的形式得到下一次迭代过程的速度切比雪夫多项式系数矩阵s
l 1
，进而得到下一次迭代过程的新速度矩阵
[0076]
按照上述迭代过程，直到相邻两次迭代速度切比雪夫系数误差小于给定误差或达到最大迭代次数时终止迭代，其中两次速度迭代的误差计算如下
[0077][0078]
优选地，所述步骤s5采用：
[0079]
由积分形式的惯导位置方程以及第l 1次迭代计算出的速度切比雪夫系数构建函数迭代形式：
[0080][0081]
另一方面将第l 1次位置迭代结果使用切比雪夫多项式表示
[0082][0083]
将第l 1次位置在切比雪夫零点处的值写成矩阵形式
[0084][0085]
其中，m
p
表示切比雪夫多项式拟合的最大阶数，f的定义与姿态解算中的定义相同，其中m＝m
p
，ρ
l 1,i
表示切比雪夫系数，将切比雪夫系数写成矩阵的形式
[0086][0087]
ρ
l 1
直接由当前迭代得到的速度切比雪夫系数s
l 1
计算：
[0088][0089]
其中，cd＝ud；
[0090]
利用当前迭代过程中已得到的速度切比雪夫多项式系数s
l 1
，基于矩阵形式的函数迭代方法，解算出位置切比雪夫多项式系数矩阵ρ
l 1
，进而解算出下一次迭代过程中的位置矩阵，从而完成位置的解算；
[0091]
按照上述迭代过程，直到相邻两次迭代位置切比雪夫系数误差小于给定误差或达到最大迭代次数时终止迭代，其中两次位置迭代的误差计算如下
[0092][0093]
根据本发明提供的一种基于矩阵形式函数迭代的惯导解算系统，包括：
[0094]
模块m1：在导航参考坐标系下构建惯性导航方程，基于构建的惯性导航方程得到惯导姿态积分方程、惯导速度积分方程以及惯导位置积分方程；
[0095]
模块m2：对陀螺和加速度计的观测值进行插值或拟合，以获得区间内角速度与比力的切比雪夫多项式近似表达；
[0096]
模块m3：基于区间内角速度与比力的切比雪夫多项式近似表达，利用矩阵形式的函数迭代方法对惯导姿态积分方程进行求解，从而精确地解算出姿态；
[0097]
模块m4：根据计算出的姿态值利用矩阵形式的函数迭代方法对惯导速度积分方程进行求解，从而精确地解算出速度；
[0098]
模块m5：基于计算出的速度值利用矩阵形式的函数迭代方法对惯导位置积分方程进行求解，从而精确地解算出位置；
[0099]
所述导航参考坐标系包括地球坐标系。
[0100]
优选地，所述模块m1采用：
[0101]
惯性导航方程在地球坐标系下的表达式为：
[0102][0103][0104][0105]
其中，表示惯导姿态微分方程；上标/下标b表示载体坐标系；e表示地球坐标系；i表示惯性坐标系；n表示导航坐标系；分别表示e系相对于b系的姿态四元数和姿态旋转矩阵；表示b系相对于e系的姿态旋转矩阵；表示陀螺测量的角速度；表示地球自转角速度；表示惯导速度微分方程；ve表示载体相对于地球的速度表示在地球坐标系下；fb表示加速度计测量的比力；ge与pe分别表示地球坐标系下的地球重力加速度向量以及载体在地球坐标系下的位置；
×
表示向量叉乘；表示四元数的乘法；表示惯导位置微分
方程；
[0106]
考虑在当前采样时间窗口[0t]内，对惯性导航方程两边积分，得积分形式的惯性导航方程：
[0107][0108][0109][0110]
其中，q(0)、ve(0)、pe(0)分别表示初始姿态四元数、初始速度以及初始位置；q表示姿态四元数。
[0111]
优选地，所述模块m2采用：
[0112]
假设在当前采样时间窗口t∈[0tn]内，有陀螺测量的角速度或者角增量加速度计测量的比力或者速度增量其中tk(k＝1,2,...,n),n为采样的个数；使用仿射变换将时间窗口映射到τ∈[-1 1]
[0113][0114]
对陀螺和加速度计的观测值进行插值或拟合，以获得区间内角速度与比力fb的切比雪夫多项式近似表达：
[0115][0116][0117]
其中，nw，nf分别表示角速度和比力的多项式拟合最大阶数；ci表示角速度多项式近似表达中第i阶切比雪夫多项式的系数，di表示比力多项式近似表达中第i阶切比雪夫多项式的系数；fi表示第一类切比雪夫多项式，其循环定义如下
[0118][0119]
优选地，所述模块m3采用：
[0120]
由积分形式的惯导姿态方程构建函数迭代形式：
[0121][0122]
其中，下标l表示第l次迭代结果；使用切比雪夫多项式将其近似为其中，mq表示切比雪夫多项式拟合的最大阶数；α
l,i
表示切比雪夫系数；下标l表示第l次迭代，下标i对应第i阶切比雪夫多项式的系数；将切比雪夫系数写成矩阵的形式：
[0123][0124]
已知第l次迭代姿态四元数q
l
以及多项式插值获得的角速度则：
[0125][0126]
z＝diag([1/2,1,
…
,1])
ꢀꢀꢀ
(14)
[0127][0128][0129][0130]
其中，下标m＝mq，σk为切比雪夫零点，定义为
[0131][0132]
对于n
×
m维的矩阵a＝[a1,a2,
…
,an]
t
,diag(a)定义为：
[0133][0134]
将第l 1次迭代的姿态四元数表示为切比雪夫多项式
[0135][0136]
将第l 1次迭代的姿态四元数在切比雪夫零点处的值表示成矩阵形式，
[0137][0138]
其中，b
l 1,i
表示第l 1次迭代的姿态切比雪夫系数；下标i对应第i阶切比雪夫多项式；在求得上述α
l
之后，第l 1次迭代姿态四元数的切比雪夫系数矩阵b
l 1
由下式计算得到：
[0139][0140]
其中，
[0141]
χ0＝[q(0),0,
…
,0]
t
ꢀꢀꢀ
(22)
[0142][0143][0144]
矩阵d的第1行第j列元素为因此，基于上一次迭代得到的姿态矩阵q
l
，通过矩阵计算的形式得到下一次迭代过程的姿态切比雪夫多项式系数矩阵b
l 1
，进而得到下一次迭代过程的新姿态矩阵q
l 1
；
[0145]
按照上述迭代过程，直到相邻两次迭代姿态切比雪夫系数误差小于给定误差或达到最大迭代次数时终止迭代，其中相邻两次迭代的误差计算如下
[0146][0147]
所述模块m4采用：
[0148]
由积分形式的惯导速度方程构建函数迭代形式：
[0149][0150][0151]
其中，q
*
表示四元数q的共轭四元数；使用切比雪夫多项式近似为其中，mv表示切比雪夫多项式拟合的最大阶数；β
l,i
表示切比雪夫系数；将切比雪夫系数写成矩阵的形式
[0152][0153]
已知当前区间四元数迭代结果q,多项式插值获得的比力fb以及第l次的速度位置迭代结果，β
l
由下式计算
[0154][0155][0156][0157]
将第l 1次速度迭代结果使用切比雪夫多项式表示
[0158][0159]
将第l 1次速度在切比雪夫零点处的值写成矩阵形式
[0160][0161]
其中，s
l 1,i
表示第l 1次迭代的速度切比雪夫系数，下标i对应第i阶切比雪夫多项式；在求得β
l
之后，第l 1次迭代速度切比雪夫系数由下式计算：
[0162][0163]
其中，m＝mv，η0＝[ve(0),0,
…
,0]
t
；因此，基于先前已完成的姿态解算结果，利用上一次迭代得到的速度矩阵通过矩阵计算的形式得到下一次迭代过程的速度切比雪夫多项式系数矩阵s
l 1
，进而得到下一次迭代过程的新速度矩阵
[0164]
按照上述迭代过程，直到相邻两次迭代速度切比雪夫系数误差小于给定误差或达到最大迭代次数时终止迭代，其中两次速度迭代的误差计算如下
[0165][0166]
所述模块m5采用：
[0167]
由积分形式的惯导位置方程以及第l 1次迭代计算出的速度切比雪夫系数构建函数迭代形式：
[0168][0169]
另一方面将第l 1次位置迭代结果使用切比雪夫多项式表示
[0170][0171]
将第l 1次位置在切比雪夫零点处的值写成矩阵形式
[0172][0173]
其中，m
p
表示切比雪夫多项式拟合的最大阶数，f的定义与姿态解算中的定义相同，其中m＝m
p
，ρ
l 1,i
表示切比雪夫系数，将切比雪夫系数写成矩阵的形式
[0174][0175]
ρ
l 1
直接由当前迭代得到的速度切比雪夫系数s
l 1
计算：
[0176][0177]
其中，cd＝ud；
[0178]
利用当前迭代过程中已得到的速度切比雪夫多项式系数s
l 1
，基于矩阵形式的函数迭代方法，解算出位置切比雪夫多项式系数矩阵ρ
l 1
，进而解算出下一次迭代过程中的位置矩阵，从而完成位置的解算；
[0179]
按照上述迭代过程，直到相邻两次迭代位置切比雪夫系数误差小于给定误差或达到最大迭代次数时终止迭代，其中两次位置迭代的误差计算如下
[0180][0181]
与现有技术相比，本发明具有如下的有益效果：本发明可以函数迭代逼近方法，将惯性导航解算误差降低到计算机截断误差水平，即姿态、速度、位置解算中的圆锥、划桨和卷轴误差几乎可以被完全消除。与目前精度最高的算法相比，该发明能够实现同样的高精度解算，且运行效率得到了大幅提升，达到了与传统两子样算法相当的高效率。因此，该发明使得惯性导航解算同时实现了高精度和高效率两大性能。
附图说明
[0182]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0183]
图1为基于矩阵形式函数迭代方法的惯导解算流程图。
具体实施方式
[0184]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
[0185]
实施例1
[0186]
针对现有技术中的缺陷，本发明的目的是提供一种基于矩阵形式函数迭代的高精度高效率惯导解算方法，包括：在导航参考坐标系下构建惯性导航方程，基于构建的惯性导航方程得到惯导姿态积分方程、惯导速度积分方程以及惯导位置积分方程；导航参考坐标系包括地球坐标系，但是不限于地球坐标系。
[0187]
1)基于陀螺仪的测量数据，利用矩阵形式的函数迭代方法对惯导姿态微分方程进行求解，从而精确地解算出姿态。
[0188]
2)基于第一步解算出的姿态值以及加表的测量数据，利用矩阵形式的函数迭代方法对惯导速度微分方程进行求解，从而精确地解算出速度。
[0189]
3)基于上述解算出的速度值，同样利用矩阵形式的函数迭代方法对惯导位置微分方程进行求解，从而精度地解算出位置。
[0190]
其中，所述惯性导航方程在地球坐标系下的表述为
[0191][0192][0193][0194]
其中上标/下标b表示载体坐标系(b系)，e表示地球坐标系(e系)，i表示惯性坐标系(i系)，n表示导航坐标系(n系)。不失一般性，本发明定义导航坐标系为北-天-东。分别表示e系相对于b系的姿态四元数和姿态旋转矩阵，表示陀螺测量的角速度，表示地球自转角速度，ve:表示载体相对于地球的速度表示在e系下(又称为地速度)，fb表示加速度计测量的比力，ge与pe分别表示e系下的地球重力加速度向量以及载体在e系下的位置。
×
表示向量叉乘，表示四元数的乘法。对于两个四元数和其中s与η分别表示四元数的标量与矢量部分，四元数的乘法定义如下：
[0195][0196]
与定义如下
[0197][0198]
其中(
·
)
×
表示三维向量对应的反对称矩阵，in表示n维单位矩阵。
[0199]
考虑在当前采样时间窗口[0t]内，对上述惯性导航方程两边积分，得积分形式的惯性导航方程如下
[0200][0201][0202][0203]
其中q(0)、ve(0)、pe(0)分别表示初始姿态四元数，初始速度和初始位置；为了表达简洁，在不引起歧义的情况忽略了惯导方程中姿态的-上下标。
[0204]
假设在当前采样时间窗口t∈[0tn]内，有陀螺测量的角速度或者角增量加速度计测量的比力或者速度增量其中tk(k＝1,2,...,n),n为采样的个数。使用仿射变换将时间窗口映射到τ∈[-1 1]
[0205][0206]
为了后续计算的需要，本发明首先对陀螺和加速度计的观测值进行插值或拟合，以获得区间内角速度与比力fb的切比雪夫多项式近似表达
[0207]
[0208][0209]
其中nw，nf分别表示角速度和比力的多项式拟合最大阶数。ci表示角速度多项式近似表达中第i阶切比雪夫多项式的系数，di表示比力多项式近似表达中第i阶切比雪夫多项式的系数。
[0210]fi
为第一类切比雪夫多项式(简称切比雪夫多项式)，其循环定义如下
[0211]
f0(τ)＝1,f1(τ)＝τ,
[0212]fi 1
(τ)＝2τfi(τ)-f
i-1
(τ),i≥1
[0213]
1)基于矩阵形式函数迭代的惯导姿态解算算法
[0214]
由积分形式的惯导姿态方程，可以构建如下函数迭代形式
[0215][0216]
其中下标l表示第l次迭代结果，使用切比雪夫多项式将其近似为
[0217][0218]
其中mq表示切比雪夫多项式拟合的最大阶数，α
l,i
表示切比雪夫系数，下标l表示第l次迭代，下标i对应第i阶切比雪夫多项式的系数。将切比雪夫系数写成矩阵的形式
[0219][0220]
已知第l次迭代姿态四元数q
l
以及多项式插值获得的角速度α
l
可以由下式计算
[0221][0222]
其中
[0223]
z＝diag([1/2,1,
…
,1])
[0224][0225][0226][0227]
[0228][0229]
其中下标m＝mq，σk为切比雪夫零点，定义为
[0230][0231]
对于n
×
m维的矩阵a＝[a1,a2,
…
,an]
t
,diag(a)定义为
[0232][0233]
将第l 1次迭代的姿态四元数表示为切比雪夫多项式
[0234][0235]
将第l 1次迭代的姿态四元数在切比雪夫零点处的值表示成矩阵形式，
[0236][0237]bl 1,i
表示第l 1次迭代的姿态切比雪夫系数，下标i对应第i阶切比雪夫多项式。在求得上述α
l
之后，第l 1次迭代姿态四元数的切比雪夫系数矩阵b
l 1
可以由下式计算
[0238][0239]
其中
[0240]
χ0＝[q(0),0,
…
,0]
t
[0241]cs
＝udzf
t
[0242]
[0243][0244]
矩阵d的第1行第j列元素为因此，基于上一次迭代得到的姿态矩阵q
l
，可以通过矩阵计算的形式得到下一次迭代过程的姿态切比雪夫多项式系数矩阵b
l 1
，进而可以得到下一次迭代过程的新姿态矩阵q
l 1
，由此我们通过简洁的矩阵形式函数迭代方法完成了姿态的解算。
[0245]
按照上述迭代过程，直到相邻两次迭代姿态切比雪夫系数误差小于给定误差或达到最大迭代次数时终止迭代，其中相邻两次迭代的误差计算如下
[0246][0247]
2)基于矩阵形式函数迭代的惯导速度解算算法；
[0248]
类似姿态求解，由积分形式的惯导速度方程，可以构建如下函数迭代形式
[0249][0250]
其中
[0251][0252]q*
表示四元数q的共轭四元数。使用切比雪夫多项式近似为
[0253][0254]
其中mv表示切比雪夫多项式拟合的最大阶数，β
l,i
表示切比雪夫系数，下标的含义与上述姿态解算中一致，将切比雪夫系数写成矩阵的形式
[0255][0256]
已知当前区间四元数迭代结果q,多项式插值获得的比力fb以及第l次的速度位置迭代结果，β
l
可以由下式计算
[0257][0258]
其中
[0259]
[0260]
其中
[0261][0262][0263][0264][0265][0266][0267][0268]
将第l 1次速度迭代结果使用切比雪夫多项式表示
[0269][0270]
将第l 1次速度在切比雪夫零点处的值写成矩阵形式
[0271][0272]
其中s
l 1,i
表示第l 1次迭代的速度切比雪夫系数，下标i对应第i阶切比雪夫多项式。在求得β
l
之后，第l 1次迭代速度切比雪夫系数可以由下式计算
[0273][0274]
其中m＝mv，η0＝[ve(0),0,
…
,0]
t
。因此，基于先前已完成的姿态解算结果，利用上一次迭代得到的速度矩阵可以通过矩阵计算的形式得到下一次迭代过程的速度切比雪夫多项式系数矩阵s
l 1
，进而可以得到下一次迭代过程的新速度矩阵由此我们通过简洁的矩阵迭代形式完成了速度的解算。
[0275]
按照上述迭代过程，直到相邻两次迭代速度切比雪夫系数误差小于给定误差或达到最大迭代次数时终止迭代，其中两次速度迭代的误差计算如下
[0276][0277]
3)基于-矩阵形式函数迭代的位置求解算法
[0278]
由积分形式的惯导位置方程以及第l 1次迭代计算出的速度切比雪夫系数，可以构建如下函数迭代形式
[0279][0280]
另一方面将第l 1次位置迭代结果使用切比雪夫多项式表示
[0281][0282]
将第l 1次位置在切比雪夫零点处的值写成矩阵形式
[0283][0284]
其中m
p
表示切比雪夫多项式拟合的最大阶数，f的定义与姿态解算中的定义相同，其中m＝m
p
，ρ
l 1,i
表示切比雪夫系数，其下标的含义与上述姿态、速度解算中一致，将切比雪夫系数写成矩阵的形式
[0285][0286]
ρ
l 1
可以直接由当前迭代得到的速度切比雪夫系数s
l 1
计算
[0287][0288]
其中
[0289][0290]cd
＝ud
[0291]
利用当前迭代过程中已得到的速度切比雪夫多项式系数s
l 1
，基于矩阵形式的函数迭代方法，我们同样能容易地解算出位置切比雪夫多项式系数矩阵ρ
l 1
，进而解算出下一次迭代过程中的位置矩阵，依此类推完成位置的解算。
[0292]
按照上述迭代过程，直到相邻两次迭代位置切比雪夫系数误差小于给定误差或达到最大迭代次数时终止迭代，其中两次位置迭代的误差计算如下
[0293][0294]
上述惯导姿态、速度、位置解算算法的流程图如图1所示。上述解算过程中，cs与cd是常量矩阵，可以提前计算并存储在计算机中。此外切比雪夫零点处的变量计算，如姿态计算中的以及速度计算中的可以通过并行计算加速。相比于传统函数迭代，本发明可以显著提升惯导解算效率。
[0295]
本发明提供的基于矩阵形式函数迭代的惯导解算系统，可以通过本发明提供的基于矩阵形式函数迭代的惯导解算方法中的步骤流程实现。本领域技术人员，可以将所述基
于矩阵形式函数迭代的惯导解算方法理解为基于矩阵形式函数迭代的惯导解算系统的一个优选例。
[0296]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0297]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。