互补滤波姿态估计程序

原创 2016年08月30日 21:33:42
/**
 * 6DOF 互补滤波姿态估计(via Mahony)
 * @param[in] halfT:状态估计周期的一半
 */
const float Kp = 3.5, Ki = 0.05;
float exInt, eyInt, ezInt;
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // roll,pitch,yaw 都为 0 时对应的四元数值。
void imuEstimatedAttitude(float gx, float gy, float gz, float ax, float ay, float az, float halfT) 
{
    float norm;
    float vx, vy, vz;  // v-陀螺仪积分后推算出的重力向量
    float ex, ey, ez;  // e-两重力向量的叉积(向量间的误差 a与v之间)   

    // 加速度原始数据归一化,得到单位化的加速度。
    norm = sqrt(ax*ax + ay*ay + az*az);
    ax = ax/norm;
    ay = ay/norm;
    az = az/norm;

    // 坐标系变换,将地理坐标系“R系”的重力向量转换到机体坐标系“b系”的v_xyz
    vx = 2*(q1*q3 - q0*q2);
    vy = 2*(q0*q1 + q2*q3);
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

    // a_xyz是测量得到的重力向量,v_xyz是陀螺积分后的姿态来推算出的重力向量,它们都是机体坐标参照系上的重力向量。
    // 那它们之间的误差向量,就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
    // 这个叉积向量仍旧是位于机体坐标系上的,而陀螺积分误差也是在机体坐标系,
    // 而且叉积的大小与陀螺积分误差成正比,正好拿来纠正陀螺仪积分误差。
    ex = (ay*vz - az*vy);
    ey = (az*vx - ax*vz);
    ez = (ax*vy - ay*vx);

    // 误差作PI修正
    exInt = exInt + ex*Ki;
    eyInt = eyInt + ey*Ki;
    ezInt = ezInt + ez*Ki;

    gx = gx + Kp*ex +exInt;
    gy = gy + Kp*ey +eyInt;
    gz = gz + Kp*ez +ezInt;

    // 四元数微分方程求解
    q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0/norm;
    q1 = q1/norm;
    q2 = q2/norm;
    q3 = q3/norm;

    roll =  atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)*57.3;     
    pitch =  asinf(2*q1*q3 - 2*q0*q2)*57.3;                                                          
    yaw  =  -atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 -2*q3*q3 + 1)*57.3; 
}
版权声明:本文为博主原创文章。

四旋翼姿态解算——互补滤波算法及理论推导

上次我们讨论了姿态解算基础理论以及几个比较重要的公式的一些推导,如果有不熟悉的请点击这里查看。这次来介绍一些实际的姿态解算算法吧! 一般在程序中,姿态解算的方式有两种:一种是欧拉角法,一种是四元数法...
  • hongbin_xu
  • hongbin_xu
  • 2017年02月24日 17:37
  • 3572

Mahony 互补滤波

版权声明:本文为博主原创文章,转载请注明,欢迎交流~ Mahony互补滤波 目录(?)[+] 上接【互补滤波器】,继续学习互补滤波。。。。 参考...
  • qing101hua
  • qing101hua
  • 2016年11月04日 01:00
  • 5562

互补滤波精髓

  • 2014年08月21日 09:56
  • 773KB
  • 下载

基于四元数的简单互补滤波姿态解算

序言鉴于现在飞控的热度不减当年,越来越多“年轻人”加入飞控制作学习的阶段,也有许多新手上来就气势汹汹的进行姿态解算,往往使用的就是较为常用的Mahony的互补滤波姿态解算,笔者在多年前也一样,最开始接...
  • wubaobao1993
  • wubaobao1993
  • 2016年11月11日 17:02
  • 4659

mahony 互补滤波器

px4 中 mahony 滤波算法的分析与源码
  • luoshi006
  • luoshi006
  • 2016年05月27日 10:57
  • 11121

Mahony 互补滤波算法

 http://blog.csdn.net/luoshi006/article/details/51513580 上接【互补滤波器】,继续学习互补滤波。。。。 参考: Keeping...
  • huohongpeng
  • huohongpeng
  • 2017年06月21日 08:59
  • 707

姿态解算&&互补滤波

本文详细分析了互补滤波的原理,以及对飞行器上姿态解算的几个常用方法进行了比较,值得学习...
  • u012814946
  • u012814946
  • 2016年10月18日 16:55
  • 2446

一阶互补滤波

MPU6050可以获得加速度和角速度,但是由于传感器的噪声信号比较大,无法直接通过两个数据来获得该轴上的角度,因此,需要进行互补滤波来近似得到一个比较准确的角度。 传感器原始数据特点分析:加速度数据,...
  • ysudykx
  • ysudykx
  • 2017年04月01日 16:14
  • 4536

互补滤波算法

  • 2011年12月07日 10:50
  • 4KB
  • 下载

关于互补滤波原理

先介绍一下互补滤波的基本概念,这是阿莫论坛上一个会员的总结:对mpu6050来说,加速度计对四轴或小车的加速度比较敏感,取瞬时值计算倾角误差比较大;而陀螺仪积分得到的角度不受小车加速度的影响,但是随着...
  • superrunner_wujin
  • superrunner_wujin
  • 2017年02月19日 20:44
  • 3123
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:互补滤波姿态估计程序
举报原因:
原因补充:

(最多只允许输入30个字)