基于卡尔曼滤波的姿态控制方法及装置
摘要:
本发明公开了一种基于卡尔曼滤波的姿态控制方法及装置,该方法包括:根据当前时刻陀螺仪测量得到的参数和采样时间间隔,得到预测矩阵;获取上一时刻姿态值最优解对应的矩阵,根据预测矩阵和上一时刻姿态值最优解对应的矩阵,得到更新后的预测矩阵;根据当前时刻加速度计测量得到的参数和磁力计测量得到的参数,得到加速度计观测矩阵和磁力计观测矩阵;对更新后的预测矩阵和加速度计观测矩阵进行第一阶卡尔曼滤波计算,得到当前时刻临时姿态最优解对应的矩阵;对磁力计观测矩阵和当前时刻临时姿态最优解对应的矩阵进行第二阶卡尔曼滤波计算,得到当前时刻姿态值最优解对应的矩阵;利用当前时刻姿态值最优解对应的矩阵控制设备的姿态。
公开/授权文献
0/0