EEPW论坛

#include <CurieIMU.h>
 
int ax, ay, az;
int gx, gy, gz;

#define LED_PIN 13
 

 
void setup() {
  CurieIMU.begin();

   Serial.begin(9600);
   
    Serial.print("Starting Gyroscope calibration...");
    CurieIMU.autoCalibrateGyroOffset();
    Serial.println(" Done");
    Serial.print("Starting Acceleration calibration...");
    CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
    Serial.println(" Done");

    Serial.println("Internal sensor offsets AFTER calibration...");
    Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
    Serial.println("");
    
}

double total_angle=0;

/* 通过卡尔曼滤波得到的最终角度 */
float Angle=0.0; 
 
/*由角速度计算的倾斜角度 */
float Angle_gy=0.0; 
 
float Q_angle=0.9;  
float Q_gyro=0.001;
float R_angle=0.5;
float dt=0.01;        /* dt为kalman滤波器采样时间; */
char  C_0 = 1;
float Q_bias, Angle_err;
float PCt_0=0.0, PCt_1=0.0, E=0.0;
float K_0=0.0, K_1=0.0, t_0=0.0, t_1=0.0;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
 
/* 卡尔曼滤波函数,具体实现可以参考网上资料,也可以使用其它滤波算法 */
void Kalman_Filter(float Accel,float Gyro)                
{
        Angle+=(Gyro - Q_bias) * dt; 
 
        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; 
 
        Pdot[1]=- PP[1][1];
        Pdot[2]=- PP[1][1];
        Pdot[3]=Q_gyro;
         
        PP[0][0] += Pdot[0] * dt;   
        PP[0][1] += Pdot[1] * dt;   
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;
                 
        Angle_err = Accel - Angle;
         
        PCt_0 = C_0 * PP[0][0];
        PCt_1 = C_0 * PP[1][0];
         
        E = R_angle + C_0 * PCt_0;
         
        if(E!=0)
        {
          K_0 = PCt_0 / E;
          K_1 = PCt_1 / E;
        }
         
        t_0 = PCt_0;
        t_1 = C_0 * PP[0][1];
 
        PP[0][0] -= K_0 * t_0;
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;
                 
        Angle        += K_0 * Angle_err;
        Q_bias        += K_1 * Angle_err;
}
 
void loop() {
    // read raw accel/gyro measurements from device
 
    double ax_angle=0.0;
    double gx_angle=0.0;
    unsigned long time=0;
    unsigned long mictime=0;
    static unsigned long pretime=0;
    float gyro=0.0;
    if(pretime==0)
    {
        pretime=millis();
        return;
    }
    mictime=millis();
 
   CurieIMU.readMotionSensor(ay, ax, az, gy, gx, gz);
 
/* 加速度量程范围设置2g 16384 LSB/g
* 计算公式：
* 前边已经推导过这里再列出来一次
* x是小车倾斜的角度,y是加速度计读出的值
* sinx = 0.92*3.14*x/180 = y/16384
* x=180*y/(0.92*3.14*16384)=
*/
     
    //ax -= AX_ZERO;
ax_angle=ax/262;
     
/* 陀螺仪量程范围设置250 131 LSB//s 
* 陀螺仪角度计算公式:
* 小车倾斜角度是gx_angle,陀螺仪读数是y,时间是dt
* gx_angle +=(y/(131*1000))*dt 
*/
    //gy -= GX_ZERO;
    time=mictime-pretime;
 
    gyro=gy/131.0;
    gx_angle=gyro*time;
    gx_angle=gx_angle/1000.0;
    total_angle-=gx_angle;
     
    dt=time/1000.0;
    Kalman_Filter(ax_angle,gyro);
 
    Serial.print(ax_angle); Serial.print(",");
    Serial.print(total_angle); Serial.print(",");
    Serial.println(Angle);
 
    pretime=mictime;
}

把MPU6050的卡尔曼滤波程序，改成了用Arduino/Genuion 101的Curie IMU
看了一下串口数据
滤波后的数据果然要好了好多

CurieIMU.readMotionSensor(ay, ax, az, gy, gx, gz);

倾角数据有了
卡尔曼滤波加上了
可是小车何时能站起来呢

请容许我哭一会去

自己给自己打气

加油，一定会站起来的

可上九天揽月，可下五洋捉鳖

为何这么熟悉，貌似不是原创呀

总之，世上无难事，只要肯登攀！

继续学习角度的获取

重新学习了一下角度的关系

原来我当初没有好好学习，神马几何、三角函数都还给老师了

还好，有万能的互联网，我们可以充分发挥拿来主义精神

首先，我的IMU的安装方向：（Z轴朝下，我找不到图）

很遗憾，很多人都是把X方向朝前，所以如果没搞明白方向，拿人家代码是无法使用的
（即使搞明白也没法用，我的大破车，据说太大了，555）

然后，几个角度的问题：

经常看到，pitch yaw roll，听起来挺高大上的，但是也不知道是啥，继续哭

找到一篇文章：
http://www.123kuai.com/index.php?a=show&c=index&catid=9&id=54

说的太好了, 复制到下边了。

先把坐标复制上来

根据下文的坐标系以及说明，总结如下：

roll, 绕X轴转角
pitch, 绕Y轴转角
yaw, 绕Z轴转角
以上角度遵循右手定则

结合到我的小车IMU里，roll是我们要的角度

按右手定则，前倾为负，后仰为整

~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

要了解飞机姿态，需要首先知道什么是地面坐标系和机体坐标系。
■地面坐标系（earth-surface inertial reference frame）Sg--------Oxgygzg


①在地面上选一点Og
②使xg轴在水平面内并指向某一方向
③zg轴垂直于地面并指向地心
④yg轴在水平面内垂直于xg轴，其指向按右手定则确定

■机体坐标系(Aircraft-body coordinate frame)Sb-------oxyz



①原点O取在飞机质心处，坐标系与飞机固连
②x轴在飞机对称平面内并平行于飞机的设计轴线指向机头
③y轴垂直于飞机对称平面指向机身右方
④z轴在飞机对称平面内，与x轴垂直并指向机身下方
■欧拉角/姿态角（Euler Angle）
 
机体坐标系与地面坐标系的关系是三个Euler角，反应了飞机相对地面的姿态。
俯仰角θ（pitch）：机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。

偏航角ψ（yaw）：
机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。

滚转角Φ（roll）：机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。

根据下文的坐标系以及说明，总结如下：

roll, 绕X轴转角
pitch, 绕Y轴转角
yaw, 绕Z轴转角
以上角度遵循右手定则

结合到我的小车IMU里，roll是我们要的角度

按右手定则，前倾为负，后仰为整

上边一堆内容，有价值的内容如上。