CN102944888A - 一种基于二阶扩展卡尔曼的低运算量gps定位方法 - Google Patents

Abstract

本发明公开了一种基于二阶扩展卡尔曼的低运算量GPS定位方法，包括如下步骤：获取本地k-1时刻的定位结果，计算出当前k时刻的先验估计状态量和先验估计误差协方差；建立观测量估计方程；计算卡尔曼增益K_k；计算当前k时刻的定位结果和误差协方差。本发明在达到EKF2性能的同时降低运算量，适合低端硬件。

Description

一种基于二阶扩展卡尔曼的低运算量GPS定位方法

技术领域

本发明属于GPS定位方法领域，涉及一种GPS定位解算方法，具体涉及一种基于二阶扩展卡尔曼的低运算量GPS定位方法，该方法的计算量适用于低端硬件。

背景技术

目前在GPS定位领域，常用的定位算法有最小二乘法(WLS)、扩展卡尔曼滤波算法(EKF)、二阶扩展卡尔曼滤波算法(EKF2)。WLS是最基本的定位结算算法，但是在动态多径的条件下，WLS的定位效果非常不理想。因此，我们采用EKF2的定位解算算法。卡尔曼滤波器由一系列递归数学公式描述。它提供了一种高效可计算的方法来估计过程的状态，并使估计均方误差最小，是一种高效的预测滤波定位技术。在采用扩展卡尔曼算法进行定位时，可以有效的抑制动态时由多径带来的定位点抖动严重的问题，同时，在静态时也可以达到比最小二乘要好得多的定位结果。但是，一般的EKF2算法对于计算量的需求比较大，对硬件要求较高，不适合低端硬件。本发明提供一种低运算量的EKF2,算法，在达到EKF2性能的同时降低运算量。

发明内容

发明目的：针对上述现有技术存在的问题和不足，本发明的目的是提供一种基于二阶扩展卡尔曼的低运算量GPS定位方法，在达到EKF2性能的同时降低运算量，适合低端硬件。

技术方案：为实现上述发明目的，本发明采用的技术方案为一种基于二阶扩展卡尔曼的低运算量GPS定位方法，包括如下步骤：

(1)获取本地k-1时刻的定位结果，计算出当前k时刻的先验估计状态量和先验估计误差协方差：

${\hat{X}}_k^{\_}=A\·{\hat{X}}_{k-1}$ ${\hat{P}}_k^{\_}=A{\hat{P}}_{k-1}{A}^T+Q_k$

其中，

为k时刻的用户接收机的先验估计状态量，

为k-1时刻的用户接收机的后验估计状态量，

为k时刻的用户接收机的先验估计误差协方差，

为k-1时刻的用户接收机的后验估计误差协方差，A为转移矩阵： $A=\left[\begin{array}{cccccccc}1& 0& 0& \mathrm{\Δt}& 0& 0& 0& 0\\ 0& 1& 0& 0& \mathrm{\Δt}& 0& 0& 0\\ 0& 0& 1& 0& 0& \mathrm{\Δt}& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& \mathrm{\Δt}\\ 0& 0& 0& 0& 0& 0& 1& 0\end{array}\right]$

其中，Δt为两次定位间的时间间隔；Q_k为k时刻的过程噪声误差，

$Q_k=\left[\begin{array}{cccc}{10}^{-4}{I}_{3\×3}& 0_{3\×3}& 0& 0\\ 0_{3\×3}& {10}^3{I}_{3\×3}& 0& 0\\ 0& 0& {10}^5& 0\\ 0& 0& 0& 100\end{array}\right]$

其中，I_3×3为3阶单位矩阵，0_3×3为3阶全零方阵；

(2)建立观测量估计

方程：

$Z_k^{\_}=h_k\left({\hat{X}}_k^{\_}\right)+m_{1k}$

其中，

$h={[{\mathrm{\ρ}}_1,\·\·\·,{\mathrm{\ρ}}_n,{\stackrel{\·}{\mathrm{\ρ}}}_1,\·\·\·,{\stackrel{\·}{\mathrm{\ρ}}}_n]}^T$ ${\mathrm{\ρ}}_i=\left|\right|r_i^s-\mathrm{ru}\left|\right|+{\mathrm{\δ}}_{\mathrm{tu}}+{\mathrm{\ϵ}}_{\mathrm{\ρi}}$

${\stackrel{\·}{\mathrm{\ρ}}}_i=\left|\right|{(r_i^s-\mathrm{ru})}^T\·(V_i^s-\mathrm{Vu})\left|\right|/\left|\right|r_i^s-\mathrm{ru}\left|\right|+{\mathrm{\δ}}_{\mathrm{fu}}+{\mathrm{\ϵ}}_{{\stackrel{\·}{\mathrm{\ρ}}}_i}$

$m_{1k}=\frac{1}{2}\underset{p}{\overset{{2n}_k}{\mathrm{\Σ}}}{e}_p\·{(X_k-{\hat{X}}_k^{\_})}^T\·H_{2k}^p\·(X_k-{\hat{X}}_k^{\_})$

表示k时刻的测量值h_k中的第p个函数在

处的二阶导数，x_j、x_l分别为状态量X_k中的任意两个元素，p∈[1,2n_k]，

为卫星的位置坐标向量，

为卫星的速度坐标向量，

为卫星的伪距测量误差，

是伪距变化率的测量误差，n为卫星个数，i∈[1,n]，e_p为2n_k维单位矩阵中第p列向量,n_k为k时刻卫星个数，δ_tu、δ_fu分别为钟差和钟差变化率；

(3)计算卡尔曼增益K_k：

$K_k=({\hat{P}}_k^{\_}\·H_{1k}^T+\frac{1}{2}\·\det X_k\·m_{1k}^T){\left(\begin{array}{c}{H}_{1k}\·{\hat{P}}_k^{\_}\·H_{1k}^T+R_k+\frac{1}{2}\·H_{1k}\·\det X_k\·m_{1k}^T+\frac{1}{2}\·m_{1k}\·\det X_k^T\·H_{1k}\\ +\underset{p,q}{\mathrm{\Σ}}\frac{1}{4}(\det X_k^T\·H_{2k}^p\·{\hat{P}}_k^{\_}\·H_{2k}^q\·\det X_k)e_p\·e_q^T\end{array}\right)}^{-1}$

其中，X_k为k时刻的用户接收机的真实状态量，

表示k时刻的测量值h_k在

处的一阶导数，

表示k时刻的测量值h_k中的第q个函数在

处的二阶导数，R_k为伪距和多普勒的测量误差协方差：

其中v_k表示测量噪声矩阵，E(·)表示求协方差；

(4)计算当前k时刻的定位结果和误差协方差：

${\hat{P}}_k=\mathrm{diag}\left((I-K_k{H}_{1k}){\hat{P}}_k^{\_}{(I-K_k{H}_{1k})}^T\right)$ ${\hat{X}}_k={\hat{X}}_k^{\_}+K_k(Z_k-Z_k^{-})$

其中，

为k时刻的用户接收机的后验估计误差协方差，diag()为求矩阵的对角矩阵，

为k时刻的用户接收机的后验估计状态量，I为单位矩阵，Z_k为观测量的真实值；

(5)重复所述步骤(1)至步骤(4)，得到一系列定位点。

需要说明的是，本领域技术人员理解，本发明中的步骤(2)：“其中，h=…”，这里的h没有加上下标k，表示一般情况，加上下标k则表示在k时刻的取值，其他相关参数同理。

有益效果：本发明为GPS中采用低运算量的二阶扩展卡尔曼滤波算法进行定位的方法，根据二阶扩展卡尔曼滤波算法中卡尔曼增益能够随着测量值的改变而动态变化的特点，可以实现实时的调整滤波参数，达到滤波定位最优化，提高了定位的静态和动态效果。同时，在减小了运算量之后，本发明的计算量与一阶扩展卡尔曼滤波算法的计算量相仿，可以在一些低端设备上运行，例如可以在以80MHz主频的ARM7TDMI为核心的处理芯片上运行。本发明在达到与一般二阶扩展卡尔曼滤波算法相同的定位效果的同时，降低了成本。

附图说明

图1是GPS***模型示意图；

图2是现有技术GPS接收机整机***结构框图；

图3是本发明的二阶扩展卡尔曼解算方法流程图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

本发明是一种低运算量的二阶扩展卡尔曼定位方法，包括数据的获取、算法的论证、算法中各种运算的简化、静态定位精度、动态定位效果等部分。其最终的结果是，数据从GPS基带获取卫星的数据和本地接收机给出的伪距、多普勒，通过该算法的计算可以得到定位结果。在算法的论证和算法运算的简化中，从理论上严格论证了算法的可行性和严谨性，同时，利用算法中数据的性质和工程的实际要求，对运算进行了简化处理，使其可以在低端硬件上运行。经过以上步骤结算出来的定位点，在静态时，定位精度可以达到0.5m，静态多径情况下持续6小时的测试，可以达到精度10m；动态时，弱信号情况下，可以准确定位，最大偏差不超过4m，强多径情况下，偏差不超过10m，并且可以保证路线的连续性。本发明获取卫星位置、速度和本地接收机的伪距、多普勒信息、伪距误差协方差。同时，从算法中读取上一时刻的定位结果和定位误差协方差，计算出算法所需的中间参数和卡尔曼增益。根据得到的参数和增益计算当前时刻定位结果和误差协方差。重复上述过程，经过1s后，再进行下一次解算。

本发明中采用过的状态量为；

X＝[x,y,z,Vx,Vy,Vz,δ_tu,δ_fu]^T

其中，ru＝[x,y,z]^T是用户的坐标向量，Vu＝[Vx,Vy,Vz]^T是用户的速度向量，δ_tu、δ_fu分别为钟差和钟差变化率。

采用过的观测量为：

Z＝h(X)

其中，

$h={[{\mathrm{\ρ}}_1,\·\·\·,{\mathrm{\ρ}}_n,{\stackrel{\·}{\mathrm{\ρ}}}_1,\·\·\·,{\stackrel{\·}{\mathrm{\ρ}}}_n]}^T$ ${\mathrm{\ρ}}_i=\left|\right|r_i^s-\mathrm{ru}\left|\right|+{\mathrm{\δ}}_{\mathrm{tu}}+{\mathrm{\ϵ}}_{\mathrm{\ρi}}$

${\stackrel{\·}{\mathrm{\ρ}}}_i=\left|\right|{(r_i^s-\mathrm{ru})}^T\·(V_i^s-\mathrm{Vu})\left|\right|/\left|\right|r_i^s-\mathrm{ru}\left|\right|+{\mathrm{\δ}}_{\mathrm{fu}}+{\mathrm{\ϵ}}_{{\stackrel{\·}{\mathrm{\ρ}}}_i}$

和

该卫星的位置和速度坐标向量，

为其伪距测量误差，是伪距变化率的测量误差。n为卫星个数，i∈[1,n]。

本发明的具体步骤如下：

1、获取本地上一时刻的定位结果，计算出当前时刻的先验估计值和先验估计协方差

${\hat{X}}_k^{\_}=A\·{\hat{X}}_{k-1}$ ${\hat{P}}_k^{\_}=A{\hat{P}}_{k-1}{A}^T+Q_k$

其中，X_k、

和

分别为k时刻的用户接收机的真实状态量、后验估计状态量和先验估计状态量，

为后验估计误差协方差，

为先验估计协方差；A为转移矩阵，

$A=\left[\begin{array}{cccccccc}1& 0& 0& \mathrm{\Δt}& 0& 0& 0& 0\\ 0& 1& 0& 0& \mathrm{\Δt}& 0& 0& 0\\ 0& 0& 1& 0& 0& \mathrm{\Δt}& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& \mathrm{\Δt}\\ 0& 0& 0& 0& 0& 0& 1& 0\end{array}\right]$

Δt为两次定位间的时间间隔，该处为1s；

Q_k为k时刻的过程噪声误差，

$Q_k=\left[\begin{array}{cccc}{10}^{-4}{I}_{3\×3}& 0_{3\×3}& 0& 0\\ 0_{3\×3}& {10}^3{I}_{3\×3}& 0& 0\\ 0& 0& {10}^5& 0\\ 0& 0& 0& 100\end{array}\right]$

I_3×3为3阶单位矩阵，0_3×3为3阶全零方阵。

2、建立观测量估计

方程

$Z_k^{-}=h_k\left({\hat{X}}_k^{\_}\right)+m_{1k}$

其中，

e_p为2n_k维单位矩阵中第p列向量,n_k为k时刻卫星个数。

3、计算卡尔曼增益，

$K_k=({\hat{P}}_k^{\_}\·H_{1k}^T+\frac{1}{2}\·\det X_k\·m_1^T){\left(\begin{array}{c}{H}_{1k}\·{\hat{P}}_k^{\_}\·H_{1k}^T+R_k+\frac{1}{2}\·H_{1k}\·\det X_k\·m_1^T+\frac{1}{2}\·m_{1k}\·\det X_k^T\·H_{1k}\\ +\underset{p,q}{\mathrm{\Σ}}\frac{1}{4}(\det X_k^T\·H_{2k}^p\·{\hat{P}}_k^{\_}\·H_{2k}^q\·\det X_k)e_p\·e_q^T\end{array}\right)}^{-1}$

其中，

$\det X_k=X_k-{\hat{X}}_k^{\_}$

$H_{1k}={\frac{{\∂h}_k\left(X_k\right)}{{\∂X}_k}|}_{X_k={\hat{X}}_k^{\_}}$

表示k时刻的测量值h_k在

处的一阶导数，

$H_{2k}^p={\frac{{\∂}^2{\left(h_k\left(X_k\right)\right)}_p}{{\∂x}_j{\∂x}_l}|}_{X_k={\hat{X}}_k^{\_}}$

表示k时刻的测量值h_k中的第p个函数在

处的二阶导数，x_j、x_l分别为状态量X_k中的任意两个元素，

与

的意义相同，p、q∈[1,2n_k]；R_k为伪距和多普勒的测量误差协方差，

$R_k=E\left(v_k{v}_k^T\right)$

v_k表示测量噪声矩阵，E()表示求协方差。

4、计算当前时刻的定位结果和误差协方差，

${\hat{P}}_k=\mathrm{diag}\left((I-K_k{H}_{1k}){\hat{P}}_k^{\_}{(I-K_k{H}_{1k})}^T\right)$ ${\hat{X}}_k={\hat{X}}_k^{\_}+K_k(Z_k-Z_k^{-})$

其中，diag()为求矩阵的对角矩阵，Z_k为***的观测值的真实值。

5、重复执行上述过程，就会得到一系列定位点。

在实际运行中，由于真实值Xk无法得到，因此，我们采用代替

所以有 $\det X_k\≈{\hat{X}}_k^{\_}-{\hat{X}}_{k-1}=A{\hat{X}}_{k-1}-{\hat{X}}_{k-1}=(A-I){\hat{X}}_{k-1}={\left[\begin{array}{c}\mathrm{Vu}\\ 0\\ 0\\ 0\\ {\mathrm{\δ}}_{\mathrm{fu}}\\ 0\end{array}\right]}_{8\×1}$

从上式看出，detX_k和m_1k仅仅和上一状态接收机的速度和钟差变化率有关。同时，detX_k和m_1k是对EKF2算法的修正。接收机的速度和钟差变化率是通过多普勒频移计算出来的，在精确度上，速度比位置更加准确，因此，本发明是在EKF2定位算法的基础上采用多普勒频移对位置进行修正和平滑。

在计算量上，我们采用如下的方法减小计算量：

(1)在EKF2算法中，Q_k、R_k是对称正定对角矩阵，

是对称正定矩阵，同时，有许多是二次型、稀疏矩阵，在计算时可以大量的减少计算量；

(2)在计算Kalman Gain时需要用到矩阵求逆，可以证明增益计算方程中右侧的分母矩阵是对称正定矩阵，因此利用该性质将矩阵求逆的计算量减少；

在涉及到矩阵A的运算中，由于A是稀疏矩阵，所以不采用矩阵乘法进行计算，直接在程序中写出每一项的结果，减少计算量。同时，Q_k、R_k在循环之前已经通过前面得到，不计算在循环时间中。

参看图1，所示为GPS***模型图，其中包含：店面GPS接收机101、接收机到卫星的伪距102、发射时刻的GPS卫星103、接收机时刻的卫星104。主要过程是，GPS信号经过天线发射，穿过大气层，存在散射，折射等影响，到达接收机天线。接收机所测定的距离，不光含有卫星与接收机的视线距离，还加含了各种延迟所造成的距离，成为“伪距”。

伪距是卫星定位的基础，根据空间两点之间的距离公式

因此，要准确定位需要至少4颗卫星的伪距和卫星坐标。这样，才可以采用扩展卡尔曼方程进行解算出接收机的坐标。

图2给出了GPS接收机整机***结构框图。包括射频前端处理模块、基带信号处理模块和定位导航模块。射频前端处理模块通过天线接收所有可见的GPS卫星信号，经前置滤波器和前置放大器后，再与本机振荡器产生的正弦波本振信号进行混频而下变频成中频信号，最后经模数转换将中频信号转变成离散时间的数字中频信号。中频信号经过基带数字信号处理模块中的捕获、跟踪等算法，复制出与接收到的卫星信号相一致的本地载波和本地伪码信号，从中获得GPS伪距和载波相位等测量值以及解调出导航电文。在基带数字信号处理模块处理完数字中频信号后，各个通道分别输出其所跟踪的卫星信号的伪距、多普勒频移和载波相位等测量值以及信号上解调出来的导航电文，而这些卫星测量值和导航电文中的星历参数等信息再经后续的定位导航运算功能模块的处理，接收机最终获得GPS定位结果，或者再输出各种导航信息。

图3给出了二阶扩展卡尔曼算法的软件流程图。首先获取本地上一时刻的定位结果，计算出当前时刻的先验估计值和先验估计协方差

${\hat{X}}_k^{\_}=A\·{\hat{X}}_{k-1}$ ${\hat{P}}_k^{\_}=A{\hat{P}}_{k-1}{A}^T+Q_k$

然后建立观测量方程

$Z_k^{-}=h_k\left({\hat{X}}_k^{\_}\right)+m_{1k}$

其次计算卡尔曼增益K_k

最后计算当前时刻的定位结果和误差协方差，

${\hat{P}}_k=\mathrm{diag}\left((I-K_k{H}_{1k}){\hat{P}}_k^{\_}{(I-K_k{H}_{1k})}^T\right)$ ${\hat{X}}_k={\hat{X}}_k^{\_}+K_k(Z_k-Z_k^{-})$

之后再重复执行以上操作就实现了连续定位。

Claims (1)

1.一种基于二阶扩展卡尔曼的低运算量GPS定位方法，包括如下步骤：

(1)获取本地k-1时刻的定位结果，计算出当前k时刻的先验估计状态量和先验估计误差协方差：

${\hat{X}}_k^{\_}=A\·{\hat{X}}_{k-1}$ ${\hat{P}}_k^{\_}=A{\hat{P}}_{k-1}{A}^T+Q_k$

其中，

为k时刻的用户接收机的先验估计状态量，

为k-1时刻的用户接收机的后验估计状态量，

为k时刻的用户接收机的先验估计误差协方差，

为k-1时刻的用户接收机的后验估计误差协方差，

A为转移矩阵： $A=\left[\begin{array}{cccccccc}1& 0& 0& \mathrm{\Δt}& 0& 0& 0& 0\\ 0& 1& 0& 0& \mathrm{\Δt}& 0& 0& 0\\ 0& 0& 1& 0& 0& \mathrm{\Δt}& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& \mathrm{\Δt}\\ 0& 0& 0& 0& 0& 0& 1& 0\end{array}\right]$

其中，Δt为两次定位间的时间间隔；Q_k为k时刻的过程噪声误差，

$Q_k=\left[\begin{array}{cccc}{10}^{-4}{I}_{3\×3}& 0_{3\×3}& 0& 0\\ 0_{3\×3}& {10}^3{I}_{3\×3}& 0& 0\\ 0& 0& {10}^5& 0\\ 0& 0& 0& 100\end{array}\right]$

其中，I_3×3为3阶单位矩阵，0_3×3为3阶全零方阵；

(2)建立观测量估计

方程

$Z_k^{-}=h_k\left({\hat{X}}_k^{\_}\right)+m_{1k}$

其中，

$h={[{\mathrm{\ρ}}_1,\·\·\·,{\mathrm{\ρ}}_n,{\stackrel{\·}{\mathrm{\ρ}}}_1,\·\·\·,{\stackrel{\·}{\mathrm{\ρ}}}_n]}^T$ ${\mathrm{\ρ}}_i=\left|\right|r_i^s-\mathrm{ru}\left|\right|+{\mathrm{\δ}}_{\mathrm{tu}}+{\mathrm{\ϵ}}_{\mathrm{\ρi}}$

${\stackrel{\·}{\mathrm{\ρ}}}_i=\left|\right|{(r_i^s-\mathrm{ru})}^T\·(V_i^s-\mathrm{Vu})\left|\right|/\left|\right|r_i^s-\mathrm{ru}\left|\right|+{\mathrm{\δ}}_{\mathrm{fu}}+{\mathrm{\ϵ}}_{{\stackrel{\·}{\mathrm{\ρ}}}_i}$

$m_{1k}=\frac{1}{2}\underset{p}{\overset{{2n}_k}{\mathrm{\Σ}}}{e}_p\·{(X_k-{\hat{X}}_k^{\_})}^T\·H_{2k}^p\·(X_k-{\hat{X}}_k^{\_})$

表示k时刻的测量值h_k中的第p个函数在

处的二阶导数，x_j、x_l分别为状态量X_k中的任意两个元素，p∈[1,2n_k]，

为卫星的位置坐标向量，

为卫星的速度坐标向量，为卫星的伪距测量误差，

是伪距变化率的测量误差，n为卫星个数，i∈[1,n]，e_p为2n_k维单位矩阵中第p列向量,n_k为k时刻卫星个数，δ_tu、δ_fu分别为钟差和钟差变化率；

(3)计算卡尔曼增益K_k：

$K_k=({\hat{P}}_k^{\_}\·H_{1k}^T+\frac{1}{2}\·\det X_k\·m_{1k}^T){\left(\begin{array}{c}{H}_{1k}\·{\hat{P}}_k^{\_}\·H_{1k}^T+R_k+\frac{1}{2}\·H_{1k}\·\det X_k\·m_{1k}^T+\frac{1}{2}\·m_{1k}\·\det X_k^T\·H_{1k}\\ +\underset{p,q}{\mathrm{\Σ}}\frac{1}{4}(\det X_k^T\·H_{2k}^p\·{\hat{P}}_k^{\_}\·H_{2k}^q\·\det X_k)e_p\·e_q^T\end{array}\right)}^{-1}$

其中，

X_k为k时刻的用户接收机的真实状态量，

表示k时刻的测量值h_k在

处的一阶导数，

表示k时刻的测量值h_k中的第q个函数在

处的二阶导数，R_k为伪距和多普勒的测量误差协方差：

其中v_k表示测量噪声矩阵，E(·)表示求协方差；

(4)计算当前k时刻的定位结果和误差协方差：

${\hat{P}}_k=\mathrm{diag}\left((I-K_k{H}_{1k}){\hat{P}}_k^{\_}{(I-K_k{H}_{1k})}^T\right)$ ${\hat{X}}_k={\hat{X}}_k^{\_}+K_k(Z_k-Z_k^{-})$

其中，为k时刻的用户接收机的后验估计误差协方差，diag()为求矩阵的对角矩阵，

为k时刻的用户接收机的后验估计状态量，I为单位矩阵，Z_k为观测量的真实值；

(5)重复所述步骤(1)至步骤(4)，得到一系列定位点。

Images