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

huangyiting1990

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

        for ( int i = 0 ; i < 3 ; i++)
        {
                deltaGyroAngle[i] = (gyro[i]) * looptime;
        }

        GEstG[2] = GEstG[2] - (deltaGyroAngle[0])  * GEstG[0];
        GEstG[0] = (deltaGyroAngle[0]) * GEstG[2]  + GEstG[0];


        GEstG[1] =  GEstG[1] + (deltaGyroAngle[1]) * GEstG[2];
        GEstG[2] = -(deltaGyroAngle[1]) * GEstG[1] + GEstG[2];


        GEstG[0] = GEstG[0] - (deltaGyroAngle[2])  * GEstG[1];
        GEstG[1] = (deltaGyroAngle[2]) * GEstG[0]  + GEstG[1];

       其中 :  gyro[i] 为各个轴向的角速度 ， looptime 为单个计算周期实际时间间隔 。 GEstG[i] 为姿态角(单位为g)

       疑惑 :(1) 这个是什么算法，欧拉角？  那是不是计算顺序固定不能更改？
               (2) (deltaGyroAngle[0])  * GEstG[0]   这个角度直接乘以加速度，得到的结果还是加速度？     

       数学没学好啊，一脸懵逼
              

qwe2231695

完整代码在这里,一起看看
https://github.com/silver13/Bold ... ilverware/src/imu.c

qwe2231695

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

huangyiting1990

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

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

lcw_swust

我也没看懂,不过我的姿态解算是用的向量旋转公式.
推导在这帖子的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这个系数需要实际测试一下)
然后与加速度向量进行互补滤波融合。