CN103674009A - 获取移动终端运动轨迹的方法、装置和移动终端 - Google Patents

Abstract

本发明提出一种获取移动终端运动轨迹的方法、装置和移动终端，其中所述方法包括以下步骤：获取移动终端在上一个计算时刻的角速度值和在上一个计算时刻的姿态值；根据在上一个计算时刻的角速度值和上一个计算时刻的姿态值预测当前计算时刻的第一姿态值；获取移动终端在当前计算时刻的加速度值和/或航向角值；根据当前计算时刻的加速度值和/或航向角值对当前计算时刻的第一姿态值进行修正；以及根据每个计算时刻对应的修正之后的第一姿态值获取移动终端的运动轨迹。本发明实施例通过结合移动终端的角速度值、加速度值和航向角值可以获得精度高的运动轨迹。

Description

获取移动终端运动轨迹的方法、装置和移动终端

技术领域

本发明涉及移动终端制造技术领域，尤其涉及一种获取移动终端运动轨迹的方法、装置和移动终端。

背景技术

随着移动终端技术的日益发展，移动终端的功能越来越丰富，特别是传感器技术的发展使得获取移动终端的运动轨迹成为可能。目前，可以利用移动终端携带的加速度计识别对移动终端的单一甩动，无法获得精准的运动轨迹。

发明内容

本发明旨在至少解决上述技术问题之一。

为此，本发明的第一个目的在于提出一种获取移动终端运动轨迹的方法，该方法通过结合移动终端的角速度值、加速度值和航向角值可以获得精度高的移动轨迹。

本发明的第二个目的在于提出一种获取移动终端运动轨迹的装置。

本发明的第三个目的在于提出一种移动终端。

为了实现上述目的，根据本发明第一方面的实施例的获取移动终端运动轨迹的方法包括以下步骤：获取移动终端在上一个计算时刻的角速度值和在所述上一个计算时刻的姿态值；根据在所述上一个计算时刻的角速度值和所述上一个计算时刻的姿态值预测当前计算时刻的第一姿态值；获取所述移动终端在所述当前计算时刻的加速度值和/或航向角值；根据所述当前计算时刻的加速度值和/或航向角值对所述当前计算时刻的第一姿态值进行修正；以及根据每个计算时刻对应的所述修正之后的所述第一姿态值获取所述移动终端的运动轨迹。

根据本发明实施例的获取移动终端运动轨迹的方法，通过上一个计算时刻的角速度值和姿态值预测当前计算时刻的姿态值，并通过当前计算时刻的加速度和/或航向角值对姿态值进行修正，可以获得精度高的运动轨迹。

为了实现上述目的，根据本发明第二方面的实施例的获取移动终端运动轨迹的装置包括：第一获取模块，用于获取移动终端在上一个计算时刻的角速度值和在所述上一个计算时刻的姿态值；预测模块，用于根据在所述上一个计算时刻的角速度值和所述上一个计算时刻的姿态值预测当前计算时刻的第一姿态值；第二获取模块，用于获取所述移动终端在所述当前计算时刻的加速度值和/或航向角值；修正模块，用于根据所述当前计算时刻的所述加速度值和/或所述航向角值对所述当前计算时刻的第一姿态值进行修正；以及第三获取模块，用于根据每个计算时刻对应的所述修正之后的所述第一姿态值获取所述移动终端的运动轨迹。

根据本发明实施例的获取移动终端运动轨迹的装置，通过上一个计算时刻的角速度值和姿态值预测当前计算时刻的姿态值，并通过当前计算时刻的加速度和/或航向角值对姿态值进行修正，可以获得精度高的运动轨迹。

为了实现上述目的，根据本发明第三方面的实施例的移动终端包括本发明第二方面实施例的获取移动终端运动轨迹的装置。

根据本发明实施例的移动终端，通过获取移动终端运动轨迹的装置可以获得精度高的运动轨迹。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中，

图1是根据本发明一个实施例的获取移动终端运动轨迹的方法的流程图；

图2是根据本发明一个实施例的获取移动终端运动轨迹的方法的流程图；

图3是根据本发明一个实施例的获取移动终端运动轨迹的装置的结构框图；

图4是根据本发明实施例的修正模块的结构框图；以及

图5是根据本发明实施例的修正模块的结构框图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。相反，本发明的实施例包括落入所附加权利要求书的精神和内涵范围内的所有变化、修改和等同物。

在本发明的描述中，需要理解的是，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性。在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。此外，在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

下面参考附图描述根据本发明实施例的获取移动终端运动轨迹的方法、装置和移动终端。

一种获取移动终端运动轨迹的方法，包括以下步骤：获取移动终端在上一个计算时刻的角速度值和在上一个计算时刻的姿态值；根据在上一个计算时刻的角速度值和上一个计算时刻的姿态值预测当前计算时刻的第一姿态值；获取移动终端在当前计算时刻的加速度值和/或航向角值；根据当前计算时刻的加速度值和/或航向角值对当前计算时刻的第一姿态值进行修正；以及根据每个计算时刻对应的修正之后的第一姿态值获取移动终端的运动轨迹。

图1是根据本发明一个实施例的获取移动终端运动轨迹的方法的流程图。

如图1所示，获取移动终端运动轨迹的方法包括下述步骤。

步骤S101，获取移动终端在上一个计算时刻的角速度值和在上一个计算时刻的姿态值。

步骤S102，根据在上一个计算时刻的角速度值和上一个计算时刻的姿态值预测当前计算时刻的第一姿态值。

具体地，在本发明的一个实施例中，可以按照以下公式预测当前计算时刻的第一姿态值，

${\hat{X}}_{k,k-1}=f({\hat{X}}_{k-1},k-1)=\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],$

其中，

为当前计算时刻的第一姿态值，p,q，r为上一个计算时刻的角速度值，q₀,q₁,q₂,q₃为上一个计算时刻的姿态值，k为当前计算时刻，k-1为上一个计算时刻，

为上一个计算时刻的姿态值（该姿态值经过修正）。

步骤S103，获取移动终端在当前计算时刻的加速度值和/或航向角值。

在本发明的一个实施例中，通过陀螺仪获取角速度值，通过加速度计获取加速度值，和通过电子罗盘获取航向角值。

步骤S104，根据当前计算时刻的加速度值和/或航向角值对当前计算时刻的第一姿态值进行修正。

步骤S105，根据每个计算时刻对应的修正之后的第一姿态值获取移动终端的运动轨迹。

其中，每个计算时刻修正之后的第一姿态值构成移动终端的运动轨迹。计算时刻划分的越细致则获得的运动轨迹越精确，同时增加计算负担。

根据本发明实施例的获取移动终端运动轨迹的方法，通过上一个计算时刻的角速度值和姿态值预测当前计算时刻的姿态值，并通过当前计算时刻的加速度和/或航向角值对姿态值进行修正，可以获得精度高的运动轨迹。

图2是根据本发明一个实施例的获取移动终端运动轨迹的方法的流程图。

如图2所示，获取移动终端运动轨迹的方法包括下述步骤。

步骤S201，获取移动终端在上一个计算时刻的角速度值和在上一个计算时刻的姿态值。

步骤S202，根据在上一个计算时刻的角速度值和上一个计算时刻的姿态值预测当前计算时刻的第一姿态值。

具体地，在本发明的一个实施例中，可以按照以下公式预测当前计算时刻的第一姿态值，

${\hat{X}}_{k,k-1}=f({\hat{X}}_{k-1},k-1)=\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],$

其中，为当前计算时刻的第一姿态值，p,q，r为上一个计算时刻的角速度值，q₀,q₁,q₂,q₃为上一个计算时刻的姿态值，k为当前计算时刻，k-1为上一个计算时刻，

为上一个计算时刻的姿态值（该姿态值经过修正）。

在本发明的一个实施例中，初始计算时刻的姿态值通过初始计算时刻的加速度值和/或航向角值计算获得。

步骤S203，获取移动终端在当前计算时刻的加速度值和/或航向角值。

在本发明的一个实施例中，通过陀螺仪获取角速度值，通过加速度计获取加速度值，和通过电子罗盘获取航向角值。

步骤S204，根据当前计算时刻的加速度值和/或航向角值获取当前计算时刻的第二姿态值Z(x)，其中Z(x)＝[φθψ]^T。

步骤S205，根据上一个计算时刻的修正误差方差矩阵P_k-1和过程噪声矩阵Q获取当前计算时刻的误差方差矩阵P_k,k-1。

具体地，按照以下公式获取当前计算时刻的误差方差矩阵P_k,k-1，

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q,$

其中， ${\mathrm{\Φ}}_{k,k-1}=\frac{\∂f}{\∂{\hat{X}}_{k-1}}={\frac{\∂f[{\hat{X}}_{k-1},k-1]}{{\∂X}_{k-1}}|}_{X_{k-1}={\hat{X}}_{k-1}}=\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].$

在本发明的一个实施例中，过程噪声矩阵Q根据陀螺仪、加速度计和电子罗盘的属性获得，其中，过程噪声矩阵Q为4*4的常数矩阵。过程噪声矩阵Q为对角矩阵，并且对角线上的元素都大于零，可以根据陀螺仪、加速度计和电子罗盘的属性预测获得，然后通过实验筛选最佳参数。

步骤S206，根据当前计算时刻的误差方差矩阵P_k,k-1和观测噪声矩阵R获取当前计算时刻的增益矩阵K_k。

具体地，按照以下公式计算所述当前计算时刻的增益矩阵K_k，

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R]}^{-1},$

其中， $H_k={\frac{\∂h}{{\∂X}_k}|}_{{\hat{X}}_{k,k-1}}=\left[\begin{array}{cccc}\frac{\∂h}{{\∂q}_0}& \frac{\∂h}{{\∂q}_1}& \frac{\∂h}{{\∂q}_2}& \frac{\∂h}{{\∂q}_3}\end{array}\right],$

$\frac{\∂h}{{\∂q}_0}=\left[\begin{array}{c}\frac{{2q}_1[1-2(q_1^2+q_2^2)]}{{[1-2(q_1^2+q_2^2)]}^2+4{(q_2{q}_3+q_0{q}_1)}^2}\\ \frac{{2q}_2}{\sqrt{1-4{(q_1{q}_3-q_0{q}_2)}^2}}\\ \frac{{2q}_3[1-2(q_2^2+q_3^2)]}{{[1-2(q_2^2+q_3^2)]}^2+4{(q_1{q}_2+q_0{q}_3)}^2}\end{array}\right],\frac{\∂h}{{\∂q}_1}=\left[\begin{array}{c}\frac{{2q}_0[1-2(q_1^2+q_2^2)]+{4q}_1(q_2{q}_3+q_0{q}_1)}{{[1-2(q_1^2+q_2^2)]}^2+4{(q_2{q}_3+q_0{q}_1)}^2}\\ \frac{-{2q}_3}{\sqrt{1-4{(q_1{q}_3-q_0{q}_2)}^2}}\\ \frac{{2q}_2[1-2(q_2^2+q_3^2)]}{{[1-2(q_2^2+q_3^2)]}^2+4{(q_1{q}_2+q_0{q}_3)}^2}\end{array}\right]$

$\frac{\∂h}{{\∂q}_2}=\left[\begin{array}{c}\frac{{2q}_3[1-2(q_1^2+q_2^2)]+{4q}_2(q_2{q}_3+q_0{q}_1)}{{[1-2(q_1^2+q_2^2)]}^2+4{(q_2{q}_3+q_0{q}_1)}^2}\\ \frac{{2q}_0}{\sqrt{1-4{(q_1{q}_3-q_0{q}_2)}^2}}\\ \frac{{2q}_1[1-2(q_2^2+q_3^2)]+{4q}_2(q_1{q}_2+q_0{q}_3)}{{[1-2(q_2^2+q_3^2)]}^2+4{(q_1{q}_2+q_0{q}_3)}^2}\end{array}\right]$

$\frac{\∂h}{{\∂q}_3}=\left[\begin{array}{c}\frac{{2q}_2[1-2(q_1^2+q_2^2)]}{{[1-2(q_1^2+q_2^2)]}^2+4{(q_2{q}_3+q_0{q}_1)}^2}\\ \frac{-{2q}_1}{\sqrt{1-4{(q_1{q}_3-q_0{q}_2)}^2}}\\ \frac{{2q}_0[1-2(q_2^2+q_3^2)]+{4q}_3(q_1{q}_2+q_0{q}_3)}{{[1-2(q_2^2+q_3^2)]}^2+4{(q_1{q}_2+q_0{q}_3)}^2}\end{array}\right].$

h(x)为上一个计算时刻的姿态值，h(x)＝[φθψ]^T，φ、θ和

分别为对应不同坐标的姿态值，q₀,q₁,q₂,q₃和φ，θ，

可以通过以下方式进行转换，

在本发明的一个实施例中，观测噪声矩阵R根据加速度计和电子罗盘的精度获得，其中，观测噪声矩阵R为3*3的常数矩阵。观测噪声矩阵R也为对角矩阵，并且对角线上的元素都大于零，例如，观测噪声矩阵R可以取其对角线上的值为对应的加速度计和电子罗盘的精度的平方。

步骤S207，根据上一个计算时刻的姿态值h(x)、当前计算时刻的第二姿态值Z(x)和增益矩阵K_k按照以下公式对当前计算时刻的第一姿态值

进行修正，

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z\left(x\right)-h\left(x\right)),$

其中，

为修正之后的当前计算时刻的第一姿态值。

步骤S208，根据每个计算时刻对应的修正之后的第一姿态值获取移动终端的运动轨迹。

其中，每个计算时刻修正之后的第一姿态值构成移动终端的运动轨迹。计算时刻划分的越细致则获得的运动轨迹越精确，同时增加计算负担。

在本发明的一个实施例中，还包括步骤（图中未示出）：对当前计算时刻的误差方差矩阵P_k,k-1按照以下公式进行修正，

P_k=[I-K_kH_k]P_k，k-1，

其中，P_k为当前计算时刻的修正误差方差矩阵。

根据本发明实施例的获取移动终端运动轨迹的方法，结合上一个计算时刻的角速度值和姿态值以及当前计算时刻的加速度和/或航向角值，进一步可以获得精度高的运动轨迹。

一种获取移动终端运动轨迹的装置，包括：第一获取模块，用于获取移动终端在上一个计算时刻的角速度值和在上一个计算时刻的姿态值；预测模块，用于根据在上一个计算时刻的角速度值和上一个计算时刻的姿态值预测当前计算时刻的第一姿态值；第二获取模块，用于获取移动终端在当前计算时刻的加速度值和/或航向角值；修正模块，用于根据当前计算时刻的加速度值和/或航向角值对当前计算时刻的第一姿态值进行修正；以及第三获取模块，用于根据每个计算时刻对应的修正之后的第一姿态值获取移动终端的运动轨迹。

图3是根据本发明一个实施例的获取移动终端运动轨迹的装置的结构框图。

如图3所示，获取移动终端运动轨迹的装置包括：第一获取模块100、预测模块200、第二获取模块300、修正模块400和第三获取模块500。

具体地，第一获取模块100用于获取移动终端在上一个计算时刻的角速度值和在上一个计算时刻的姿态值。

预测模块200用于根据在上一个计算时刻的角速度值和上一个计算时刻的姿态值预测当前计算时刻的第一姿态值。具体地，预测模块200可以按照以下公式预测当前计算时刻的第一姿态值，

${\hat{X}}_{k,k-1}=f({\hat{X}}_{k-1},k-1)=\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],$

其中，为当前计算时刻的第一姿态值，p，q，r为上一个计算时刻的角速度值，q₀,q₁,q₂,q₃为上一个计算时刻的姿态值，k为当前计算时刻，k-1为上一个计算时刻，

为上一个计算时刻的姿态值（该姿态值经过修正）。

第二获取模块300用于获取移动终端在当前计算时刻的加速度值和/或航向角值。其中，通过陀螺仪获取角速度值，通过加速度计获取加速度值，和通过电子罗盘获取航向角值。

修正模块400用于根据当前计算时刻的加速度值和/或航向角值对当前计算时刻的第一姿态值进行修正。

第三获取模块500用于根据每个计算时刻对应的修正之后的第一姿态值获取移动终端的运动轨迹。

根据本发明实施例的获取移动终端运动轨迹的装置，通过上一个计算时刻的角速度值和姿态值预测当前计算时刻的姿态值，并通过当前计算时刻的加速度和/或航向角值对姿态值进行修正，可以获得精度高的运动轨迹。

图4是根据本发明实施例的修正模块400的结构框图。

如图4所示，修正模块400包括：第一获取单元410、第二获取单元420、第三获取单元430和第一修正单元440。

具体地，第一获取单元410用于根据当前计算时刻的加速度值和/或航向角值获取当前计算时刻的第二姿态值Z(x)，其中Z(x)＝[φθψ]^T。

第二获取单元420用于根据上一个计算时刻的修正误差方差矩阵P_k-1和过程噪声矩阵Q获取当前计算时刻的误差方差矩阵P_k，k-1，具体的获取过程可参考步骤S205。

第三获取单元430用于根据当前计算时刻的误差方差矩阵P_k，k-1和观测噪声矩阵R获取当前计算时刻的增益矩阵K_k，具体的获取过程可参考步骤S206。

第一修正单元440用于根据上一个计算时刻的姿态值h(x)、当前计算时刻的第二姿态值Z(x)和增益矩阵K_k对当前计算时刻的第一姿态值

进行修正，具体的修正过程可参考步骤S207。

根据修正模块400可以获得每个计算时刻精确的姿态值。

图5是根据本发明实施例的修正模块400的结构框图。

如图5所示，在图4所示的实施例的基础上修正模块400还包括第二修正单元450。具体地，第二修正单元450用于对当前计算时刻的所述误差方差矩阵P_k，k-1按照以下公式进行修正，

P_k=[I-K_kH_k]P_k，k-1。

其中，P_k为当前计算时刻的修正误差方差矩阵。

一种移动终端，包括本发明任一项实施例所述的获取移动终端运动轨迹的装置。

根据本发明实施例的移动终端，通过获取移动终端运动轨迹的装置可以获得精度高的运动轨迹。由此，该移动终端还可以实现其他与移动轨迹相关的功能。

应当理解，根据本发明实施例获得的移动终端的运动轨迹可用于不同的场合，例如可以根据运动轨迹进行移动终端的加密解锁，利用运动轨迹可以设置游戏、应用等。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行***执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列（PGA），现场可编程门阵列（FPGA）等。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同限定。

Claims (17)

1.一种获取移动终端运动轨迹的方法，其特征在于，包括以下步骤：

获取移动终端在上一个计算时刻的角速度值和在所述上一个计算时刻的姿态值；

根据在所述上一个计算时刻的角速度值和所述上一个计算时刻的姿态值预测当前计算时刻的第一姿态值；

获取所述移动终端在所述当前计算时刻的加速度值和/或航向角值；

根据所述当前计算时刻的加速度值和/或航向角值对所述当前计算时刻的第一姿态值进行修正；以及

根据每个计算时刻对应的所述修正之后的所述第一姿态值获取所述移动终端的运动轨迹。

2.根据权利要求1所述的方法，其特征在于，按照以下公式预测所述当前计算时刻的第一姿态值，

${\hat{X}}_{k,k-1}=\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],$

其中，

为所述当前计算时刻的第一姿态值，p，q，r为所述上一个计算时刻的角速度值，q₀，q₁，q₂，q₃为所述上一个计算时刻的姿态值，k为所述当前计算时刻，k-1为所述上一个计算时刻。

3.根据权利要求1所述的方法，其特征在于，通过陀螺仪获取所述角速度值，通过加速度计获取所述加速度值，和通过电子罗盘获取所述航向角值。

4.根据权利要求2所述的方法，其特征在于，所述根据所述当前计算时刻的加速度值和/或航向角值对所述当前计算时刻的第一姿态值进行修正，进一步包括：

根据所述当前计算时刻的加速度值和/或航向角值获取所述当前计算时刻的第二姿态值Z(x)，其中Z(x)＝[φθψ]^T；

根据所述上一个计算时刻的修正误差方差矩阵P_k-1和过程噪声矩阵Q获取所述当前计算时刻的误差方差矩阵P_k,k-1；

根据所述当前计算时刻的误差方差矩阵P_k，k-1和观测噪声矩阵R获取所述当前计算时刻的增益矩阵K_k；以及

根据所述上一个计算时刻的姿态值h(x)、所述当前计算时刻的第二姿态值Z(x)和所述增益矩阵K_k按照以下公式对所述当前计算时刻的第一姿态值

进行修正，

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z\left(x\right)-h\left(x\right)),$

其中，

为所述修正之后的所述当前计算时刻的第一姿态值，h(x)=[φθψ]^T，φ、θ和

分别为对应不同坐标的姿态值。

5.根据权利要求4所述的方法，其特征在于，按照以下公式获取所述当前计算时刻的误差方差矩阵P_k，k-1，

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q,$

其中， ${\mathrm{\Φ}}_{k,k-1}=\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].$

6.根据权利要求4所述的方法，其特征在于，按照以下公式获取所述当前计算时刻的增益矩阵K_k，

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R]}^{-1},$

其中， $H_k={\frac{\∂h}{{\∂X}_k}|}_{{\hat{X}}_{k,k-1}}=\left[\begin{array}{cccc}\frac{\∂h}{{\∂q}_0}& \frac{\∂h}{{\∂q}_1}& \frac{\∂h}{{\∂q}_2}& \frac{\∂h}{{\∂q}_3}\end{array}\right].$

7.根据权利要求4所述的方法，其特征在于，还包括：对所述当前计算时刻的误差方差矩阵P_k，k-1按照以下公式进行修正，

P_k=[I-K_kH_k]P_k，k-1，

其中，P_k为所述当前计算时刻的修正误差方差矩阵。

8.根据权利要求3所述的方法，其特征在于，所述过程噪声矩阵Q根据所述陀螺仪、所述加速度计和所述电子罗盘的属性获得，其中，所述过程噪声矩阵Q为4*4的常数矩阵，所述观测噪声矩阵R根据所述加速度计和所述电子罗盘的精度获得，其中，所述观测噪声矩阵R为3*3的常数矩阵。

9.一种获取移动终端运动轨迹的装置，其特征在于，包括：

第一获取模块，用于获取移动终端在上一个计算时刻的角速度值和在所述上一个计算时刻的姿态值；

预测模块，用于根据在所述上一个计算时刻的角速度值和所述上一个计算时刻的姿态值预测当前计算时刻的第一姿态值；

第二获取模块，用于获取所述移动终端在所述当前计算时刻的加速度值和/或航向角值；

修正模块，用于根据所述当前计算时刻的加速度值和/或航向角值对所述当前计算时刻的第一姿态值进行修正；以及

第三获取模块，用于根据每个计算时刻对应的所述修正之后的所述第一姿态值获取所述移动终端的运动轨迹。

10.根据权利要求9所述的装置，其特征在于，所述预测模块按照以下公式预测所述当前计算时刻的第一姿态值，

${\hat{X}}_{k,k-1}=\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]$

其中，

为所述当前计算时刻的第一姿态值，p,q，r为所述上一个计算时刻的角速度值，q₀，q₁，q₂，q₃为所述上一个计算时刻的姿态值，k为所述当前计算时刻，k-1为所述上一个计算时刻。

11.根据权利要求9所述的装置，其特征在于，通过陀螺仪获取所述角速度值，通过加速度计获取所述加速度值，和通过电子罗盘获取所述航向角值。

12.根据权利要求10所述的装置，其特征在于，所述修正模块包括：

第一获取单元，用于根据所述当前计算时刻的加速度值和/或航向角值获取所述当前计算时刻的第二姿态值Z(x)，其中Z(x)＝[φθψ]^T；

第二获取单元，用于根据所述上一个计算时刻的修正误差方差矩阵P_k-1和过程噪声矩阵Q获取所述当前计算时刻的误差方差矩阵P_k,k-1；

第三获取单元，用于根据所述当前计算时刻的误差方差矩阵P_k,k-1和观测噪声矩阵R获取所述当前计算时刻的增益矩阵K_k；以及

第一修正单元，用于根据所述上一个计算时刻的姿态值h(x)、所述当前计算时刻的第二姿态值Z(x)和所述增益矩阵K_k按照以下公式对所述当前计算时刻的第一姿态值

进行修正，

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z\left(x\right)-h\left(x\right))$

其中，

为所述修正之后的所述当前计算时刻的第一姿态值，h(x)=[φθψ]^T，φ、θ和分别为对应不同坐标的姿态值。

13.根据权利要求12所述的装置，其特征在于，所述第二获取单元按照以下公式获取所述当前计算时刻的误差方差矩阵P_k，k-1，

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q,$

其中， ${\mathrm{\Φ}}_{k,k-1}=\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].$

14.根据权利要求12所述的装置，其特征在于，所述第三获取单元按照以下公式获取所述当前计算时刻的增益矩阵K_k，

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R]}^{-1},$

其中， $H_k={\frac{\∂h}{{\∂X}_k}|}_{{\hat{X}}_{k,k-1}}=\left[\begin{array}{cccc}\frac{\∂h}{{\∂q}_0}& \frac{\∂h}{{\∂q}_1}& \frac{\∂h}{{\∂q}_2}& \frac{\∂h}{{\∂q}_3}\end{array}\right].$

15.根据权利要求12所述的装置，其特征在于，所述修正模块还包括：

第二修正单元，用于对所述当前计算时刻的所述误差方差矩阵P_k，k-1按照以下公式进行修正，

P_k=[I-K_kH_k]P_k，k-1。

其中，Pk为所述当前计算时刻的修正误差方差矩阵。

16.根据权利要求11任一项所述的装置，其特征在于，所述过程噪声矩阵Q根据所述陀螺仪、所述加速度计和所述电子罗盘的属性获得，其中，所述过程噪声矩阵Q为4*4的常数矩阵，所述观测噪声矩阵R根据所述加速度计和所述电子罗盘的精度获得，其中，所述观测噪声矩阵R为3*3的常数矩阵。

17.一种移动终端，其特征在于，具有如权利要求9-16任一项所述的获取移动终端运动轨迹的装置。

Images