智能车制作

标题: 这段互补滤波的代码你们能看懂么？ [打印本页]

作者: skldjfkjf 时间: 2013-11-30 21:57
标题: 这段互补滤波的代码你们能看懂么？

#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 时间: 2013-11-30 21:57
感觉和论坛里普通的互补滤波不太一样啊

作者: rubick 时间: 2013-11-30 22:23
不就是三维的，多了几个叉乘点乘么

作者: Eternalve 时间: 2013-11-30 23:01
这是四元数算法吧四轴之类的飞控用的多

作者: 为丞而琳 时间: 2013-11-30 23:51
这是四元数算法，不是互补滤波算法

作者: skldjfkjf 时间: 2013-12-1 11:44

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

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

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

作者: skldjfkjf 时间: 2013-12-1 11:49

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

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

作者: .o烏龍茶o灬 时间: 2013-12-1 15:03
这个是飞行姿态解算程序啊····四元数来的···不是滤波

作者: wen123 时间: 2013-12-1 16:44
互补滤波是这样的：陀螺积分角度+=角速度*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 时间: 2013-12-1 16:45
其实你这个是带有滤波功能的四元素算法！！！！！！！

作者: 青青水草 时间: 2013-12-1 17:03

作者: skldjfkjf 时间: 2013-12-1 19:30

wen123 发表于 2013-12-1 16:45
其实你这个是带有滤波功能的四元素算法！！！！！！！

谢谢你的解释
不过四元数仅仅是描述姿态的方式，不是算法
我在另一个帖子里说互补滤波和最大梯度法基本是一样的

顺便问下，你的四轴做的咋样啦？

作者: skldjfkjf 时间: 2013-12-1 19:30

青青水草发表于 2013-12-1 17:03

笑而不语是吧？还是说说你的高见吧？

作者: wen123 时间: 2013-12-1 19:41

skldjfkjf 发表于 2013-12-1 19:30
谢谢你的解释
不过四元数仅仅是描述姿态的方式，不是算法
我在另一个帖子里说互补滤波和最大梯度法基本 ...

加了滤波滞后就是算法了！！大家都叫惯了而已！！，四轴等资金了！！！！

作者: wen123 时间: 2013-12-1 19:44

skldjfkjf 发表于 2013-12-1 19:30
谢谢你的解释
不过四元数仅仅是描述姿态的方式，不是算法
我在另一个帖子里说互补滤波和最大梯度法基本 ...

四轴基本上弄好了！！差钱！！钱没到！

，，还有互补和大梯度法性质是一样的，但是大梯度法可以用来抑制陀螺仪的零偏！！

作者: skldjfkjf 时间: 2013-12-1 22:21

wen123 发表于 2013-12-1 19:44
四轴基本上弄好了！！差钱！！钱没到！，，还有互补和大梯度法性质是一样的，但是大梯度法可 ...

最大梯度法？难道你也在匿名四轴圈里混？

作者: wen123 时间: 2013-12-1 22:41

skldjfkjf 发表于 2013-12-1 22:21
最大梯度法？难道你也在匿名四轴圈里混？

我会说我电子论坛我基本上都逛么？？阿莫啥都有！！！匿名，烈火！等等好多玩飞行器全在里面！！！

作者: skldjfkjf 时间: 2013-12-2 14:58

wen123 发表于 2013-12-1 22:41
我会说我电子论坛我基本上都逛么？？阿莫啥都有！！！匿名，烈火！等等好多玩飞行器全在里面！！！

最大梯度法是那些人自创的概念，我就知道你是在那里混的

作者: skldjfkjf 时间: 2013-12-2 14:59

wen123 发表于 2013-12-1 22:41
我会说我电子论坛我基本上都逛么？？阿莫啥都有！！！匿名，烈火！等等好多玩飞行器全在里面！！！

对了，你是哪个大学的，膜拜下

作者: wen123 时间: 2013-12-2 18:32

skldjfkjf 发表于 2013-12-2 14:58
最大梯度法是那些人自创的概念，我就知道你是在那里混的

你也经常逛么？

作者: wen123 时间: 2013-12-2 18:32

skldjfkjf 发表于 2013-12-2 14:59
对了，你是哪个大学的，膜拜下

膜拜就算了吧！！共同进步！！

作者: skldjfkjf 时间: 2013-12-3 17:05

wen123 发表于 2013-12-2 18:32
你也经常逛么？

还行，现在四轴做的也差不多了

作者: 小哥很天真 时间: 2014-4-3 19:51
#include "IMU.h" X128没有这个头文件怎么办啊？

作者: jiushibuzhidao 时间: 2014-7-30 22:52
大哥耶，你这个四元数是不是直接传从mpu6050读出来的数据就行了啊，不用对数据再做什么变换了吗？

作者: chinahuangyong 时间: 2014-8-2 00:44
标题: RE: 这段互补滤波的代码你们能看懂么？

wen123 发表于 2013-12-1 16:44
互补滤波是这样的：陀螺积分角度+=角速度*dt；融合角度=陀螺权值*陀螺积分角度+(1-陀螺权值)*加速度角度；还 ...

多谢楼主呀！

作者: sunshineabner 时间: 2014-8-2 03:28
表示看不懂

作者: xshenpan 时间: 2014-8-2 18:15
MARK,学习了！

作者: Amber_Tang 时间: 2015-9-13 13:57

wen123 发表于 2013-12-1 16:45
其实你这个是带有滤波功能的四元素算法！！！！！！！

请问这段代码中“imu.h”这个头文件哪里有

欢迎光临智能车制作 (http://dns.znczz.com/)