CN102519463A - 一种基于扩展卡尔曼滤波的导航方法及装置 - Google Patents

Abstract

本发明公开了一种基于扩展卡尔曼滤波的导航方法及装置。通过对实际导航系统建立模型，模型包括状态模型、状态转移模型和观测模型，对导航方法中滤波过程进行分阶优化以及对矩阵运算进行改进。分析十一阶参数之间的联系与独立性，对矩阵运算进行研究、对系统进行拆分处理。从而达到了运算周期短，减少计算量，节省存储空间从而提高效率的目的,同时也降低了对硬件的要求。

Description

一种基于扩展卡尔曼滤波的导航方法及装置

技术领域

本发明适用于高实时性、高精度要求的的导航系统领域，尤其适用于一种利用低成本设备获取高精度结果的基于扩展卡尔曼滤波的导航方法及装置。

背景技术

导航系统的主要作用是提供实时地位置、速率、姿态信息，并且可以使之按照预先设定的路径行走。但是由于外界的诸多干扰使有用信号参杂很多噪声，卡尔曼滤波可以解决此类问题。卡尔曼滤波是一种递归估计，只要获知上一时刻状态的估计值和前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器将位置、速度、姿态角信息融合起来，并且通过预测与更新来滤除噪声，从而使获取的信息更准确，使控制更加精确。

卡尔曼滤波技术的发明给数字信号处理带来了一场意义重大的革命。卡尔曼滤波算法虽然很成熟，但是目前算法大多只支持少数状态的滤波处理。由于算法中频繁用到矩阵运算，其中包括矩阵相乘，相加减以及求逆，如果按照普通的运算法则将会消耗极大内存空间，并且执行时间冗长，效率低。所以设计高效的算法是很有必要的。此基于扩展卡尔曼滤波的导航方法及装置是对十一个状态量进行融合滤波，通过研究矩阵运算来提高算法效率，从而降低滤波周期，使之在高频低周期无操作系统的导航硬件中顺利可靠运行。这样可以减少对导航系统硬件的要求，降低成本。

发明内容

本发明主要目的是公开一种基于扩展卡尔曼滤波的导航方法，对导航方法中滤波过程进行分阶优化以及对矩阵运算进行改进。分析十一阶参数之间的联系与独立性，将滤波系统拆分成两个子系统即一个五阶系统，一个六阶系统。通过研究矩阵的特征，改进通用的算法，减少计算量，节省存储空间从而达到提高效率的目的。

本发明目的还在于公开实现基于上述方法的一种导航装置。

本发明的基于扩展卡尔曼滤波的导航方法通过如下步骤实现：

(1)采集数据：由数据采集模块获取当前状态观测值，包括角速度p、q、r；线加速度a_x、a_y、a_z；地磁通量H_x、H_y、H_z；速度和位置信息x、y、z、u、v、w；

(2)预处理数据：将步骤(1)获得角速度、线加速度、地磁通量转换成所需要的姿态角，再进一步转化为四元数；

(3)建立模型：利用步骤(1)、(2)获得数据，对复杂非线性导航系统进行泰勒展开降阶线性化，分别建立状态模型、状态转移模型和观测模型；

(4)扩展卡尔曼滤波：根据卡尔曼滤波基本方程，利用步骤(3)建立的状态量模型和上次状态的估计值来预测当前状态的估计值，作为对当前状态观测值的修正；

(5)输出数据：输出修正后的当前状态的估计值。

传感器元件得到的原始信息是不能直接被赋值给滤波器，必须经过一定的转换得到所需的姿态角(横滚角

俯仰角θ、偏航角ψ)。

其中横滚角与俯仰角是通过线加速度来转换得到，偏航角是通过地磁通量来转换得到。在得到姿态角后需要通过转换公式将姿态角转化为四元数。转换公式如下所述：

线加速度转角度：

$g=\sqrt{a_x^2+a_y^2+a_z^2}$

φ＝atan 2(a_y，a_z)                        (1)

θ＝a tan 2(a_x，g)

地磁通量转角度：

$\mathrm{Xh}=H_x={\hat{H}}_x\cos \mathrm{\θ}+{\hat{H}}_y\sin \mathrm{\θ}\sin \mathrm{\φ}-{\hat{H}}_z\sin \mathrm{\θ}\cos \mathrm{\φ}$

$\mathrm{Yh}=H_y={\hat{H}}_y\cos \mathrm{\φ}+{\hat{H}}_z\sin \mathrm{\φ}---\left(2\right)$

$\mathrm{\ψ}=\arctan \left(\frac{H_y}{H_x}\right)=\arctan \left(\frac{\mathrm{Xh}}{\mathrm{Yh}}\right)$

角度转四元数：

$\mathrm{shpsi}=\sin \frac{\mathrm{\ψ}}{2}$ $\mathrm{chtheta}=\cos \frac{\mathrm{\θ}}{2}$

$\mathrm{shtheta}=\sin \frac{\mathrm{\θ}}{2}$ $\mathrm{chpsi}=\cos \frac{\mathrm{\ψ}}{2}$

$\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}\mathrm{chphi}*\mathrm{chtheta}*\mathrm{chpsi}+\mathrm{shphi}*\mathrm{shtheta}*\mathrm{shpsi}\\ -\mathrm{chphi}*\mathrm{shtheta}*\mathrm{shpsi}+\mathrm{shphi}*\mathrm{chtheta}*\mathrm{chpsi}\\ \mathrm{chphi}*\mathrm{shtheta}*\mathrm{chpsi}+\mathrm{shphi}*\mathrm{chtheta}*\mathrm{shpsi}\\ \mathrm{chphi}*\mathrm{chtheta}*\mathrm{shpsi}-\mathrm{shphi}*\mathrm{shtheta}*\mathrm{chpsi}\end{array}\right]---\left(3\right)$

在实际工程中，大多数系统为非线性或系统噪声非白噪声，普通卡尔曼滤波器就不再是最优，但系统的性能通常是可接受的次优，采用泰勒级数舍项的线性化方法使非线性系统近似为线性系统。建立的模型主要包括状态量模型、状态转移模型以及观测模型：

假设非线性系统的模型是

Z(k)＝h(X(k)，k)

这个非线性模型的状态量就是运载体的位置、速度、四元数表示出的姿态以及重力加速度，即

X＝[x y z u v w q₀ a₁ q₂ q₃ g]^T

观测量就是位置、速度以及姿态角，即

Z＝[x y z u v w φθψ]^T

(1)状态量模型：

$f(x,u)=\left[\begin{array}{c}\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 1-2(q_1^2+q_2^2)\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]\\ \left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]-\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ 1-2(q_1^2+q_2^2)\end{array}\right]g-\left[\begin{array}{ccc}0& -r& q\\ r& 0& -p\\ -q& p& 0\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]\\ \frac{1}{2}\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\\ 0\end{array}\right]$

(2)状态转移模型：

状态转移矩阵即为我们在卡尔曼基本方程中所描述的f函数，分别对状态向量的偏导，记为

$\frac{\∂f}{\∂X}=\left[\begin{array}{ccccccccccc}\frac{\∂f}{\∂x}& \frac{\∂f}{\∂y}& \frac{\∂f}{\∂z}& \frac{\∂f}{\∂u}& \frac{\∂f}{\∂v}& \frac{\∂f}{\∂w}& \frac{\∂f}{\∂q_0}& \frac{\∂f}{\∂q_1}& \frac{\∂f}{\∂q_2}& \frac{\∂f}{\∂q_3}& \frac{\∂f}{\∂g}\end{array}\right]$

(3)观测模型：

状态观测量可以为位置、速度、角度，因此状态观测模型可以描述为

位置观测模型

H_xyz＝[I^3×3 0^3×7]

速率观测模型

H_uvw＝[0^3×3 I^3×3 0^3×4]

较欧拉角表示法，由于四元数表示法表现出的优越性，所以我们在导航系统中将姿态用四元数表示，四元数与欧拉角之间的转换如下：

将此模型线性化为

所述尔曼滤波是一个迭代的算法，通过预测与更新来实现。所述扩展卡尔曼滤波是基于五个基本方程式来实现，主要包括预测部分和修正部分，具体步骤如下：

预测部分：

状态进一步预测： ${\hat{X}}_{k,k-1}=f[{\hat{X}}_{k-1},k-1]$

状态估计量： ${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-h[{\hat{X}}_{k,k-1},k\left]\right]$

修正部分：

滤波增益阵： $K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$

进一步预测方差阵：

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+\mathrm{\Γ}[{\hat{X}}_{k-1},k-1]Q_{k-1}{\mathrm{\Γ}}^T[{\hat{X}}_{k-1},k-1]$

其中，其中Γ是噪声输入矩阵，Q_k是对称非负定矩阵，是系统随机过程的白噪声方差阵，R_k是随机观测的白噪声方差阵，为正定矩阵。

估计误差方差阵：P_k＝[I-K_kH_k]P_k，k-1

EKF的实质是利用上一次的状态估计量来预测本次的状态：首先将上次的状态估计量代入系统状态方程，得到状态一步预测，再将

代入观测方程得到观测量的预测值

然后利用观测量的测量值与预测值之间的偏差来修正状态一步预测

从而得到状态估计量

正是由于利用了不同方式所得到的观测值的偏差来修正状态估计量，才使得各种白噪声干扰得到了有效抑制。

算法的优化主要体现在系统拆分及矩阵运算优化。

(1)根据EKF算法对系统进行拆分

普通运算法则是将十一阶状态量作为整体处理，但从系统分析角度来看，其中重力加速度的滤波必然为一条直线，位置的状态方程中并没有位置量，它们是通过三个速度量来计算的，所以如果将位置量单独提出来滤波那么位置状态方程对位置量的求导都将为零，这将导致不能使用EKF来滤波消除误差。因此可将十一阶系统拆分成两个小系统：

系统一：将速度位置变量既作为状态向量也作为观测向量；

系统二：将四元数作为状态向量，将三个角度作为观测量。

拆分与不拆分前后EKF浮点元算量对比如下表所示：

系统拆分方法	浮点计算量
		不拆分10阶	13073
拆分后	3898

从上表看到，若不拆分计算需要的浮点计算量是13073，而拆分后的浮点计算量为3898，仅为原来的29.8％，大大提高了运算效率。由于位置速度状态方程中都包含有四元数，而四元数状态方程又与位置速度无关，所以应该将四元数滤波放在前面。

(2)对扩展卡尔曼滤波方程组性质研究以及对两矩阵相乘结果为对称矩阵算法的改进

(a)对扩展卡尔曼滤波方程组性质研究

对于式子 $P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+\mathrm{\Γ}[{\hat{X}}_{k-1},k-1]Q_{k-1}{\mathrm{\Γ}}^T[{\hat{X}}_{k-1},k-1],$ 当滤波第一次启动的时候，P₀是对称矩阵，，其中Φ_k，k-1是方阵，

是对称矩阵。

根据定理及推论：若A为n阶对称矩阵，则必有正交阵P，使P^-1AP＝P^TAP＝Λ，其中Λ是以A的n个特征值为对角元素的对角矩阵。若A为n阶是对称可逆矩阵，则A的逆矩阵必为对称矩阵。那么当k＝1，2，3…N时，P_k，k-1及P_k都是对称矩阵。

(b)对两矩阵相乘结果为对称矩阵算法的改进

如果已知c＝a*b运算中结果c为对称矩阵，可以利用矩阵对称的相关性质将上述两个矩阵相乘的算法进行简化：首先在初始化c阵时并不用将其全部元素都赋值为零，而仅仅将其下(上)三角元素初始化为零值，然后在计算c的每个元素时只计算相应下(上)三角矩阵元素即可，再利用对称特点赋值给剩下的上(下)三角，这样就大大减少了运算量。

普通算法计算矩阵结果为对角矩阵的情况下，初始化c的复杂度为O(n²)，计算矩阵c的复杂度为O(mn²)整个算法的复杂度即为O((m+1)n²)，在初始化c阵时每次进行一次赋值运算，在计算c阵元素时每次进行一次乘法运算、一次加法运算及一次赋值运算，如果将赋值、两数相加及两数相乘都算作一个单位时间，则这两个矩阵相乘的时间消耗为T(n)＝(3m+1)n²。

在算法经过改进后，初始化c阵下三角元素的复杂度为O(n(n+1)/2)，计算矩阵c的复杂度为O(mn(n+1)/2)，整个算法的复杂度为O((m+1)n(n+1)/2)，在初始化c阵下三角矩阵时进行了(n+1)/2次赋值运算，在计算c阵元素时最内层循环了m*n(n+1)/2次，上三角矩阵元素赋值n(n+1)/2次，所以这两个矩阵相乘的时间消耗为对比T(n)和T_s(n)，随着n的增大，结果为普通矩阵的矩阵相乘所消耗时间比结果为对称矩阵的矩阵相乘所消耗时间多。

(3)对矩阵求逆算法的改进

高斯-约当消元求逆是一种常用的矩阵求逆算法，特别是全选主元的Gauss-Jordan求逆法具有直观、简单、稳定的特点。其步骤如下：

(3.1)从第K行、第K列开始的右下角子阵中选取绝对值最大的元素，同时要将这个最大元素行号和列号都记录下来，然后通过对矩阵中的元素进行行列对调，使得这个最大元素在主元的位置。这个过程就是全选主元的过程。

(3.2)m(k，k)＝1/m(k，k)

(3.3)m(k，j)＝m(k，j)*m(k，k)，j＝0，1，...，n-1；j！＝k

(3.4)m(i，j)＝m(i，j)-m(i，k)*m(k，j)，i，j＝0，1，...，n-1；i，j！＝k

(3.5)m(i，k)＝-m(i，j)*m(k，k)，i＝0，1，...，n-1；i！＝k

(3.6)如果全选主元过程中有行列交换，就必须对这些行列交换进行恢复，即从最后交换的行列一直往前恢复，不过要注意的是，如果原来是行交换，那么在恢复的时候就要用列交换，反之亦然。

如果不全选主元，直接应用Gauss-Jordan算法求逆，那么就是上述的五步，然而，不选主元，一旦碰到m(k，k)＝0，那么第二步就会出错，因此，不选主元的Gauss-Jordan求逆是一种非常不稳定的算法，在应用过程中很容易出错。但是，它消耗的时间显然比全选主元的算少得多。全选主元的算法时间大约为n(6n²+15n+7)，不全选主元的算法消耗时间为4n³+n。而在实际应用中，绝大部分情况下，导航系统上的EKF滤波过程所用的求逆都满足m(k，k)≠0，为了综合两种算法的优点，将对Gauss-Jordan进行改进：以不选主元的求逆算法为主要算法，在每次进行求逆运算前都拷贝一份待求逆矩阵，然后应用不选主元的算法，如果在求逆过程中发现m(k，k)＝0，则放弃使用不选主元算法而改用全选主元算法。

在拷贝矩阵时需要执行n²次赋值运算，接着在每次循环时需要做两次判断主元是否为零，所以一共要判断2n次；计算除k行k列元素外的其它元素时每个最里层for循环内都需要进行一次乘法、一次除法、一次减法以及两次赋值运算，一共需要的次数为：

计算除m(k，k)外k行k列元素每次需要执行3次操作，一共需要的次数为3n(n-1)；最后计算m(k，k)需要执行一次除法、一次赋值操作，所以一共是2n次操作。

综合可以得到对称矩阵求逆算法的时间复杂度为：

通过对普通求逆算法以及改进的求逆算法进行对比得到全选主元高斯求逆法的时间消耗比较多，随着n的增大，它的计算量有可能是不选主元高斯求逆的两倍甚至更多。在n＝10～20这个区域内，改进后算法的计算量约是改进前不选主元高斯求逆算法的2/3。

最后测验得到：不做任何修改的的EKF算法执行1000次滤波平均时间为71.944秒，而进行拆分并且优化矩阵运算后执行1000次滤波平均时间为35.245秒。所以此算法发明是高效的。

一种基于扩展卡尔曼滤波的导航装置，如图1所示，包括数据采集模块、数据处理系统和数据反馈模块，所述数据采集模块用于采集当前状态观测值，所述信号处理系统用于将当前状态观测值转换为所需姿态角并进一步转换为所需模型矩阵，所述数据反馈模块用于输出当前状态估计值。

所述导航装置还包括扩展卡尔曼滤波器，所述扩展卡尔曼滤波器接收上述信号处理系统的信号，通过扩展卡尔曼滤波，将当前状态观测值修正为估计值，并反馈给数据反馈模块，得到精确导航数据。

所述数据采集模块包括电子罗盘、全球定位系统和惯性测量元件；所述电子罗盘用于测量地磁通量，所述全球定位系统用以测量速度和位置信息。

所述惯性测量元件包括加速度计和陀螺仪；所述加速度计用于测量线加速度，陀螺仪用于测量角速度。

所述扩展卡尔曼滤波器和数据处理系统由ARM7芯片提供；所述ARM7芯片通过SPI接口与惯性导航模块和电子罗盘通讯；所述ARM7芯片通过DEBUG口与GPS模块通讯。

所述数据处理系统由算法实现，将惯性测量元件获取的角速度和线加速度，通过公式(1)转换为横滚角和俯仰角。电子罗盘获取的地磁通量通过公式(2)得到偏航角ψ。再通过公式(3)得到四元数。最后将全球定位系统获得的位置、速度信息，与四元数和重力加速融合，得到状态量。

本发明与现有技术相比具有如下优点和有益效果：

1、本发明可移植性强，适用性广。

2、本发明涉及的状态量多，但运行速度快，运算周期短，能在30赫兹频率下可靠运行。

3、本发明所用的硬件精度不高，但效果好，节省了使用成本。

附图说明

图1是发明的系统结构框图。

图2是实施例采用的处理器结构图。

图3是实施例中数据采集模块的惯性测量元件硬件结构图。

图4是实施例中数据采集模块的电子罗盘硬件结构图。

图5是拆分运算的流程框图。

具体实施方式

下面将结合实施例及附图对本发明作进一步的说明，但本发明的实施不限于此。

如图2～4所示，本发明具体实施例采用arm7(具有0.9MIPS/MHz的三级流水线和冯·诺伊曼结构)系列微处理器，型号为ADIS16350的惯性测量元件IMU以及型号为PNI11096的电子罗盘COMPASS。

图1是导航系统装置的结构框图，包括数据采集模块、数据处理系统、扩展卡尔曼滤波器和数据反馈模块。

数据采集模块是由惯性测量元件IMU、电子罗盘COMPASS、全球定位系统组合而成。利用IMU来获取角速度和线加速度。IMU数据是通过SPI口来获取的。读取传感器中的数据时，先给出要读取数据的寄存器地址，IMU收到信号后按照地址把相应的数据输出。写入内容的特征是：最前两位为1、0，接着是6位地址、8位数据命令。读数据时要先对序列做修正，将DIN的前两位改为00，接着输入地址信息，后八位命令可忽略。在下一个数据帧中DOUT序列就包含16位所需数据。

再通过电子罗盘COMPASS获得地磁通量，其工作步骤为：

(1)将SSNOT引脚电平拉低

(2)复位罗盘(TESET为高电平)

(3)主机发送8位的命令，PNI即执行命令

(4)PNI进行数据处理

(5)DRDY置1表示数据已经准备好，接着的16个时钟周期将数据移到MISO线

(6)下次读取数据时从(2)步骤开始，如果不使用PIN了就将SSNOT置1。

COMPASS发送数据，只要有数据发送，DRDY＝＝1，从而连续地读COMPASS三个轴的地磁通量。

速度位置信息直接由GPS得出。GPS接收板的信息通过调试单元DEBUG以中断方式和ARM板通信，波特率是38400。读取GPS信息，先使能RXRDY中断，GPS把数据一个字符一个字符直接发送到ARM7的接收器保持寄存器中，然后CPU响应RXRDY中断进入中断服务程序，从DEBUG口获取速度位置信息。

数据处理系统由算法实现，将惯性测量元件获取的角速度和线加速度，通过公式(1)转换为横滚角和俯仰角。电子罗盘获取的地磁通量通过公式(2)得到偏航角ψ。再通过公式(3)得到四元数。最后将全球定位系统获得的位置、速度信息，与四元数和重力加速融合，得到状态量并输入到扩展卡尔曼滤波器。

数据反馈模块：ARM7与PC104(一种工业计算机总线标准)之间通过串口0(USARTO)通信，ARM通过串口0把所有集成信息发送给PC104，而PC104也会通过串口0把数据发送给ARM；它的波特率是115200；传输数据的方式是DMA中断方式。这些信息反馈给控制系统产生实时控制，同时也通过无线装置发射给地面。

图5是算法拆分运算示意图，此发明采用拆分处理即分成两个子系统分别运算。首先是对四元数进行滤波，滤波步骤如上所述，接着将滤波后的四元数作为速度位置的滤波参数，再按照上述流程进行滤波处理。

具体实施不受上述数据采集所需的硬件约束，能够将接口所需的数值赋予相应的参数即可。建立的模型与算法研究都在本发明的保护范围之内。

Claims (10)

1.一种基于扩展卡尔曼滤波的导航方法，其特征在于包括以下步骤：

（1）采集数据：由数据采集模块获取当前状态观测值，包括角速度p、q、r；线加速度                                                

；地磁通量

；速度和位置信息

；

（2）预处理数据：将步骤（1）获得角速度、线加速度、地磁通量转换成所需要的姿态角，再进一步转化为四元数；

（3）建立模型：利用步骤（1）、（2）获得数据，对复杂非线性导航系统进行泰勒展开降阶线性化，分别建立状态模型、状态转移模型和观测模型；

（4）扩展卡尔曼滤波：根据卡尔曼滤波基本方程，利用步骤（3）建立的模型和上次状态的估计值来预测当前状态的估计值，作为对当前状态观测值的修正；

（5）输出数据：输出修正后的当前状态的估计值。

2.根据权利要求1所述的基于扩展卡尔曼滤波的导航方法，其特征在于步骤（2）姿态角包括横滚角

、俯仰角和偏航角

，由下式得到：

；

         ；

所述四元数矩阵转换式：

；

其中，

；

；

；

；

；

。

3.根据权利要求1所述的基于扩展卡尔曼滤波的导航方法，其特征在于步骤（3）中所述状态模型以位置、速度、四元数以及重力加速度为参数对当前状态进行修正得到矩阵；所述是状态转移模型由状态模型矩阵的元素分别对位置、速度、四元数以及重力加速度求偏导而得；观测模型是由观测量分别对四元数求偏导而得。

4.根据权利要求1所述的基于扩展卡尔曼滤波的导航方法，其特征在于步骤（4）扩展卡尔曼滤波通过以下方式处理矩阵求逆运算：判断主元是否为零：是，则采用高斯-约当算法；否，则采用全选主元算法。

5.根据权利要求1所述的基于扩展卡尔曼滤波的导航方法，其特征在于步骤（4）扩展卡尔曼滤波将状态模型和观测模型分为两个子系统，对于具有不同特征的状态量进行分块、按序滤波：系统一将速度、位置变量既作为状态向量也作为观测向量；系统二将四元数作为状态向量，将姿态角作为观测向量。

6.根据权利要求1所述的基于扩展卡尔曼滤波的导航方法，如果已知矩阵运算结果为对称矩阵，可以通过下面方式来对算法进行简化：  

（1）初始化结果存放的矩阵，将其下（上）三角元素初始化为零；

（2）在计算过程中只计算相应的下（上）三角矩阵元素；

（3）将下（上）三角矩阵中的元素对称赋给上（下）三角矩阵。

7.一种基于扩展卡尔曼滤波的导航装置，包括数据采集模块、数据处理系统和数据反馈模块，所述数据采集模块用于采集当前状态观测值，所述信号处理系统用于将当前状态观测值转换为所需姿态角并进一步转换为所需模型矩阵，所述数据反馈模块用于输出当前状态估计值，其特征在于还包括扩展卡尔曼滤波器，所述扩展卡尔曼滤波器接收上述信号处理系统的信号，通过扩展卡尔曼滤波，将当前状态观测值修正为估计值，并反馈给数据反馈模块，得到精确导航数据。

8.根据权利要求7所述的扩展卡尔曼滤波导航装置，其特征在于所述数据采集模块包括电子罗盘、全球定位系统和惯性测量元件；所述电子罗盘用于测量地磁通量，所述全球定位系统用以测量速度和位置信息。

9.根据权利要求8所述的扩展卡尔曼滤波导航装置，其特征在于所述惯性测量元件包括加速度计和陀螺仪；所述加速度计用于测量线加速度，陀螺仪用于测量角速度。

10.根据权利要求9所述的扩展卡尔曼滤波导航装置，其特征在于所述扩展卡尔曼滤波器和数据处理系统由ARM7芯片提供；所述ARM7芯片通过SPI接口与惯性导航模块和电子罗盘通讯；所述ARM7芯片通过DEBUG口与GPS模块通讯。

Images