AHRS 代码求讲解

zz274510110

最近在学习欧拉角、旋转矩阵、四元数等，下面这套代码是姿态矫正中的一个文件，入口参数gx、gy、gz是陀螺仪数据三个分量，ax、ay、az是加速度数据三个分量，mx、my、mz是磁阻数据三个分量。
函数的目的是更新AHRS更新四元数。
只看懂了前面一少部分，后面的差乘那些，不明白，不知道出于那些公式，又是什么目的。

请懂得的人讲解一下，让大家的姿态算法更进一步啊



/*
* AHRS
* Copyright 2010 S.O.H. Madgwick
*
* This program is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser Public License as
* published by the Free Software Foundation, either version 3 of the
* License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
* Lesser Public License for more details.
*
* You should have received a copy of the GNU Lesser Public License
* along with this program.  If not, see
* <http://www.gnu.org/licenses/>.
*/
// AHRS.c
// S.O.H. Madgwick
// 25th August 2010
//
// Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
// compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
// axis only.
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.  See my report for an overview of the use of quaternions in this application.
//
// User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
// radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
//


#include "AHRS.h"
#include <math.h>


#define Kp 2.0f

zz274510110

#define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f                // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.5f                // half the sample period


float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
        float norm;
        float hx, hy, hz, bx, bz;
        float vx, vy, vz, wx, wy, wz;
        float ex, ey, ez;

        // auxiliary variables to reduce number of repeated operations
        float q0q0 = q0*q0;
        float q0q1 = q0*q1;
        float q0q2 = q0*q2;
        float q0q3 = q0*q3;
        float q1q1 = q1*q1;
        float q1q2 = q1*q2;
        float q1q3 = q1*q3;
        float q2q2 = q2*q2;   
        float q2q3 = q2*q3;
        float q3q3 = q3*q3;         
       
        // normalise the measurements
        norm = sqrt(ax*ax + ay*ay + az*az);      
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;
        norm = sqrt(mx*mx + my*my + mz*mz);         
        mx = mx / norm;
        my = my / norm;
        mz = mz / norm;         
       
        // compute reference direction of flux
        hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
        hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
        hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
        bx = sqrt((hx*hx) + (hy*hy));
        bz = hz;        
       
        // estimated direction of gravity and flux (v and w)
        vx = 2*(q1q3 - q0q2);
        vy = 2*(q0q1 + q2q3);
        vz = q0q0 - q1q1 - q2q2 + q3q3;
        wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
        wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
        wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
       
        // error is sum of cross product between reference direction of fields and direction measured by sensors
        ex = (ay*vz - az*vy) + (my*wz - mz*wy);
        ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
        ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
       
        // integral error scaled integral gain
        exInt = exInt + ex*Ki;
        eyInt = eyInt + ey*Ki;
        ezInt = ezInt + ez*Ki;
       
        // adjusted gyroscope measurements
        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt;
       
        // integrate quaternion rate and normalise
        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;  
       
        // normalise quaternion
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;
}

zywei_09

有人发过的~而且就在你发帖的这一页，名字是“捷联惯导算法心得”；
http://www.amobbs.com/thread-5492189-1-1.html

zywei_09

需要一些惯性导航的基本知识，总体思路是把加计得到的重力方向矢量和陀螺仪积分得到的重力方向矢量叉乘，得到两矢量的差，在此差的基础上PI运算，消除误差。

zz274510110

zywei_09 发表于 2012-8-18 23:30
需要一些惯性导航的基本知识，总体思路是把加计得到的重力方向矢量和陀螺仪积分得到的重力方向矢量叉乘，得 ...

谢谢你，我会好好咀嚼你那句总结，并认真看这条链接~~

260186221

楼主 这个代码哪里下载的呢？ 能否给个网址

小乖

我也去看看，谢谢楼主

小乖

翻来翻去还是这几个帖子

fangchangqing

来看看   

codwest

收了
好东西

yat

同求高手
mark AHRS 代码

机器人天空

mark........

提线木偶

好好理解《捷联惯导算法心得》这个帖子

吉祥瑞

我也在研究飞控算法  mark一下

yat

mark Madgwick

nhw1234

学习了

时光

zz274510110 发表于 2012-8-18 22:43
#define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#de ...

请问楼主 你是怎么理解那个Halft的  我一直都不明白  是我们设置的陀螺仪的采样频率的一半  还是系统从上电一开始到我们运行那个姿态更新函数 的时间一半啊  难道我们得一直统计这个时间间隔吗？可是如果我程序中间有些延时的话 那根本没法计算呀。。小弟愚钝 求指导。。。。

1558582190

时光 发表于 2014-8-27 22:47
请问楼主 你是怎么理解那个Halft的  我一直都不明白  是我们设置的陀螺仪的采样频率的一半  还是系统从上 ...

你去看一阶龙格库塔法就明白了