[发明专利]姿态角获取方法及装置

摘要

本发明涉及一种姿态获取方法，包括如下步骤：获取当前时刻角速度数据、加速度数据和地磁场强度数据；获取上一时刻系统状态量，获取上一时刻系统误差协方差矩阵值，进行迭代计算系统误差协方差矩阵，根据所述当前时刻协方差矩阵值和所述系统测量方程的雅克比矩阵值获取卡尔曼增益矩阵值；根据所述卡尔曼增益矩阵值、上一时刻系统状态量、系统测量量和所述系统测量方程获取当前时刻系统状态量；根据所述当前时刻系统状态量中的四元数获取姿态角。还提供了对应装置。动态响应快，可以捕捉高于5Hz的人体动作，静态误差在±0.1°，对人体动作跟随效果好，用加速度与磁强数据校准姿态，实时计算陀螺仪的误差。

主权项

1.一种姿态角获取方法，其特征在于，包括如下步骤：S110、获取当前时刻角速度数据、加速度数据和地磁场强度数据；S120、获取上一时刻系统状态量，根据当前时刻角速度数据和上一时刻系统状态量求取系统状态矩阵值；S130、获取上一时刻系统误差协方差矩阵值，根据上一时刻系统误差协方差矩阵值和所述系统状态矩阵值获取当前时刻系统误差协方差矩阵值；S140、根据所述当前时刻地磁场强度数据和上一时刻系统状态量，建立系统测量方程h，利用所述上一时刻系统状态量对所述系统测量方程求偏导，获取系统测量方程的雅克比矩阵值；S150、根据所述当前时刻系统误差协方差矩阵值和所述系统测量方程的雅克比矩阵值获取卡尔曼增益矩阵值；S160、根据所述卡尔曼增益矩阵值、上一时刻系统状态量、系统测量量和所述系统测量方程获取当前时刻系统状态量，所述系统测量量表示为Y＝(ax ay az mx my mz)T，其中，Y表示系统测量量，ax、ay、az表示当前时刻加速度数据在芯片坐标系下x、y、z轴的分量，mx、my、mz表示当前时刻地磁场强度数据在芯片坐标系下x、y、z轴的分量；S170、根据所述当前时刻系统状态量中的四元数获取姿态角。