huangyiting1990 发表于 2020-4-1 09:34:04

看到一个无法理解的姿态解算算法，请教一下大家

            看到一个姿态解算算法无法理解，贴上来大伙给分析分析。

        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   这个角度直接乘以加速度，得到的结果还是加速度？   

       数学没学好啊，一脸懵逼
            

qwe2231695 发表于 2020-4-1 12:46:46

完整代码在这里,一起看看
https://github.com/silver13/BoldClash-BWHOOP-B-03/blob/master/Silverware/src/imu.c

qwe2231695 发表于 2020-4-1 13:01:33

好像是罗德里格旋转公式,加小角度优化掉三角函数

huangyiting1990 发表于 2020-4-1 13:24:58

qwe2231695 发表于 2020-4-1 13:01
好像是罗德里格旋转公式,加小角度优化掉三角函数

是的， 就是这个飞控。
这个姿态解算算法，真心看不太懂{:lol:}

lcw_swust 发表于 2020-4-1 14:24:42

本帖最后由 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]
查看完整版本: 看到一个无法理解的姿态解算算法，请教一下大家