Java教程

匿名mahony互补滤波代码详解

本文主要是介绍匿名mahony互补滤波代码详解,对大家解决编程问题具有一定的参考价值,需要的程序猿们随着小编来一起学习吧!

下面是我对匿名源码修改整理后得到的代码,去掉了冗余便于讲解,此外给出了一些优化建议。
一、函数调用
在这里插入图片描述

二、Drv_Icm20602_Read()
在这里插入图片描述
读取16位ADC采样值

三、imu_origDataPrepare()
在这里插入图片描述
因为传感器安装角度和载体坐标系可能不一致,因此需要一个基础的坐标转换,最终得到x前、y右、z下的载体坐标系。

在这里插入图片描述
因为加速度计对高频抖动敏感,因此需要一个低通滤波器。陀螺仪加一个低通滤波无伤大雅。

在这里插入图片描述
当传感器不在载体中心时,载体旋转时传感器会有一个向心加速度,该加速度会被传感器测量出来,因此要去掉。

在这里插入图片描述
将16位ADC转换为物理单位值,对于该例程为±16g的加速度,

这篇关于匿名mahony互补滤波代码详解的文章就介绍到这儿,希望我们推荐的文章对大家有所帮助,也希望大家多多支持为之网!