jimmyxumotor 发表于 2015-11-2 23:15:16

请教一个MEMS计算角度的问题

在论坛上看到计算角度的一段程序，里面有个arctan 的计算，不知道怎么算这个？？


/*角度计算*/
void Angle_Calcu(void)       
{

//范围为2g时，换算关系：16384 LSB/g
//deg = rad*180/3.14

/*利用z轴上的重力加速度计算出，芯片与水平面的夹角*/
                      
           Accel_x=ACCE_Data_X(ACCEL_XOUT_H);//x轴
//        Accel_y=ACCE_Data_Y(ACCEL_YOUT_H);//y轴
        Accel_z=ACCE_Data_Z(ACCEL_ZOUT_H);//z轴

        Gyro_y = GYRO_Data_Y(GYRO_YOUT_H);//GetData合成的原始数据


        if(Accel_x<32764)
        {
                x=Accel_x/16384;
        }
        else
        {
                x=1-(Accel_x-49152)/16384;
        }

//        if(Accel_y<32764)
//        {
//                y=Accel_y/16384;
//        }
//        else
//        {
//                y=1-(Accel_y-49152)/16384;
//        }

        if(Accel_z<32764)
        {
                z=Accel_z/16384;
        }
        else
        {
                z=(Accel_z-49152)/16384;
        }

        Angle_az=(atan(x/z))*180/3.14;

/*角度的正负号*/                                                                                       
        if(Accel_x<32764)
        {
        Angle_az=+Angle_az;
        }
        if(Accel_x>32764)
        {
        Angle_az=-Angle_az;
        }

    /*角速度*/
        if(Gyro_y<32768)//向前运动
        {
        Gyro_y=-(Gyro_y/16.4);//范围为1000deg/s时，换算关系：16.4 LSB/(deg/s)
        }
        if(Gyro_y>32768)//向后运动
        {
        Gyro_y=+(65535-Gyro_y)/16.4;
        }
       
        //Angle_gy = Angle_gy + Gyro_y*0.025;//角速度积分得到倾斜角度.越大积分出来的角度越大       
              
        //-------卡尔曼滤波融合-----------------------

        Kalman_Filter(Angle_az,Gyro_y);       //卡尔曼滤波计算倾角
                                                                                                                          
}

沉默胜过白金 发表于 2015-11-3 08:30:14

楼主，把你卡尔曼滤波的那块贴出来把。

jimmyxumotor 发表于 2015-11-3 09:02:20

void Kalman_Filter(float angle_m,float gyro_m)                        //gyro_m:gyro_measure
{
        angle += (gyro_m-q_bias) * dt;
       
        Pdot = Q_angle - P - P;
        Pdot =- P;
        Pdot =- P;
        Pdot = Q_gyro;
       
        P += Pdot * dt;
        P += Pdot * dt;
        P += Pdot * dt;
        P += Pdot * dt;
       
        angle_err = angle_m - angle;
       
        PCt_0 = C_0 * P;
        PCt_1 = C_0 * P;
       
        E = R_angle + C_0 * PCt_0;
       
        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;
       
        t_0 = PCt_0;
        t_1 = C_0 * P;

        P -= K_0 * t_0;
        P -= K_0 * t_1;
        P -= K_1 * t_0;
        P -= K_1 * t_1;
       
        angle        += K_0 * angle_err;
        q_bias        += K_1 * angle_err;
        angle_dot = gyro_m-q_bias;
}

jimmyxumotor 发表于 2015-11-3 09:03:12

卡尔曼都是网上的看的，还不是太理解，边用边琢磨吧

jimmyxumotor 发表于 2015-11-3 09:03:56

通过加速度速度去计算角度，要用到ATAN，不知道大伙这里是怎么计算的？

jimmyxumotor 发表于 2015-11-3 09:05:28

卡尔曼的输入是角度和角速度，要根据MEMS的数据换算出来

jimmyxumotor 发表于 2015-11-3 09:06:27

这个过程还没有搞清楚啊，有人说用积分，不太理解，请大家指点一下，谢谢！

jimmyxumotor 发表于 2015-11-3 10:07:33

我找到了，里面调用了C的一个MATH.h 的文件，我的IAR是32K的限制版，用不了那个文件。{:cry:}
页: [1]
查看完整版本: 请教一个MEMS计算角度的问题