这段互补滤波的代码你们能看懂么？

skldjfkjf

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

//----------------------------------------------------------------------------------------------------
// Definitions

#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

//---------------------------------------------------------------------------------------------------
// Variable definitions

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

//====================================================================================================
// Function
//====================================================================================================

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;         

        // normalise the measurements
        norm = sqrt(ax*ax + ay*ay + az*az);      
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;      
把加计的三维向量转成单位向量。


        // estimated direction of gravity
        vx = 2*(q1*q3 - q0*q2);
        vy = 2*(q0*q1 + q2*q3);
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;


        // error is sum of cross product between reference direction of field and direction measured by sensor
        ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);





        // 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;
}

//====================================================================================================
// END OF CODE
//====================================================================================================

skldjfkjf

感觉和论坛里普通的互补滤波不太一样啊

rubick

不就是三维的，多了几个叉乘点乘么

Eternalve

这是 四元数算法 吧  四轴之类的飞控用的多

为丞而琳

这是四元数算法，不是互补滤波算法

skldjfkjf

rubick 发表于 2013-11-30 22:23
不就是三维的，多了几个叉乘点乘么

楼下的说这不是互补滤波，到底是不是啊？？

感觉和你们常用的那种不一样啊

skldjfkjf

为丞而琳 发表于 2013-11-30 23:51
这是四元数算法，不是互补滤波算法

四元数仅仅是描述姿态的一种方式，不是算法

.o烏龍茶o灬

这个是飞行姿态解算程序啊····四元数来的···不是滤波

wen123

互补滤波是这样的：陀螺积分角度+=角速度*dt；融合角度=陀螺权值*陀螺积分角度+(1-陀螺权值)*加速度角度；还有一种叫最大梯度法的，就是你发的这段代码

  // 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;




大概是这样的： 陀螺积分角度+=角速度*dt；角度偏差=加速度角度-陀螺积分角度；融合角度=陀螺角度+衰减系数*角度偏差；用来矫正陀螺本身的静态漂移：角度偏差积分+=角度偏差；角速度=角速度+衰减系数*角度偏差积分；这个就可以一定程度上矫正陀螺的零漂。

wen123

其实你这个是带有滤波功能的四元素算法！！！！！！！