无陀螺惯性测量系统约束角速度估计方法

摘要

本发明提供了一种无陀螺惯性测量系统约束角速度估计方法，该方法具有估计步骤清晰明了、估计误差不随时间变化的特点，可用于无陀螺惯性测量系统冗余加速度计配置情况下角速度的估计，能够有效提高角速度的估计精度。

说明书

技术领域

本发明属于惯性测量应用领域，涉及惯性测量系统高精度导航计算方法，特指一种基于扩展Kalman滤波技术的无陀螺惯性测量系统约束角速度估计方法。

背景技术

传统的惯性测量系统包括三个陀螺仪以测量载体角速度，三个加速度计测量三个方向比力，直接积分角速度可用，从而得到载体姿态，所以所得数值解(估计值)中的姿态误差随着时间发生线性增长，而位置涉及到比力和姿态的二次积分，使得位置误差随着时间的增长发生立方级数的增长。在无陀螺惯性导航系统中，载体的角加速度正比于加速度计输出，需经过一次积分才能得到载体的角速度估计值，由于加速度计测量误差的影响，这使得角速度估计值的误差随时间的增加而增大，而传统的有陀螺仪惯性测量系统，可以直接用于测量角速度，测量精度只与陀螺仪本身的精度相关，避免了误差增大对角速度结果的影响。正是因为无陀螺惯性测量系统需要积分才能得到角速度，其姿态误差与时间的平方成正比，而位置误差与时间的四次方成正比，所以无陀螺惯性测量系统误差累积速度比传统惯性系统更快。但正是由于无陀螺惯性测量系统中没有陀螺仪部件，所以具有可靠性高、成本低的优势。

对于无陀螺惯性测量系统而言，研究高精度的角速度估计方法非常重要，可以有效降低无陀螺惯性测量系统导航计算误差积累速度，提高导航精度。这可以从两个方面考虑：一方面是利用优良的误差补偿方法提高导航计算精度；另一方面是直接从导航解算方法上入手。由于无陀螺惯性测量系统一般都采用冗余加速度计设计，加速度计个数一般都大于六个，例如采用九加速度计构型、十二加速度计构型，每个加速度计都能够获得一个独立测量量，而待解导航参数是加速度和角加速度，只有六个量，因此无陀螺惯性测量系统中存在大量的冗余信息，利用无陀螺惯性测量系统加速度计输出信息可以直接得到角速度乘积项、平方项的估计结果，但并不能够据此直接得到准确度较高的角速度的估计值。

发明内容

本发明提供了一种无陀螺惯性测量系统约束角速度估计方法，该方法解决了现有技术中对无陀螺惯性测量系统的冗余信息利用度低、所得角速度估计值与真实值之间的误差会随的技术问题。

本发明提供的一种无陀螺惯性测量系统约束角速度估计方法，步骤(1)：对于所含加速度计大于12个且能获得角速度二次项估计值的无陀螺惯性测量系统，利用所有加速度计的输出得到线加速度、角加速度、角速度平方项、角速度交叉项次项的线性估计和估计方差；

步骤(1)包括以下步骤：

假设无陀螺惯性测量系统包含N，其中N≥12个加速度计，加速度计矢量Y的输出表示为：

$Y=J\left(\begin{array}{c}\stackrel{·}{\omega }\\ \stackrel{·}{W}\end{array}\right)+\left(\begin{array}{c}{D}_{\omega 1}\\ .\\ .\\ .\\ D_{\omega N}\end{array}\right)\left(\begin{array}{c}{\omega }_x{\omega }_y\\ {\omega }_x{\omega }_z\\ {\omega }_y{\omega }_z\\ {\omega }_x^2\\ {\omega }_y^2\\ {\omega }_z^2\end{array}\right)---\left(5\right)$

其中为载体待测的三维角加速度矢量，为待测的载体三维视加速度矢量，ω_x,ω_y,ω_z为角速度ω的三个分量，Y为N个加速度计组成的测量矢量；d_i为第i个加速度计敏感轴方向，表示矢量d_i的转置，r_i为第i个加速度计中心位置矢量，i＝1,2,…N，N为加速度计个数，J称为构型矩阵：

$J=\left[\begin{array}{cc}{J}_{\stackrel{·}{\omega }}& J_a\end{array}\right],J_{\stackrel{·}{\omega }}={\left(\begin{array}{c}{(r_1\times d_1)}^T\\ .\\ .\\ .\\ {(r_N\times d_N)}^T\end{array}\right)}_{N\times 3},J_a={\left(\begin{array}{c}{d_1}^T\\ .\\ .\\ .\\ {d_N}^T\end{array}\right)}_{N\times 3}---\left(2\right)$

则D_ω为N×6维矩阵，其中，x₃为角速度交叉项矢量，x₄为角速度平方项矢量，x₃和x₄称为角速度二次项，则：

$Y=J\left(\begin{array}{c}\stackrel{·}{\omega }\\ \stackrel{·}{W}\end{array}\right)+\left(\begin{array}{c}{D}_{\omega 1}\\ .\\ .\\ .\\ D_{\omega N}\end{array}\right)\left(\begin{array}{c}{x}_3\\ x_4\end{array}\right)=\left[\begin{array}{cc}J& D_{\omega }\end{array}\right]\left(\begin{array}{c}\stackrel{·}{\omega }\\ \stackrel{·}{W}\\ x_3\\ x_4\end{array}\right)---\left(6\right)$

其中x₃为角速度交叉项、x₄为角速度平方项，根据式(6)，角加速度矢量、视加速度矢量、角速度交叉项和角速度平方项的线性估计为：

$\left(\begin{array}{c}\stackrel{\widehat{·}}{\omega }\\ \stackrel{\widehat{·}}{W}\\ {\hat{x}}_3\\ {\hat{x}}_4\end{array}\right)={\left({\left(\begin{array}{cc}J& D_{\omega }\end{array}\right)}^T\right[\begin{array}{cc}J& D_{\omega }\end{array}\left]\right)}^{-1}{\left(\begin{array}{cc}J& D_{\omega }\end{array}\right)}^{T}Y---\left(7\right)$

此处所得为关于角加速度矢量视加速度矢量角速度交叉项x₃和角速度平方项x₄的中间估计量；

无陀螺惯性测量系统中的各加速度计误差已完成标定和补偿，加速度计测量误差为零均值高斯白噪声，测量方差为σ²，无陀螺系统中的各加速度计测量方差都相同，式(7)的估计方差为：

Q_est＝([J>ω]^T[J>ω])^-1D[Y]＝([J>ω]^T[J>ω])^-1σ²>

其中Q_est为12×12维矩阵；

步骤(2)：根据角加速度的估计值，建立角速度微分方程，根据角加速度的估计方差，构建角速度状态方程过程噪声，并将状态方程离散化；

步骤(2)包括以下步骤：

实际待估变量是角速度矢量ω，令Kalman滤波中的状态变量为角速度矢量ω＝[ω_x>y>z]^T，在离散情况下，角速度差分方程可表示为：

$\begin{array}{c}{\omega }_{x,k}={\omega }_{x,k-1}+{\stackrel{·}{\omega }}_x\Delta t+w_{x,k-1}\Delta t\\ {\omega }_{y,k}={\omega }_{y,k-1}+{\stackrel{·}{\omega }}_y\Delta t+w_{y,k-1}\Delta t\\ {\omega }_{z,k}={\omega }_{z,k-1}+{\stackrel{·}{\omega }}_z\Delta t+w_{z,k-1}\Delta t\end{array}---\left(9\right)$

其中，Δt为时间步长，ω_x,k、ω_y,k、ω_z,k分别为k时刻的X、Y、Z三个方向的角速度值，ω_x,k-1、ω_y,k-1、ω_z,k-1分别为k-1时刻的X、Y、Z三个方向的角速度值，Δt为滤波时间步长，w_x,k-1,w_y,k-1,w_z,k-1为k-1时刻角加速度解算的随机噪声，定义过程量：

$u={\left(\begin{array}{ccc}{\stackrel{·}{\omega }}_x\Delta t& {\stackrel{·}{\omega }}_y\Delta t& {\stackrel{·}{\omega }}_z\Delta t\end{array}\right)}^T---\left(10\right)$

实际计算中u可以根据式(7)估计的角加速度乘以时间步长得到为：

$u={\left(\begin{array}{ccc}{\stackrel{\widehat{·}}{\omega }}_x\Delta t& {\stackrel{\widehat{·}}{\omega }}_y\Delta t& {\stackrel{\widehat{·}}{\omega }}_z\Delta t\end{array}\right)}^T---\left(11\right)$

将式(11)代入式(9)，同时写成矢量形式，角速度ω的离散化状态动力学模型可表示为：

ω_k＝ω_k-1+u_k-1+w_k-1Δt>

根据式(8)得到的角加速度估计方差是加速度计随机误差的线性组合，为矩阵Q_est的第一行至第三行以及第一列至第三列组成的矩阵，式(12)动力学模型的过程噪声方差为：

Q_ω＝(Δt)²Q_est(1:3,1:3)>

假设角速度状态变量初值为：

${\hat{\omega }}_{0/0}={\left(\begin{array}{ccc}{\hat{\omega }}_{x0}& {\hat{\omega }}_{y0}& {\hat{\omega }}_{z0}\end{array}\right)}^T---\left(14\right)$

其中是k＝0的初始时刻X、Y、Z三个方向的角速度初始值，角速度初始值的方差假设为P_0/0，为3×3矩阵，具体计算时根据情况给定；

角速度状态变量的预测值为：

${\hat{\omega }}_{k/k-1}={\hat{\omega }}_{k-1/k-1}+u_{k-1}---\left(15\right)$

其中，表示k-1时刻的估计值，为Kalman滤波中k时刻的预测值，u_k-1表示第k-1时刻的计算值，根据Kalman滤波算法，得到预测值的方差为：

P_k/k-1＝P_k-1/k-1+Q_ωk-1>

其中P_k-1/k-1为k-1时刻估计值的方差矩阵，P_k/k-1为k时刻角速度预测值的方差矩阵，Q_ωk-1为k-1时刻过程的方差矩阵，由(13)式给出；

步骤(3)：根据角速度二次项的估计值和估计方差，建立角速度测量方程以及测量噪声的方差矩阵，并将方程离散化；

步骤(3)包括以下步骤：

将式(7)给出的角速度二次项估计值作为滤波过程中的测量模型，同时考虑加速度计测量所存在的随机误差，得到角速度二次项的测量模型表示为：

$h_k\left(\omega \right)={\left(\begin{array}{cccccc}{\omega }_x{\omega }_y& {\omega }_x{\omega }_z& {\omega }_y{\omega }_z& {\omega }_x^2& {\omega }_y^2& {\omega }_z^2\end{array}\right)}^T+v_k---\left(17\right)$

其中v_k是角速度二次项估计值的随机误差；

随机误差方差表示为：

R_k＝Q_est(7:12,7:12)>

同样根据式(8)可知，式(12)所示动力学模型的过程噪声与式(17)所示测量模型噪声的协方差矩阵为矩阵式(8)所示Q_est的第1行到3行和第7列到12列组成的矩阵：

M_k＝Q_est(1:3,7:12)Δt>

M_k为动力学模型过程噪声与测量模型噪声协方差阵为3×6维矩阵，Δt为时间步长，与式(10)中时间步长相同；

测量模型的Jacobian矩阵H_k为：

$H_k=\frac{\partial h_k\left(\omega \right)}{\partial \omega }{|}_{\omega ={\hat{\omega }}_{k/k-1}}={\left(\begin{array}{ccc}{\omega }_y& {\omega }_x& 0\\ {\omega }_z& 0& {\omega }_x\\ 0& {\omega }_z& {\omega }_y\\ 2{\omega }_x& 0& 0\\ 0& 2{\omega }_y& 0\\ 0& 0& 2{\omega }_z\end{array}\right)}_{\omega ={\hat{\omega }}_{k/k-1}}---\left(20\right)$

其中为式(15)中的角速度预报值，为求偏导数符号；

则Kalman滤波增益矩阵的更新方程为：

$K_k=(P_{k/k-1}{H}_k^T+M_k){(H_k{P}_{k/k-1}{H}_k^T+H_k{M}_k+M_k^T{H}_k^T+R_k)}^{-1}---\left(21\right)$

滤波方差的修正方程为：

$P_{k/k}=P_{k/k-1}-K_k(H_k{P}_{k/k-1}+M_k^T)---\left(22\right)$

其中，P_k/k为k时刻角速度估计值的方差矩阵，角速度变量的修正方程为：

${\hat{\omega }}_{k/k}={\hat{\omega }}_{k/k-1}+K_k(y_k-h_k({\hat{\omega }}_{k/k-1}\left)\right)---\left(23\right)$

其中，表示k时刻的无陀螺惯性系统角速度估计值，是根据(7)式计算得到的角速度二次项解向量；

根据结果，式(15)、式(16)、式(21)、式(22)和式(23)即为无陀螺惯性系统角速度约束估计中Kalman滤波递推公式，根据Kalman滤波递推公式和滤波初值和初始方差P_0/0即可得出和P_k/k。

本发明的技术效果：

1、本发明提供无陀螺惯性测量系统约束角速度估计方法充分利用冗余加速度计测量所得输出，首先获得角加速度、角速度二次项的线性估计，然后基于Kalman滤波法得到准确度较高的角速度估计值，本发明提供方法得到的角速度估计值的误差不随测量时间的增加而增大。

2、本发明提供的无陀螺惯性测量系统约束角速度估计方法简单明了，所得结果准确可靠，误差不随时间的增加而变化。

3、本发明提供的无陀螺惯性测量系统约束角速度估计方法中角速度估计误差不随测量时 间增加而变大，使得所得估计值能等价于陀螺仪的角速度测量结果，减少所得估计值与真实值之间的差值。

4、本发明提供的无陀螺惯性测量系统约束角速度估计方法可根据实际使用中的初始值进行实时递推计算。因此该方法可以有效提高无陀螺惯性测量系统角速度估计精度。

具体请参考根据本发明的无陀螺惯性测量系统约束角速度估计方法提出的各种实施例的如下描述，将使得本发明的上述和其他方面显而易见。

附图说明

图1是本发明优选实施例无陀螺惯性测量系统约束角速度估计方法的流程示意图；

图2是典型12加速度计无陀螺惯性测量系统的几何构型示意图，箭头方向表示加速度计的敏感轴指向，加速度计中心在立方体棱的中点处，共四组12个加速度计，立方体边长为0.2m；

图3是本发明优选实施例方法用于仿真计算中时所得X方向角速度估计值减去角速度真值的差值随滤波时间变化过程的示意图；

图4是本发明优选实施例方法用于仿真计算中时所得Y方向角速度估计值减去角速度真值的差值随滤波时间的变化过程示意图；

图5是本发明优选实施例方法用于仿真计算中时所得Z方向角速度估计值减去角速度真值时的差值随滤波时间的变化过程示意图；

图6是本发明优选实施例方法用于仿真计算中时所得X、Y、Z三个方向上角速度估计平均平方误差根随时间变化的情况示意图。

具体实施方式

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

本文中得到的利用冗余信息提高角加速度和线加速度的计算精度，是无陀螺惯性测量系统导航计算方法研究中的主要方向。众多研究人员对这一问题提出了多种角速度计算方法，核心思想是利用无陀螺惯性系统中的冗余估计值估计载体角速度，但其算法设计和估计精度上仍然有待完善和提高。

为完成无陀螺惯性测量系统角速度的高精度估计，首先根据无陀螺惯性测量系统构型特点和冗余加速度计输出，给出线加速度、角加速度、以及角速度二次项的线性估计；其次根据角速度微分方程建立动力学模型，根据角速度二次项估计建立测量模型；最后利用Kalman滤波技术得到角速度的高精度估计。

本发明的技术方案是：首先根据无陀螺惯性系统构型特点和冗余加速度计输出，给出线加速度、角加速度以及角速度二次项的线性估计；其次根据角速度微分方程建立动力学模型， 根据角速度二次项估计值建立测量模型；最后利用扩展Kalman滤波技术得到角速度的高精度估计。本发明适用于具有12个以上加速度计且能获得角速度二次项估计值的无陀螺惯性系统。

参见图1，本发明提供的一种无陀螺惯性系统约束角速度估计方法，包括以下步骤：

本发明提供的一种无陀螺惯性测量系统约束角速度估计方法，步骤(1)：对于所含加速度计大于12个且能获得角速度二次项估计值的无陀螺惯性测量系统，利用所有加速度计的输出得到线加速度、角加速度、角速度平方项、角速度交叉项次项的线性估计和估计方差；

步骤(1)包括以下步骤：

假设无陀螺惯性测量系统包含N，其中N≥12个加速度计，加速度计矢量Y的输出表示为：

$Y=J\left(\begin{array}{c}\stackrel{·}{\omega }\\ \stackrel{·}{W}\end{array}\right)+\left(\begin{array}{c}{D}_{\omega 1}\\ .\\ .\\ .\\ D_{\omega N}\end{array}\right)\left(\begin{array}{c}{\omega }_x{\omega }_y\\ {\omega }_x{\omega }_z\\ {\omega }_y{\omega }_z\\ {\omega }_x^2\\ {\omega }_y^2\\ {\omega }_z^2\end{array}\right)---\left(5\right)$

其中为载体待测的三维角加速度矢量，为待测的载体三维视加速度矢量，ω_x,ω_y,ω_z为角速度ω的三个分量，Y为N个加速度计组成的测量矢量；d_i为第i个加速度计敏感轴方向，表示矢量d_i的转置，r_i为第i个加速度计中心位置矢量，r_i＝[r_xi>yi>zi]^T，i＝1,2,…N，N为加速度计个数，J称为构型矩阵：

$J=\left[\begin{array}{cc}{J}_{\stackrel{·}{\omega }}& J_a\end{array}\right],J_{\stackrel{·}{\omega }}={\left(\begin{array}{c}{(r_1\times d_1)}^T\\ .\\ .\\ .\\ {(r_N\times d_N)}^T\end{array}\right)}_{N\times 3},J_a={\left(\begin{array}{c}{d_1}^T\\ .\\ .\\ .\\ {d_N}^T\end{array}\right)}_{N\times 3}---\left(2\right)$

则D_ω为N×6维矩阵，其中，x₃为角速度交叉项矢量，x₄为角速度平方项矢量，x₃和x₄称为角速度二次项，则：

$Y=J\left(\begin{array}{c}\stackrel{·}{\omega }\\ \stackrel{·}{W}\end{array}\right)+\left(\begin{array}{c}{D}_{\omega 1}\\ .\\ .\\ .\\ D_{\omega N}\end{array}\right)\left(\begin{array}{c}{x}_3\\ x_4\end{array}\right)=\left[\begin{array}{cc}J& D_{\omega }\end{array}\right]\left(\begin{array}{c}\stackrel{·}{\omega }\\ \stackrel{·}{W}\\ x_3\\ x_4\end{array}\right)---\left(6\right)$

其中x₃为角速度交叉项、x₄为角速度平方项，根据式(6)，角加速度矢量、视加速度矢量、角速度交叉项和角速度平方项的线性估计为：

$\left(\begin{array}{c}\stackrel{\widehat{·}}{\omega }\\ \stackrel{\widehat{·}}{W}\\ {\hat{x}}_3\\ {\hat{x}}_4\end{array}\right)={\left({\left(\begin{array}{cc}J& D_{\omega }\end{array}\right)}^T\right[\begin{array}{cc}J& D_{\omega }\end{array}\left]\right)}^{-1}{\left(\begin{array}{cc}J& D_{\omega }\end{array}\right)}^{T}Y---\left(7\right)$

此处所得为关于角加速度矢量视加速度矢量角速度交叉项x₃和角速度平方项x₄的中间估计量；

无陀螺惯性测量系统中的各加速度计误差已完成标定和补偿，加速度计测量误差为零均值高斯白噪声，测量方差为σ²，无陀螺系统中的各加速度计测量方差都相同，式(7)的估计方差为：

Q_est＝([J>ω]^T[J>ω])^-1D[Y]＝([J>ω]^T[J>ω])^-1σ²>

其中Q_est为12×12维矩阵；

步骤(2)：根据角加速度的估计值，建立角速度微分方程，根据角加速度的估计方差，构建角速度状态方程过程噪声，并将状态方程离散化；

步骤(2)包括以下步骤：

实际待估变量是角速度矢量ω，令Kalman滤波中的状态变量为角速度矢量ω＝[ω_x>y>z]^T，在离散情况下，角速度差分方程可表示为：

$\begin{array}{c}{\omega }_{x,k}={\omega }_{x,k-1}+{\stackrel{·}{\omega }}_x\Delta t+w_{x,k-1}\Delta t\\ {\omega }_{y,k}={\omega }_{y,k-1}+{\stackrel{·}{\omega }}_y\Delta t+w_{y,k-1}\Delta t\\ {\omega }_{z,k}={\omega }_{z,k-1}+{\stackrel{·}{\omega }}_z\Delta t+w_{z,k-1}\Delta t\end{array}---\left(9\right)$

其中，Δt为时间步长，ω_x,k、ω_y,k、ω_z,k分别为k时刻的X、Y、Z三个方向的角速度值，ω_x,k-1、ω_y,k-1、ω_z,k-1分别为k-1时刻的X、Y、Z三个方向的角速度值，Δt为滤波时间步长，w_x,k-1,w_y,k-1,w_z,k-1为k-1时刻角加速度解算的随机噪声，定义过程量：

$u={\left(\begin{array}{ccc}{\stackrel{·}{\omega }}_x\Delta t& {\stackrel{·}{\omega }}_y\Delta t& {\stackrel{·}{\omega }}_z\Delta t\end{array}\right)}^T---\left(10\right)$

实际计算中u可以根据式(7)估计的角加速度乘以时间步长得到为：

$u={\left(\begin{array}{ccc}{\stackrel{\widehat{·}}{\omega }}_x\Delta t& {\stackrel{\widehat{·}}{\omega }}_y\Delta t& {\stackrel{\widehat{·}}{\omega }}_z\Delta t\end{array}\right)}^T---\left(11\right)$

将式(11)代入式(9)，同时写成矢量形式，角速度ω的离散化状态动力学模型可表示为：

ω_k＝ω_k-1+u_k-1+w_k-1Δt>

根据式(8)得到的角加速度估计方差是加速度计随机误差的线性组合，为矩阵Q_est的第一行至第三行以及第一列至第三列组成的矩阵，式(12)动力学模型的过程噪声方差为：

Q_ω＝(Δt)²Q_est(1:3,1:3)>

假设角速度状态变量初值为：

${\hat{\omega }}_{0/0}={\left(\begin{array}{ccc}{\hat{\omega }}_{x0}& {\hat{\omega }}_{y0}& {\hat{\omega }}_{z0}\end{array}\right)}^T---\left(14\right)$

其中是k＝0的初始时刻X、Y、Z三个方向的角速度初始值，角速度初始值的方差假设为P_0/0，为3×3矩阵，具体计算时根据情况给定；

角速度状态变量的预测值为：

${\hat{\omega }}_{k/k-1}={\hat{\omega }}_{k-1/k-1}+u_{k-1}---\left(15\right)$

其中，表示k-1时刻的估计值，为Kalman滤波中k时刻的预测值，u_k-1表示第k-1时刻的计算值，根据Kalman滤波算法，得到预测值的方差为：

P_k/k-1＝P_k-1/k-1+Q_ωk-1>

其中P_k-1/k-1为k-1时刻估计值的方差矩阵，P_k/k-1为k时刻角速度预测值的方差矩阵，Q_ωk-1为k-1时刻过程的方差矩阵，由(13)式给出；

步骤(3)：根据角速度二次项的估计值和估计方差，建立角速度测量方程以及测量噪声的方差矩阵，并将方程离散化；

步骤(3)包括以下步骤：

将式(7)给出的角速度二次项估计值作为滤波过程中的测量模型，同时考虑加速度计测量所存在的随机误差，得到角速度二次项的测量模型表示为：

$h_k\left(\omega \right)={\left(\begin{array}{cccccc}{\omega }_x{\omega }_y& {\omega }_x{\omega }_z& {\omega }_y{\omega }_z& {\omega }_x^2& {\omega }_y^2& {\omega }_z^2\end{array}\right)}^T+v_k---\left(17\right)$

其中v_k是角速度二次项估计值的随机误差；

随机误差方差表示为：

R_k＝Q_est(7:12,7:12)>

同样根据式(8)可知，式(12)所示动力学模型的过程噪声与式(17)所示测量模型噪声的协方差矩阵为矩阵式(8)所示Q_est的第1行到3行和第7列到12列组成的矩阵：

M_k＝Q_est(1:3,7:12)Δt>

M_k为动力学模型过程噪声与测量模型噪声协方差阵为3×6维矩阵，Δt为时间步长，与式(10)中时间步长相同；

测量模型的Jacobian矩阵H_k为：

$H_k=\frac{\partial h_k\left(\omega \right)}{\partial \omega }{|}_{\omega ={\hat{\omega }}_{k/k-1}}={\left(\begin{array}{ccc}{\omega }_y& {\omega }_x& 0\\ {\omega }_z& 0& {\omega }_x\\ 0& {\omega }_z& {\omega }_y\\ 2{\omega }_x& 0& 0\\ 0& 2{\omega }_y& 0\\ 0& 0& 2{\omega }_z\end{array}\right)}_{\omega ={\hat{\omega }}_{k/k-1}}---\left(20\right)$

其中为式(15)中的角速度预报值，为求偏导数符号；

则Kalman滤波增益矩阵的更新方程为：

$K_k=(P_{k/k-1}{H}_k^T+M_k){(H_k{P}_{k/k-1}{H}_k^T+H_k{M}_k+M_k^T{H}_k^T+R_k)}^{-1}---\left(21\right)$

滤波方差的修正方程为：

$P_{k/k}=P_{k/k-1}-K_k(H_k{P}_{k/k-1}+M_k^T)---\left(22\right)$

其中，P_k/k为k时刻角速度估计值的方差矩阵，角速度变量的修正方程为：

${\hat{\omega }}_{k/k}={\hat{\omega }}_{k/k-1}+K_k(y_k-h_k({\hat{\omega }}_{k/k-1}\left)\right)---\left(23\right)$

其中，表示k时刻的无陀螺惯性系统角速度估计值，是根据(7)式计算得到的角速度二次项解向量；

根据结果，式(15)、式(16)、式(21)、式(22)和式(23)即为无陀螺惯性系统角速度约束估计中Kalman滤波递推公式，根据Kalman滤波递推公式和滤波初值和初始方差P_0/0即可得出和P_k/k。

通过上述方法可以实现对角速度的估计，所得角速度估计值的误差不随测量时间的增加而增大。

具体步骤如下：

步骤(1)：计算无陀螺惯性系统角速度二次项估计

假设无陀螺惯性测量系统包含N(N≥12)个加速度计，无陀螺惯性测量系统基本测量方程为

$Y=J\left(\begin{array}{c}\stackrel{·}{\omega }\\ \stackrel{·}{W}\end{array}\right)+\left(\begin{array}{c}{d}_1^T{Q}^2{r}_1\\ .\\ .\\ .\\ d_N^T{Q}^2{r}_N\end{array}\right)---\left(1\right)$

其中为载体待测的三维角加速度矢量，为待测的载体三维视加速度矢量，Ω为角速度矢量形成的反对称矩阵，ω_x,ω_y,ω_z为角速度ω的三个分量，Y为N个加速度计组成的测量矢量，d_i为第i个加速度计敏感轴方向，表示矢量d₁的转置，r_i为第i个加速度计中心位置矢量，N为加速度计个数，J称为构型矩阵：

$J=\left[\begin{array}{cc}{J}_{\stackrel{·}{\omega }}& J_a\end{array}\right],J_{\stackrel{·}{\omega }}={\left(\begin{array}{c}{(r_1\times d_1)}^T\\ .\\ .\\ .\\ {(r_N\times d_N)}^T\end{array}\right)}_{N\times 3},J_a={\left(\begin{array}{c}{d_1}^T\\ .\\ .\\ .\\ {d_N}^T\end{array}\right)}_{N\times 3}---\left(2\right)$

在加速度计输出方程式(1)中，项可改写为：

$d_i^T{Q}^2{r}_i=d_i^T\left(\begin{array}{cccccc}{r}_{y_i}& r_{z_i}& 0& 0& -r_{x_i}& -r_{x_i}\\ r_{x_i}& 0& r_{z_i}& -r_{y_i}& 0& -r_{y_i}\\ 0& r_{x_i}& r_{y_i}& -r_{z_i}& -r_{z_i}& 0\end{array}\right)\left(\begin{array}{c}{\omega }_x{\omega }_y\\ {\omega }_x{\omega }_z\\ {\omega }_y{\omega }_z\\ {\omega }_x^2\\ {\omega }_y^2\\ {\omega }_z^2\end{array}\right)---\left(3\right)$

令为1×6矩阵，将D_ωi代入式(3)，得到

$d_i^T{Q}^2{r}_i=D_{\omega i}\left(\begin{array}{c}{\omega }_x{\omega }_y\\ {\omega }_x{\omega }_z\\ {\omega }_y{\omega }_z\\ {\omega }_x^2\\ {\omega }_y^2\\ {\omega }_z^2\end{array}\right)---\left(4\right)$

将式(4)代入式(1)，可得到

$Y=J\left(\begin{array}{c}\stackrel{·}{\omega }\\ \stackrel{·}{W}\end{array}\right)+\left(\begin{array}{c}{D}_{\omega 1}\\ .\\ .\\ .\\ D_{\omega N}\end{array}\right)\left(\begin{array}{c}{\omega }_x{\omega }_y\\ {\omega }_x{\omega }_z\\ {\omega }_y{\omega }_z\\ {\omega }_x^2\\ {\omega }_y^2\\ {\omega }_z^2\end{array}\right)---\left(5\right)$

令则D_ω为N×6维矩阵，其中，x₃为角速度交叉项矢量，x₄为角速度平方项矢量，x₃和x₄称为角速度二次项，则

$Y=J_{N\times 6}{\left(\begin{array}{c}\stackrel{·}{\omega }\\ \stackrel{·}{W}\end{array}\right)}_{6\times 1}+{\left(\begin{array}{c}{D}_{\omega 1}\\ .\\ .\\ .\\ D_{\omega N}\end{array}\right)}_{N\times 6}{\left(\begin{array}{c}{x}_3\\ x_4\end{array}\right)}_{6\times 1}={\left(\begin{array}{cc}J& D_{\omega }\end{array}\right)}_{N\times 12}{\left(\begin{array}{c}\stackrel{·}{\omega }\\ \stackrel{·}{W}\\ x_3\\ x_4\end{array}\right)}_{12\times 1}---\left(6\right)$

无陀螺惯性系统要求能获得角速度二次项估计，则角加速度矢量线视加速度矢量角速度交叉项和角速度平方项的线性估计为

$\left(\begin{array}{c}\stackrel{\widehat{·}}{\omega }\\ \stackrel{\widehat{·}}{W}\\ {\hat{x}}_3\\ {\hat{x}}_4\end{array}\right)={\left({\left(\begin{array}{cc}J& D_{\omega }\end{array}\right)}^T\right[\begin{array}{cc}J& D_{\omega }\end{array}\left]\right)}^{-1}{\left(\begin{array}{cc}J& D_{\omega }\end{array}\right)}^{T}Y---\left(7\right)$

假设无陀螺惯性测量系统中的加速度计误差已完成标定和补偿，加速度计测量误差为零均值高斯白噪声，测量方差为σ²，无陀螺系统中加速度计测量方差都相同。可以看出，此时角加速度和线视加速度的解算不需要角速度信息，同时还可以得到角速度交叉项和平方项二次项的线性估计。式(7)的估计方差为

Q_est＝([J>ω]^T[J>ω])^-1D[Y]＝([J>ω]^T[J>ω])^-1σ²>

其中Q_est为12×12维矩阵，角加速度、线视加速度以及角速度二项项的估计方差只与加速度计测量精度和无陀螺惯性测量系统构型参数有关，而与时间无关，从而避免了工作时间对估计结果的影响。

步骤(2)：根据角加速度估计值，建立角速度动力学方程，根据角加速度的估计方差，给出动力学方程噪声，并将动力学方程离散化；

令状态变量为角速度矢量ω＝[ω_x>y>z]^T，在离散情况下，角速度差分方程可表示为，

$\begin{array}{c}{\omega }_{x,k}={\omega }_{x,k-1}+{\stackrel{·}{\omega }}_x\Delta t+w_{x,k-1}\Delta t\\ {\omega }_{y,k}={\omega }_{y,k-1}+{\stackrel{·}{\omega }}_y\Delta t+w_{y,k-1}\Delta t\\ {\omega }_{z,k}={\omega }_{z,k-1}+{\stackrel{·}{\omega }}_z\Delta t+w_{z,k-1}\Delta t\end{array}---\left(9\right)$

其中Δt为时间步长，ω_x,k、ω_y,k、ω_z,k分别为k时刻的X、Y、Z三个方向的角速度值，ω_x,k-1、ω_y,k-1、ω_z,k-1分别为k-1时刻的X、Y、Z三个方向的角速度值,Δt为滤波时间步长，w_x,k-1,w_y,k-1,w_z,k-1为k-1时刻角加速度解算的随机噪声，定义过程量

$u={\left(\begin{array}{ccc}{\stackrel{·}{\omega }}_x\Delta t& {\stackrel{·}{\omega }}_y\Delta t& {\stackrel{·}{\omega }}_z\Delta t\end{array}\right)}^T---\left(10\right)$

实际计算中u可以根据式(7)估计的角加速度乘以时间步长得到，

$u={\left(\begin{array}{ccc}{\stackrel{\widehat{·}}{\omega }}_x\Delta t& {\stackrel{\widehat{·}}{\omega }}_y\Delta t& {\stackrel{\widehat{·}}{\omega }}_z\Delta t\end{array}\right)}^T---\left(11\right)$

将式(11)代入式(9)，同时写成矢量形式，角速度ω的离散化状态动力学模型可表示为

ω_k＝ω_k-1+u_k-1+w_k-1Δt>

根据式(8)得到的角加速度估计方差是加速度计随机误差的线性组合，为矩阵Q_est的第一行至第三行和第一列至第三列组成的矩阵，式(12)动力学模型的过程噪声方差为

Q_ω＝(Δt)²Q_est(1:3,1:3)>

假设角速度状态变量初值为

${\hat{\omega }}_{0/0}={\left(\begin{array}{ccc}{\hat{\omega }}_{x0}& {\hat{\omega }}_{y0}& {\hat{\omega }}_{z0}\end{array}\right)}^T---\left(14\right)$

其中是初始时刻X、Y、Z三个方向的角速度值初值，角速度初始值方差假设为P_0/0，为3×3矩阵，初值和初始方差P_0/0需要在实际应用时给定；

角速度状态变量的预测值为

${\hat{\omega }}_{k/k-1}={\hat{\omega }}_{k-1/k-1}+u_{k-1}---\left(15\right)$

其中，表示k-1时刻的估计值，为Kalman滤波中k时刻的预测值，u_k-1表示第k-1时刻的计算值，根据Kalman滤波算法，得到预测值的方差为：

P_k/k-1＝P_k-1/k-1+Q_ωk-1>

其中P_k-1/k-1为k-1时刻估计值的方差矩阵，P_k/k-1为k时刻角速度预测值的方差矩阵，Q_ωk-1为k-1时刻过程方差矩阵，由(13)式给出；

步骤(3)：根据角速度二次项的估计值和估计方差，建立角速度测量方程以及测量噪声的方差矩阵，并将方程离散化；由于步骤(1)中并未得到角速度估计值，只得到了角加速度和角速度二次项估计值，所以还需要进一步得到角速度估值。

将式(7)给出的角速度二次项估计值作为滤波过程中的测量模型，同时考虑加速度计测量所存在的随机误差，所得角速度二次项的测量模型可以表示为：

$h_k\left(\omega \right)={\left(\begin{array}{cccccc}{\omega }_x{\omega }_y& {\omega }_x{\omega }_z& {\omega }_y{\omega }_z& {\omega }_x^2& {\omega }_y^2& {\omega }_z^2\end{array}\right)}^T+v_k---\left(17\right)$

其中v_k是角速度二次项估计值的随机误差。根据式(8)可知，该随机误差方差是加速度计随机误差的线性组合，为矩阵Q_est的第7行到第12行和第7列到第12列组成的方阵，为

R_k＝Q_est(7:12,7:12)>

同样根据式(8)可知，式(12)所示动力学模型的过程噪声与式(17)所示测量模型噪声的协方差矩阵为矩阵式(8)所示Q_est的第1行到3行和第7列到12列组成的矩阵

M_k＝Q_est(1:3,7:12)Δt>

M_k为动力学模型过程噪声与测量模型噪声协方差阵，Δt为时间步长，为3×6维。

测量模型的Jacobian矩阵H_k为：

$H_k=\frac{\partial h_k\left(\omega \right)}{\partial \omega }{|}_{\omega ={\hat{\omega }}_{k/k-1}}={\left(\begin{array}{ccc}{\omega }_y& {\omega }_x& 0\\ {\omega }_z& 0& {\omega }_x\\ 0& {\omega }_z& {\omega }_y\\ 2{\omega }_x& 0& 0\\ 0& 2{\omega }_y& 0\\ 0& 0& 2{\omega }_z\end{array}\right)}_{\omega ={\hat{\omega }}_{k/k-1}}---\left(20\right)$

其中为式(15)中的角速度预报值，为求偏导数符号。

则Kalman滤波增益矩阵的更新方程为：

$K_k=(P_{k/k-1}{H}_k^T+M_k){(H_k{P}_{k/k-1}{H}_k^T+H_k{M}_k+M_k^T{H}_k^T+R_k)}^{-1}---\left(21\right)$

滤波方差的修正方程为

$P_{k/k}=P_{k/k-1}-K_k(H_k{P}_{k/k-1}+M_k^T)---\left(22\right)$

其中P_k/k为k时刻角速度估计值的方差矩阵，角速度变量的修正方程为

${\hat{\omega }}_{k/k}={\hat{\omega }}_{k/k-1}+K_k(y_k-h_k({\hat{\omega }}_{k/k-1}\left)\right)---\left(23\right)$

其中表示k时刻的估计值，是根据(7)式计算得到的角速度二次项解向量。

根据结果，式(15)、式(16)、式(21)、式(22)、式(23)即是无陀螺惯性系统角速度约束估计中Kalman滤波递推公式，本发明同时给出了上述5个公式中所有变量的意义及计算公式，这是无陀螺惯性系统角速度约束估计的关键所在。在实际应用中给定滤波初值和初始方差P_0/0，即可递推得到相应的估计值。为k时刻无陀螺惯性系统角速度估计值，P_k/k为k时刻角速度估计方差矩阵。

为了验证本发明提供的方法，具体算例如下：

根据载体建立体坐标系，为笛卡尔直角坐标系。载体三个方向的加速度分别为[1 0.5 2]m/s²，载体三轴都存在角运动，三方向角加速度为0.1×[-1>2，角速度初值为[3>

为验证本发明的结果，采用一种十二加速度计无陀螺惯性测量系统作为计算例子，图2给出了该系统的构型几何，但本发明并不仅限于该构型，可以适用于满足前提条件的任意构型。MEMS加速度计测量随机误差为零均值高斯白噪声，根据目前MEMS加速度计商业货架产品的主流精度水平，选择输出噪声均方差σ_ε＝1mg，则加速度计测量方差σ²＝0.0001m²/s⁴。

步骤(1)：根据图2中所示的12加速度计无陀螺惯性系统，构型矩阵为

$J=\left[\begin{array}{cc}{J}_{\stackrel{·}{\omega }}& J_a\end{array}\right]---\left(2\right)$

$J_{\stackrel{·}{\omega }}={\left(\begin{array}{c}{\left(r_1\times d_1\right)}^T\\ .\\ .\\ .\\ {\left(r_{12}\times d_{12}\right)}^T\end{array}\right)}_{12\times 3}=\left(\begin{array}{ccc}0& -0.1& 0\\ 0& -0.1& 0\\ -0.1& 0& -0.1\\ 0& -0.1& -0.1\\ 0.1& 0& 0\\ -0.1& 0& 0\\ 0& -0.1& 0\\ 0& 0.1& 0\\ -0.1& 0& 0.1\\ 0& 0.1& 0.1\\ -0.1& 0& 0\\ 0.1& 0& 0\end{array}\right),J_a={\left(\begin{array}{c}{d_1}^T\\ .\\ .\\ .\\ {d_{12}}^T\end{array}\right)}_{12\times 3}=\left(\begin{array}{ccc}1& 0& 0\\ 0& 0& 1\\ 0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\\ 0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\\ 0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right)$

矩阵D_ω为

$D_{\omega }=\left(\begin{array}{c}{D}_{\omega 1}\\ .\\ .\\ .\\ D_{\omega N}\end{array}\right)=\left(\begin{array}{cccccc}0& -0.1& 0& 0& -0.1& -0.1\\ 0& 0.1& 0& 0.1& 0.1& 0\\ -0.1& 0& 0.1& 0& 0& 0\\ 0.1& -0.1& 0& 0& 0& 0\\ 0& 0& 0.1& 0.1& 0.1& 0\\ 0& 0& 0.1& 0.1& 0& 0.1\\ 0& -0.1& 0& 0& 0.1& 0.1\\ 0& -0.1& 0& 0.1& 0.1& 0\\ 0.1& 0& 0.1& 0& 0& 0\\ -0.1& 0.1& 0& 0& 0& 0\\ 0& 0& -0.1& -0.1& -0.1& 0\\ 0& 0& -0.1& -0.1& 0& -0.1\end{array}\right)$

则加速度计输出矢量可表示为

$Y={\left(\begin{array}{cc}J& D_{\omega }\end{array}\right)}_{12\times 12}{\left(\begin{array}{c}\stackrel{·}{\omega }\\ \stackrel{·}{W}\\ x_3\\ x_4\end{array}\right)}_{12\times 1}---\left(5\right)$

根据式(7)角加速度矢量线视加速度矢量角速度交叉项和角速度平方项的线性估计为

$\left(\begin{array}{c}\stackrel{\widehat{·}}{\omega }\\ \stackrel{\widehat{·}}{W}\\ {\hat{x}}_3\\ {\hat{x}}_4\end{array}\right)={\left({\left(\begin{array}{cc}J& D_{\omega }\end{array}\right)}^T\right[\begin{array}{cc}J& D_{\omega }\end{array}\left]\right)}^{-1}{\left(\begin{array}{cc}J& D_{\omega }\end{array}\right)}^{T}Y---\left(7\right)$

式(7)的估计方差为

$Q_{est}=\left(\begin{array}{cccccccccccc}0.00625& 0& 0& 0& -0.00025& 0.00025& 0& 0& 0.00125& 0& -0.005& 0.005\\ 0& 0.00375& -0.0025& 0.00025& 0& 0& 0.0025& 0.00125& 0& 0& 0& 0\\ 0& -0.0025& 0.005& -0.00025& 0& 0& -0.0025& -0.0025& 0& 0& 0& 0\\ 0& 0.00025& -0.00025& 0.00005& 0& 0& 0.00025& 0.00025& 0& 0& 0& 0\\ -0.00025& 0& 0& 0& 0.00005& 0& 0& 0& 0.00025& -0.00025& 0.00025& -0.00025\\ 0.00025& 0& 0& 0& 0& 0.00005& 0& 0& 0.00025& -0.00025& -0.00025& 0.00025\\ 0& 0.0025& -0.0025& 0.00025& 0& 0& 0.005& 0.0025& 0& 0& 0& 0\\ 0& 0.00125& -0.0025& 0.00025& 0& 0& 0.0025& 0.000375& 0& 0& 0& 0\\ 0.00125& 0& 0& 0& 0.00025& 0.00025& 0& 0& 0.00625& -0.005& 0& 0\\ 0& 0& 0& 0& -0.00025& -0.00025& 0& 0& -0.005& 0.0075& -0.0025& 0\\ -0.005& 0& 0& 0& 0.00025& -0.00025& 0& 0& 0& -0.0025& 0.0075& -0.005\\ 0.005& 0& 0& 0& -0.00025& 0.00025& 0& 0& 0& 0& -0.005& 0.0075\end{array}\right)$

Q_est为12×12维矩阵，与时间无关。

步骤(2)：根据角加速度估计值，建立角速度动力学方程，根据角加速度的估计方差，给出动力学方程噪声，并将动力学方程离散化，时间步长为0.01秒。

令状态变量为角速度矢量ω＝[ω_x>y>z]^T，在离散情况下，角速度差分方程可表示为，

$\begin{array}{c}{\omega }_{x,k}={\omega }_{x,k-1}+0.01\times {\stackrel{·}{\omega }}_x+0.01\times w_{x,k-1}\\ {\omega }_{y,k}={\omega }_{y,k-1}+0.01\times {\stackrel{·}{\omega }}_y+0.01\times w_{y,k-1}\\ {\omega }_{z,k}={\omega }_{z,k-1}+0.01\times {\stackrel{·}{\omega }}_z+0.01\times w_{z,k-1}\end{array}---\left(9\right)$

其中ω_x,k、ω_y,k、ω_z,k分别为k时刻的X、Y、Z三个方向的角速度值，ω_x,k-1、ω_y,k-1、ω_z,k-1分别为k-1时刻的X、Y、Z三个方向的角速度值，w_x,k-1,w_y,k-1,w_z,k-1为k-1时刻角加速度解算的随机噪声，定义过程量

$u={\left(\begin{array}{ccc}0.01{\stackrel{·}{\omega }}_x& 0.01{\stackrel{·}{\omega }}_y& 0.01{\stackrel{·}{\omega }}_z\end{array}\right)}^T---\left(10\right)$

实际计算中u可以根据式(7)估计的角加速度乘以时间步长得到，

$u={\left(\begin{array}{ccc}0.01{\stackrel{\widehat{·}}{\omega }}_x& 0.01{\stackrel{\widehat{·}}{\omega }}_y& 0.01{\stackrel{\widehat{·}}{\omega }}_z\end{array}\right)}^T---\left(11\right)$

将式(11)代入式(9)，同时写成矢量形式，角速度ω的离散化状态动力学模型可表示为

ω_k＝ω_k-1+u_k-1+0.01w_k-1>

根据式(8)得到的角加速度估计方差是加速度计随机误差的线性组合，为矩阵Q_est的第一行至第三行和第一列至第三列组成的矩阵，式(12)动力学模型的过程噪声方差为

$Q_{\omega }={\left(\Delta t\right)}^2{Q}_{est}(1:3,1:3)={10}^{-6}\times \left(\begin{array}{ccc}0.625& 0& 0\\ 0& 0.375& -0.25\\ 0& -0.25& 0.5\end{array}\right)---\left(13\right)$

角速度状态变量滤波初值取为

${\hat{\omega }}_{0/0}={\left(\begin{array}{ccc}{\hat{\omega }}_{x0}& {\hat{\omega }}_{y0}& {\hat{\omega }}_{z0}\end{array}\right)}^T={\left(\begin{array}{ccc}0& 0& 0\end{array}\right)}^T---\left(14\right)$

角速度状态变量的预测值为

${\hat{\omega }}_{k/k-1}={\hat{\omega }}_{k-1/k-1}+u_{k-1}---\left(15\right)$

预测值的方差为

P_k/k-1＝P_k-1/k-1+Q_ωk-1>

步骤(3)：根据角速度二次项的估计值和估计方差，建立角速度测量方程以及测量噪声的方差矩阵，并将方程离散化；

将式(7)给出的角速度二次项估计值作为滤波过程中的测量模型，同时考虑加速度计测量所存在的随机误差，所得角速度二次项的测量模型可以表示为

$h_k\left({\omega }_k\right)={{\left(\begin{array}{cccccc}{\omega }_x{\omega }_y& {\omega }_x{\omega }_z& {\omega }_y{\omega }_z& {\omega }_x^2& {\omega }_y^2& {\omega }_z^2\end{array}\right)}_k}^T+v_k---\left(17\right)$

其中v_k是角速度二次项估计值的随机误差。根据式(8)可知，该随机误差方差是加速度计随机误差的线性组合，为矩阵Q_est的第7行到第12行和第7列到第12列组成的方阵，为

$R_k=Q_{est}(7:12,7:12)=\left(\begin{array}{cccccc}0.005& 0.0025& 0& 0& 0& 0\\ 0.0025& 0.00375& 0& 0& 0& 0\\ 0& 0& 0.00625& -0.005& 0& 0\\ 0& 0& -0.005& 0.0075& -0.0025& 0\\ 0& 0& 0& -0.0025& 0.0075& -0.005\\ 0& 0& 0& 0& -0.005& 0.0075\end{array}\right)---\left(18\right)$

式(12)所示动力学模型的过程噪声与式(17)所示测量模型噪声的协方差矩阵为矩阵式(8)所示Q_est的第1行到3行和第7列到12列组成的矩阵

$M_k=Q_{est}(1:3,7:12)\Delta t={10}^{-4}\times \left(\begin{array}{cccccc}0& 0& 0.125& 0& -0.5& 0.5\\ 0.25& 0.125& 0& 0& 0& 0\\ -0.25& -0.25& 0& 0& 0& 0\end{array}\right)---\left(19\right)$

M_k为动力学模型过程噪声与测量模型噪声协方差阵，为3×6维矩阵。

测量模型的Jacobian矩阵H_k见式(20)，Kalman滤波增益矩阵的更新方程见(21)，滤波方差的修正方程见(22)，角速度变量的修正方程见(23)式。

滤波初始方差取为

$P_{0/0}=\left(\begin{array}{ccc}0.00625& 0& 0\\ 0& 0.00375& 0\\ 0& 0& 0.005\end{array}\right)$

根据式(15)、式(16)、式(21)、式(22)、式(23)给出的递推公式，式(11)、式(13)、式(18)、式(19)、式(20)给出的相关变量计算公式，以及滤波初值和初始方差P_0/0，即可完成无陀螺惯性系统角速度的递推估计。

所得结果列于图3～6中，由图3可见X方向角速度估计的误差不会随时间的增加而变大,最大值不超过0.01弧度/秒，有图4可见，Y方向角速度估计的误差不会随时间的增加而变大，同样的图5所示为Z方向角速度估计误差不会随时间的增加而变大。图6中所示为X、Y、Z三个方向角速度估计值的均方差，可以看出均方差随时间增加缓慢变小。这说明本发明提供的角速度估计方法所得角速度误差估计不随时间增加而变大，可以有效提供无陀螺惯性测量系统角速度估计精度。

本发明所提出无陀螺惯性测量系统约束角速度估计方法具有以下优点：(1)估计方法步骤清晰，简单明了，合理可行；(2)角速度估计误差不随测量时间增加而变大，等价于陀螺仪的角速度测量功能；(3)该方法可实时递推计算。因此该方法可以有效提高无陀螺惯性测量系统角速度估计精度。

本领域技术人员将清楚本发明的范围不限制于以上讨论的示例，有可能对其进行若干改变和修改，而不脱离所附权利要求书限定的本发明的范围。尽管己经在附图和说明书中详细图示和描述了本发明，但这样的说明和描述仅是说明或示意性的，而非限制性的。本发明并不限于所公开的实施例。

通过对附图，说明书和权利要求书的研究，在实施本发明时本领域技术人员可以理解和实现所公开的实施例的变形。在权利要求书中，术语“包括”不排除其他步骤或元素，而不定冠词“一个”或“一种”不排除多个。在彼此不同的从属权利要求中引用的某些措施的事实不意味着这些措施的组合不能被有利地使用。权利要求书中的任何参考标记不构成对本发明的范围的限制。