[转载]四轴融合算法详解
1.1加速度计，测量的是"比力"。
根据牛顿惯性定律，当载体相对惯性空间以加速度a运动时，可以用载体中的加速度计测出作用在单位质量上惯性力和引力的矢量和的大小，即比力的大小。
& MSF = ma &
mg&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
——加速度计检测质量受到的比力；
——感受加速度的检测质量；
——载体的运动加速度；
加速度计测量的是机体坐标系(b系)相对于惯性空间的加速度在机体坐标系中的投影，该测试量也称为比力。而对于我们四轴姿态计算，要在机体坐标系中完成，测出的加速度就是机体上的，只要取出重力分量。重力分量在世界坐标系中是【0,0,1】.这一转换由姿态矩阵完成，而是利用陀螺仪的输出即载体相对惯性空间转动的角速率在机体坐标系下的投影计算得到。姿态矩阵是随时间的变化而不断变化的。另外，从姿态矩阵中可以单值的确定飞行器的姿态角。捷联式惯导系统中需要实时地求取姿态矩阵，以便提取飞行器姿态角（首向角、纵摇角、横摇角）以及变换比力。
机体上测得的重力正好和g对应。
地理坐标系OXtYtZt之间的方位关系.用矩阵表达。也可用四元数表达。四元数是“轴-角”描述。yaw-pitch-roll
3次转动，合并为绕通过顶点"O"的向量n转过θ/2。
其中含有不可交换误差。
函数里的积分时间，误差都用了half，因为外层算法中融合是在1/2周期时做。
void sensfusion6UpdateQ(float gx, float gy, float gz, float
ax, float ay, float az, float dt)
& float recipN
& float halfvx, halfvy,
& float halfex, halfey,
& float qa, qb,
& //弧度转变为平时常用的角度
& gx = gx * M_PI / 180;
& gy = gy * M_PI / 180;
& gz = gz * M_PI / 180;
& // Compute feedback only if accelerometer
measurement valid (avoids NaN in accelerometer normalisation)
& if(!((ax == 0.0f)
&& (ay == 0.0f)
&& (az == 0.0f)))
& & // Normalise
accelerometer measurement&
//平方根倒数快速算法,得到直接测得的加速度,机体坐标
//归一化，转成单位向量,统统除以模
& & recipNorm = invSqrt(ax *
ax + ay * ay + az * az);
& & ax *= recipN
& & ay *= recipN
& & az *= recipN
&& // Estimated direction of
gravity and vector perpendicular to magnetic
&&//根据姿态矩阵得到的加速度。世界转机体坐标，是姿态矩阵的转置
& //就是标准重力矢量【0 0
1】通过四元数旋转，得到的机体矢量&机体坐标
//这里是从世界坐标到机体坐标，应用了矩阵向量积,结果为矩阵中的行向量与右边每行元素的点积
&书上都是从b系机体坐标转移到世界坐标，而我们需要把重力向量转移到机体坐标，所以要求逆矩阵
&而又因为是标准正交矩阵，逆等于转置，得到下面的矩阵。
& & halfvx = q1 * q3 - q0 *
& & halfvy = q0 * q1 + q2 *
&//1==q0^2+q1^2+q3^2+q4^2; 做一个代数运算
目的是减少计算量
& & halfvz = q0 * q0 - 0.5f
+ q3 * q3;
& & // Error is sum of cross
product between estimated and measured direction of gravity
//叉乘，得到重力误差&忽略不可交换误差
& & halfex = (ay * halfvz -
az * halfvy);
& & halfey = (az * halfvx -
ax * halfvz);
& & halfez = (ax * halfvy -
ay * halfvx);
& & //使用PI来纠正误差
叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。
& &//特殊的互补滤波
& & // Compute and apply
integral feedback if enabled
& & if(twoKi
integralFBx += twoKi * halfex * &// integral
error scaled by Ki
integralFBy += twoKi * halfey *
integralFBz += twoKi * halfez *
& & & gx +=
integralFBx; &// apply integral feedback
& & & gy +=
integralFBy;
& & & gz +=
integralFBz;
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
& & // Apply proportional
& & gx += twoKp *
& & gy += twoKp *
& & gz += twoKp *
//到此就得到了融合过的角速度，纠正了陀螺的零偏，但是没有磁场纠正
& // Integrate rate of change of
quaternion&
&//陀螺仪直接积分得到角度,直接测得的角速度乘以变化时间得到角度,这里提前积分了
& gx *= (0.5f * dt); & //
pre-multiply common factors
& gy *= (0.5f * dt);
& gz *= (0.5f * dt);//现在gx gy gz
表示了旋转角度
& qa = q0;
& qb = q1;
& qc = q2;
& //数值积分得到角度，用于解微分方程
& q0 += (-qb * gx - qc * gy - q3 * gz);
& q1 += (qa * gx + qc * gz - q3 * gy);
& q2 += (qa * gy - qb * gz + q3 * gx);
& q3 += (qa * gz + qb * gy - qc * gx);
& // Normalise quaternion
& recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2
* q2 + q3 * q3);
& q0 *= recipN
& q1 *= recipN
& q2 *= recipN
& q3 *= recipN
四元数构造方向余弦矩阵的推导：
关于微分方程的说明
【q0,q1,q2,q3】是旧的姿态矩阵，【gx,gy,gz】是融合过的当前角速度，要求出当前的新的【q0,q1,q2,q3】
如用方向余弦矩阵,
微分方程为&
C^ = C U（角度向量)
如果用四元数，微分方程为
q^ = 1/2 w （角度向量的四元数表示）*（四元数乘法）
设在t时刻动系OXbYbZb以角速度ω相对定系OXtYtZt转动，则动系b和定系t之间的变换四元数Q同样是时间的函数，可用Q(t)表示t时刻的四元数，Q(t+Δt)表示t+Δt时刻的四元数，如图3.3所示。
从图3.3可以看出，Q(t)和ΔQ(t)之间和为Q(t+Δt),故有：
&Q(t+Δt)=ΔQ(t)Q(t)
以上网友发言只代表其个人观点，不代表新浪网的观点或立场。四轴飞行器姿态解算方法分析
摘自：.cn/article/247809.htm
仅供学习参考，转载注明原处。
本文将分析一种常见的飞行器姿态方法，Mahony的互补。此法简单有效，希望能给学习四轴飞行器的朋友们带来帮助。关于姿态和滤波的理论知识，推荐秦永元的两本书，一是《惯性导航》，目前已出到第二版了;二是《卡尔曼滤波与组合导航原理》。程序中的理论基础，可在书中寻找。
　　同时欢迎到论坛发帖交流：.cn/forum/368/1
　　下面开始进入正题：
　　先定义Kp，Ki，以及halfT 。
　　Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度
　　halfT ，姿态时间的一半。此处解算姿态速度为500HZ，因此halfT
　　#define Kp 2.0f
　　#define Ki 0.002f
　　#define halfT 0.001f
　　初始化四元数
　　float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
　　定义姿态解算误差的积分
　　float exInt = 0, eyInt = 0, ezInt = 0;
　　以下为姿态解算函数。
　　参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒;
　　参数ax，ay，az分别对应三个轴的加速度原始数据
　　由于加速度的噪声较大，此处应采用滤波后的数据
　　void IMUupdate(float gx, float gy, float gz, float ax, float ay,
　　float vx, vy,
　　float ex, ey,
　　将加速度的原始数据，归一化，得到单位加速度
　　norm = sqrt(ax*ax + ay*ay + az*az);
　　ax = ax /
　　ay = ay /
　　az = az /
　　把四元数换算成“方向余弦矩阵”中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx、vy、vz，其实就是当前的机体坐标参照系上，换算出来的重力单位向量。(用表示机体姿态的四元数进行换算)
　　vx = 2*(q1*q3 - q0*q2);
　　vy = 2*(q0*q1 + q2*q3);
　　vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
　　这里说明一点，加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。
　　在机体坐标参照系上，加速度计测出来的重力向量是ax、ay、陀螺积分后的姿态来推算出的重力向量是vx、vy、它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。
　　向量间的误差，可以用向量积(也叫外积、叉乘)来表示，ex、ey、ez就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
　　叉乘是数学基础，百度百科里有详细解释。
　　ex = (ay*vz - az*vy);
　　ey = (az*vx - ax*vz);
　　ez = (ax*vy - ay*vx);
　　将叉乘误差进行积分
　　exInt = exInt + ex*Ki;
　　eyInt = eyInt + ey*Ki;
　　ezInt = ezInt + ez*Ki;
　　用叉乘误差来做PI修正陀螺零偏，通过调节Kp，Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度
　　gx = gx + Kp*ex + exI
　　gy = gy + Kp*ey + eyI
　　gz = gz + Kp*ez + ezI
　　四元数微分方程，没啥好说的了，看上面推荐的书吧，都是理论的东西，自个琢磨琢磨
　　实在琢磨不明白，那就把指定的参数传进这个函数，再得到相应的四元数，最后转化成欧拉角即可了。不过建议还是把理论弄清楚一点。
　　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 /
　　q1 = q1 /
　　q2 = q2 /
　　q3 = q3 /
　　姿态解算后，就得到了表示姿态的四元数。但四元数不够直观，一般将其转化为欧拉角。转化时根据旋转轴的次序不同，公式也不同。以下给出其中一种公式：
　　读完程序，深刻的意识到了理论基础的重要性。Mahony的互补滤波函数，确实很巧妙，利用叉乘误差来修正四轴的姿态，姿态解算速度越快，则解算的精度越高。在许多国内开源程序中，也是用到了这种方法。在解四元数微分方程时，该程序用到了一阶毕卡解法。同样可用于解四元数微分方程的还有龙格-库塔法，由于篇幅有限，此处就不介绍龙格-库塔法了，有兴趣的网友请自行查阅相关资料。
已投稿到：
以上网友发言只代表其个人观点，不代表新浪网的观点或立场。一种常见的四轴飞行器姿态解算方法分析
> 一种常见的四轴飞行器姿态解算方法分析
一种常见的四轴飞行器姿态解算方法分析
　　全国各地已经陆续开放低空管制，北京也将在2015年全面开放低空领域，这对低空飞行器将是一个十分重大的好消息!低空飞行器也将迎来一个新的发展春天。实际上，近年飞行器发展相当迅速，国内的航拍水平越来越高，顺丰及亚马逊已在尝试将无人机用于快递行业。越来越多的人开始关注并研究飞行器。本文引用地址：
　　本文将分析一种常见的飞行器姿态方法，Mahony的互补。此法简单有效，希望能给学习四轴飞行器的朋友们带来帮助。关于姿态和滤波的理论知识，推荐秦永元的两本书，一是《惯性导航》，目前已出到第二版了;二是《卡尔曼滤波与组合导航原理》。程序中的理论基础，可在书中寻找。
　　同时欢迎到论坛发帖交流：.cn/forum/368/1
　　下面开始进入正题：
　　先定义Kp，Ki，以及halfT 。
　　Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度
　　halfT ，姿态时间的一半。此处解算姿态速度为500HZ，因此halfT 为0.001
　　#define Kp 2.0f
　　#define Ki 0.002f
　　#define halfT 0.001f
　　初始化四元数
　　float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
　　定义姿态解算误差的积分
　　float exInt = 0, eyInt = 0, ezInt = 0;
　　以下为姿态解算函数。
　　参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒;
　　参数ax，ay，az分别对应三个轴的加速度原始数据
　　由于加速度的噪声较大，此处应采用滤波后的数据
　　void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
　　float vx, vy,
　　float ex, ey,
　　将加速度的原始数据，归一化，得到单位加速度
　　norm = sqrt(ax*ax + ay*ay + az*az);
　　ax = ax /
　　ay = ay /
　　az = az /
　　把四元数换算成&方向余弦矩阵&中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx、vy、vz，其实就是当前的机体坐标参照系上，换算出来的重力单位向量。(用表示机体姿态的四元数进行换算)
　　vx = 2*(q1*q3 - q0*q2);
　　vy = 2*(q0*q1 + q2*q3);
　　vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
　　这里说明一点，加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。
　　在机体坐标参照系上，加速度计测出来的重力向量是ax、ay、陀螺积分后的姿态来推算出的重力向量是vx、vy、它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。
　　向量间的误差，可以用向量积(也叫外积、叉乘)来表示，ex、ey、ez就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
　　叉乘是数学基础，百度百科里有详细解释。
　　ex = (ay*vz - az*vy);
　　ey = (az*vx - ax*vz);
　　ez = (ax*vy - ay*vx);
　　将叉乘误差进行积分
　　exInt = exInt + ex*Ki;
　　eyInt = eyInt + ey*Ki;
　　ezInt = ezInt + ez*Ki;
　　用叉乘误差来做PI修正陀螺零偏，通过调节Kp，Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度
　　gx = gx + Kp*ex + exI
　　gy = gy + Kp*ey + eyI
　　gz = gz + Kp*ez + ezI
　　四元数微分方程，没啥好说的了，看上面推荐的书吧，都是理论的东西，自个琢磨琢磨
　　实在琢磨不明白，那就把指定的参数传进这个函数，再得到相应的四元数，最后转化成欧拉角即可了。不过建议还是把理论弄清楚一点。
　　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 /
　　q1 = q1 /
　　q2 = q2 /
　　q3 = q3 /
　　姿态解算后，就得到了表示姿态的四元数。但四元数不够直观，一般将其转化为欧拉角。转化时根据旋转轴的次序不同，公式也不同。以下给出其中一种公式：
　　读完程序，深刻的意识到了理论基础的重要性。Mahony的互补滤波函数，确实很巧妙，利用叉乘误差来修正四轴的姿态，姿态解算速度越快，则解算的精度越高。在许多国内开源程序中，也是用到了这种方法。在解四元数微分方程时，该程序用到了一阶毕卡解法。同样可用于解四元数微分方程的还有龙格-库塔法，由于篇幅有限，此处就不介绍龙格-库塔法了，有兴趣的网友请自行查阅相关资料。
电源滤波器相关文章:
负离子发生器相关文章:
加速度计相关文章:
矢量控制相关文章:
数字滤波器相关文章:
离子色谱仪相关文章:
分享给小伙伴们：
我来说两句……
微信公众号二
微信公众号一您当前的位置：&>&&>&
一种常见的四轴飞行器姿态解算方法分析
&全国各地已经陆续开放低空管制，北京也将在2015年全面开放低空领域，这对低空飞行器将是一个十分重大的好消息!低空飞行器也将迎来一个新的发展春天。实际上，近年四轴飞行器发展相当迅速，国内的航拍水平越来越高，顺丰及亚马逊已在尝试将无人机用于快递行业。越来越多的人开始关注并研究四轴飞行器。
本文将分析一种常见的四轴飞行器姿态解算方法，Mahony的互补滤波法。此法简单有效，希望能给学习四轴飞行器的朋友们带来帮助。关于姿态解算和滤波的理论知识，推荐秦永元的两本书，一是《惯性导航》，目前已出到第二版了;二是《卡尔曼滤波与组合导航原理》。程序中的理论基础，可在书中寻找。
下面开始进入正题：
先定义Kp，Ki，以及halfT 。
Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度
halfT ，姿态解算时间的一半。此处解算姿态速度为500HZ，因此halfT 为0.001
#define Kp 2.0f
#define Ki 0.002f
#define halfT 0.001f
初始化四元数
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
定义姿态解算误差的积分
float exInt = 0, eyInt = 0, ezInt = 0;
以下为姿态解算函数。
参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒;
参数ax，ay，az分别对应三个轴的加速度原始数据
由于加速度的噪声较大，此处应采用滤波后的数据
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float
float vx, vy,
float ex, ey,
将加速度的原始数据，归一化，得到单位加速度
norm = sqrt(ax*ax + ay*ay + az*az);
把四元数换算成&方向余弦矩阵&中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx、vy、vz，其实就是当前的机体坐标参照系上，换算出来的重力单位向量。(用表示机体姿态的四元数进行换算)
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
这里说明一点，加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。
在机体坐标参照系上，加速度计测出来的重力向量是ax、ay、陀螺积分后的姿态来推算出的重力向量是vx、vy、它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。
向量间的误差，可以用向量积(也叫外积、叉乘)来表示，ex、ey、ez就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
叉乘是数学基础，百度百科里有详细解释。
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
将叉乘误差进行积分
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
用叉乘误差来做PI修正陀螺零偏，通过调节Kp，Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度
gx = gx + Kp*ex + exI
gy = gy + Kp*ey + eyI
gz = gz + Kp*ez + ezI
四元数微分方程，没啥好说的了，看上面推荐的书吧，都是理论的东西，自个琢磨琢磨
实在琢磨不明白，那就把指定的参数传进这个函数，再得到相应的四元数，最后转化成欧拉角即可了。不过建议还是把理论弄清楚一点。
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);
姿态解算后，就得到了表示姿态的四元数。但四元数不够直观，一般将其转化为欧拉角。转化时根据旋转轴的次序不同，公式也不同。以下给出其中一种公式：
读完程序，深刻的意识到了理论基础的重要性。Mahony的互补滤波函数，确实很巧妙，利用叉乘误差来修正四轴的姿态，姿态解算速度越快，则解算的精度越高。在许多国内开源程序中，也是用到了这种方法。在解四元数微分方程时，该程序用到了一阶毕卡解法。同样可用于解四元数微分方程的还有龙格-库塔法，由于篇幅有限，此处就不介绍龙格-库塔法了，有兴趣的网友请自行查阅相关资料。
智能汽车、智能手机、智能冰箱、智能手表，甚至戒指、水杯……看起来在这疯狂的时代…
热门关键词
() () () () () () () () ()}