获得的所有其他 实施例，都属于本发明保护的范围。
[0057] 实施例1 :
[0058] 如图1所示，一种基于九轴MEMS传感器的农业机械全姿态角更新方法，所述基于 九轴MEMS传感器的农业机械全姿态角更新方法包括如下步骤：
[0059] 步骤S1 :建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设 定相应车体运动姿态的参数向量；
[0060] 所述步骤S1 :建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型， 并设定相应车体运动姿态的参数向量步骤具体为：
[0061] 陀螺仪误差模型通过陀螺仪误差计算公式对陀螺仪角速度进行计算，其中，陀螺 仪误差计算公式：《 = ?lb+b_"+bug，其中《为陀螺仪输出角速度，colb为陀螺仪真实角速 度，为陀螺仪零漂，bug为陀螺仪输出白噪声；
[0062] 通过电子罗盘校准椭圆模型消除磁场干扰；其中，电子罗盘校准椭圆模型：
nx，my为地磁场强度，Xoffset和Yoffset为硬磁干扰， Xsf?和Ysf为软磁干扰；
[0063] 通过七维EKF滤波模型对车体姿态进行更新处理，其中，七维EKF滤波模型为七维 状态向量的扩展卡尔曼滤波，EKF包括状态方程与观测方程：
[0064] .v= /(.r,^)+nl
[0065] y=h(x)+vl
[0066] 状态矩阵为x=[qbuJ，q为四元数向量q0,ql，q2,q3，b_"为XYZ三轴陀螺仪零 漂；其中《为陀螺仪输出角速度，wl为过程噪声矩阵，vl为观测噪声矩阵，y为观测量，y =[a$nag]T，其中a为三轴加速度值，$nag为电子罗盘计算的航向角，
[0067]
[0068]
[0069]由于地磁场强度比较微弱，容易受到周围铁质材料和电磁场的影响，所以在使用 之前必须进行校准，建立电子罗盘校准椭圆模型用于消除磁场所带的干扰，实际校准过程 是通过最小二乘法对采集到的磁场强度进行拟合，然后得出上述参数。
[0070] 步骤S2:通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强 度数据；
[0071] 所述步骤S2:通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁 场强度数据步骤具体为：
[0072] 通过陀螺仪获取车体的角速度，对陀螺仪零点漂移进行补偿；
[0073] 通过加速度传感器采集车体的加速度数据；
[0074] 通过地磁传感器采集车体的地磁场强度数据。
[0075] 步骤S3 :根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的 陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度；
[0076] 所述步骤S3:根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建 立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角 度步骤具体为：
[0077] 通过陀螺仪误差模型对角速度进行积分计算获得角度数据；
[0078] 通过对加速度数据的积分计算出速度，再次积分计算出位置信息；
[0079] 地磁场强度数据经椭圆模型计算出来的校准参数补偿和倾角修正后计算车体航 向角。
[0080] 通过MEMS传感器实时采集车体的运动信息，陀螺仪采集的车体角速度通过状态 估计量陀螺仪零偏进行修正，对其积分计算出角度增量，地磁传感器经软磁、硬磁和倾角修 正补偿后计算出航向角。
[0081] 步骤S4:通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数 据融合处理，对车体的运动姿态角进行实时更新；
[0082] 所述步骤S4:通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进 行数据融合处理，对车体的运动姿态角进行实时更新步骤具体为：
[0083] 七维EKF滤波模型通过四元数姿态更新算法对车体姿态数据进行计算，其中，EKF 算法计算过程：
[0091] k为采样时刻，&:为系统状态估计量，（_)为前一时刻，（+)为后一时刻，①k 为状态转移矩阵，Pk为最小均方误差矩阵，Q为状态向量对应的协方差矩阵，Kk为误 差增益，yk为观测向量，Hk为观测方程转移矩阵，Rk为观测向量对应的协方差矩阵。
[0092] Q为四元数向量，q0、ql、q2、q3为组成四元数向量的标量，i、j、k为三维坐标系单 位向量，更新后的姿态矩阵为：
[0093]
[0094] 0为载体坐标系到导航坐标系的旋转矩阵。
[0096] 其中Y、0、也分别为横滚角、俯仰角和航向角。
[0097] 其中，所述九轴MEMS传感器由三轴陀螺仪、三轴加速度计和三轴地磁传感器组 成。
[0098] 实施例2 :
[0099] 如图2所示，一种基于九轴MEMS传感器的农业机械全姿态角更新方法，所述基于 九轴MEMS传感器的农业机械全姿态角更新方法包括如下步骤：
[0100] 步骤S1 :建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型，并设 定相应车体运动姿态的参数向量；
[0101] 所述步骤S1 :建立陀螺仪误差模型、电子罗盘校准椭圆模型与七维EKF滤波模型， 并设定相应车体运动姿态的参数向量步骤具体为：
[0102] 陀螺仪误差模型通过陀螺仪误差计算公式对陀螺仪角速度进行计算，其中，陀螺 仪误差计算公式：《 = ?lb+b_"+bug，其中《为陀螺仪输出角速度，C0lb为陀螺仪真实角速 度，为陀螺仪零漂，bug为陀螺仪输出白噪声；
[0103] 通过电子罗盘校准椭圆模型消除磁场干扰；其中，电子罗盘校准椭圆模型：
ix，my为地磁场强度，Xoffset和Yoffset为硬磁干扰， Xsf?和Ysf为软磁干扰；
[0104] 通过七维EKF滤波模型对车体姿态进行更新处理，其中，七维EKF滤波模型为七维 状态向量的扩展卡尔曼滤波，EKF包括状态方程与观测方程：
[0105] .v= /'(-V,a>) +ul
[0106] y=h(x)+vl
[0107] 状态矩阵为x=[qbuJ，q为四元数向量qO，ql，q2，q3，b("为XYZ三轴陀螺仪零 漂；其中《为陀螺仪输出角速度，wl为过程噪声矩阵，vl为观测噪声矩阵，y为观测量，y =[a$nag]T，其中a为三轴加速度值，$nag为电子罗盘计算的航向角，
[0108]
[0109]
[0110] 由于地磁场强度比较微弱，容易受到周围铁质材料和电磁场的影响，所以在使用 之前必须进行校准，建立电子罗盘校准椭圆模型用于消除磁场所带的干扰，实际校准过程 是通过最小二乘法对采集到的磁场强度进行拟合，然后得出上述参数。
[0111] 步骤S2 :通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁场强 度数据；
[0112] 所述步骤S2 :通过九轴MEMS传感器实时获取车体运动的加速度、角速度与地球磁 场强度数据步骤具体为：
[0113] 通过陀螺仪获取车体的角速度，对陀螺仪零点漂移进行补偿；
[0114] 通过加速度传感器采集车体的加速度数据；
[0115] 通过地磁传感器采集车体的地磁场强度数据。
[0116] 步骤S3 :根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建立的 陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角度；
[0117] 所述步骤S3 :根据获取的车体运动的加速度、角速度与地球磁场强度数据通过建 立的陀螺仪误差模型、电子罗盘校准椭圆模型计算出车体的角度、速度、位置信息、航向角 度步骤具体为：
[0118] 通过陀螺仪误差模型对角速度进行积分计算获得角度数据；
[0119] 通过对加速度数据的积分计算出速度，再次积分计算出位置信息；地磁场强度数 据经椭圆模型计算出来的校准参数补偿和倾角修正后计算车体航向角。
[0120] 通过MEMS传感器实时采集车体的运动信息，陀螺仪采集的车体角速度通过状态 估计量陀螺仪零偏进行修正，对其积分计算出角度增量，地磁传感器经软磁、硬磁和倾角修 正补偿后计算出航向角。
[0121] 步骤S4:通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进行数 据融合处理，对车体的运动姿态角进行实时更新。
[0122] 所述步骤S4:通过七维EKF滤波模型对车体的角度、速度、位置信息、航向角度进 行数据融合处理，对车体的运动姿态角进行实时更新步骤