看到一个无法理解的姿态解算算法,请教一下大家
看到一个姿态解算算法无法理解,贴上来大伙给分析分析。for ( int i = 0 ; i < 3 ; i++)
{
deltaGyroAngle = (gyro) * looptime;
}
GEstG = GEstG - (deltaGyroAngle)* GEstG;
GEstG = (deltaGyroAngle) * GEstG+ GEstG;
GEstG =GEstG + (deltaGyroAngle) * GEstG;
GEstG = -(deltaGyroAngle) * GEstG + GEstG;
GEstG = GEstG - (deltaGyroAngle)* GEstG;
GEstG = (deltaGyroAngle) * GEstG+ GEstG;
其中 :gyro 为各个轴向的角速度 , looptime 为单个计算周期实际时间间隔 。 GEstG 为姿态角(单位为g)
疑惑 :(1) 这个是什么算法,欧拉角?那是不是计算顺序固定不能更改?
(2) (deltaGyroAngle)* GEstG 这个角度直接乘以加速度,得到的结果还是加速度?
数学没学好啊,一脸懵逼
完整代码在这里,一起看看
https://github.com/silver13/BoldClash-BWHOOP-B-03/blob/master/Silverware/src/imu.c 好像是罗德里格旋转公式,加小角度优化掉三角函数 qwe2231695 发表于 2020-4-1 13:01
好像是罗德里格旋转公式,加小角度优化掉三角函数
是的, 就是这个飞控。
这个姿态解算算法,真心看不太懂{:lol:} 本帖最后由 lcw_swust 于 2020-4-1 14:26 编辑
我也没看懂,不过我的姿态解算是用的向量旋转公式.
推导在这帖子的32楼,不知道和楼主这个是否一样.
https://www.amobbs.com/thread-5614575-1-1.html
姿态处理
采用二维角度旋转公式
x'=x*cosθ-y*sinθ
y'=x*sinθ+y*cosθ
应用到三维,且由于角度很小时sinX=0,cosX=1,对公式加以简化,得到:
GAX+=(Gz*GAY-Gy*GAZ)/SINDIV;
GAY+=(Gx*GAZ-Gz*GAX)/SINDIV;
GAZ+=(Gy*GAX-Gx*GAY)/SINDIV;
(Gx\Gy\Gz为陀螺仪数据,GAX\GAY\GAZ为姿态向量,和加速度差不多,可由atan2得到姿态角.)
(SINDIV这个系数需要实际测试一下)
然后与加速度向量进行互补滤波融合。
页:
[1]