求助mpu6050姿态解算时打印输出nan

DJCX

姿态解算算法用的是基于四元数的扩展卡尔曼滤波算法，imu用的是mpu6050，通过串口打印解算后的欧拉角，发现遇到一个问题，就是imu一直动着时，解算的数据是可以正常显示的，但是，把imu静置一小会，然后突然猛地拿起它，这时串口打印出的数据就变nan了，想问一下到底是啥原因
下面是程序：
#include "Qekf.h"
#include "sys.h"
#include "mpu6050.h"
#include "usart.h"

#define G 9.80665f        // m/s^2
float gyro_qekf,accel_qekf;

void Qekf_Init()
{

        switch((MPU_Read_Byte(MPU_GYRO_CFG_REG) >> 3) & 3)
        {
                case 0:
                  gyro_qekf = (2000*2)*(PI/180)/(65536-1); //ÍÓÂÝÒÇÁ¿³Ì2000
                        break;
                case 1:
                  gyro_qekf = (1000*2)*(PI/180)/(65536-1); //ÍÓÂÝÒÇÁ¿³Ì1000
                        break;
                case 2:
                  gyro_qekf = (500*2)*(PI/180)/(65536-1); //ÍÓÂÝÒÇÁ¿³Ì500
                        break;
                case 3:
                  gyro_qekf = (250*2)*(PI/180)/(65536-1); //ÍÓÂÝÒÇÁ¿³Ì250£¨Ñ¡¶¨£©
                        break;
        }//ÓÉԭʼÊý¾Ýת»»ÎªÊµ¼ÊÊý¾Ý£¬µ¥Î»rad/s
        switch((MPU_Read_Byte(MPU_ACCEL_CFG_REG) >> 3) & 3)
        {
                case 0:
                  accel_qekf = G*2*2/(65536-1); //¼ÓËٶȼÆÁ¿³Ì+-2g,mpu6050µÄÊý¾Ý¼Ä´æÆ÷ÊÇ16λ£¨Ñ¡¶¨£©
                        break;
                case 1:
                        accel_qekf = G*2*4/(65536-1); //¼ÓËٶȼÆÁ¿³Ì+-4g,mpu6050µÄÊý¾Ý¼Ä´æÆ÷ÊÇ16λ
                        break;
                case 2:
                        accel_qekf = G*2*8/(65536-1); //¼ÓËٶȼÆÁ¿³Ì+-8g,mpu6050µÄÊý¾Ý¼Ä´æÆ÷ÊÇ16λ
                        break;
                case 3:
                        accel_qekf = G*2*16/(65536-1); //¼ÓËٶȼÆÁ¿³Ì+-16g,mpu6050µÄÊý¾Ý¼Ä´æÆ÷ÊÇ16λ
                        break;
        }//ÓÉԭʼÊý¾Ýת»»ÎªÊµ¼ÊÊý¾Ý£¬µ¥Î»m/s^2
}



//²ÉÑùÖÜÆÚ                                                                                                                                                                                                                                                                                                       
static float32_t dt=0.0025;//2.5ms

//Å·À­½Ç
float pitch_qekf,roll_qekf;

//¹ý³ÌЭ·½²î¾ØÕó
#define Qs   1e-2
static arm_matrix_instance_f32 Q_matrix;
static float32_t               Q[16]={Qs ,0  ,0  ,0  ,
                                      0  ,Qs ,0  ,0  ,
                                      0  ,0  ,Qs ,0  ,
                                      0  ,0  ,0  ,Qs };

//¹Û²âЭ·½²î¾ØÕó
#define Rs_a 3*1e-2
static arm_matrix_instance_f32 R_matrix;
static float32_t               R[9]= {Rs_a  ,0      ,0    ,
                                      0     ,Rs_a   ,0    ,
                                      0     ,0      ,Rs_a };

//ËĽ׵¥Î»¾ØÕó
static arm_matrix_instance_f32 I_matrix;
static float32_t               I[16]={1  ,0  ,0  ,0  ,
                                      0  ,1  ,0  ,0  ,
                                      0  ,0  ,1  ,0  ,
                                      0  ,0  ,0  ,1  };

//ËÄÔªÊý
static float32_t               q[4]={1,0,0,0},q_p[4]={1,0,0,0},q_e[4]={0,0,0,0};
static arm_matrix_instance_f32 q_matrix      ,q_p_matrix      ,q_e_matrix;

//״̬·½³ÌÑſ˱ȾØÕó¼°ÆäתÖÃ
static float32_t               F[16],   F_t[16];
static arm_matrix_instance_f32 F_matrix,F_t_matrix;

//¹Û²âÖµÏà¹ØÏòÁ¿
static float32_t               Z[3],    Z_p[3],    Z_e[3];
static arm_matrix_instance_f32 Z_matrix,Z_p_matrix,Z_e_matrix;

//¹Û²â·½³ÌÑſ˱ȾØÕó¼°ÆäתÖÃ
static float32_t               H[12],   H_t[12];
static arm_matrix_instance_f32 H_matrix,H_t_matrix;

//״̬Э·½²îÏà¹Ø¾ØÕó
#define Ps 0
static float32_t               P_p[16],   P_temp1[16],   P_temp2[16],  P[16]={Ps  ,0   ,0   ,0  ,
                                                                              0   ,Ps  ,0   ,0  ,
                                                                              0   ,0   ,Ps  ,0  ,
                                                                              0   ,0   ,0   ,Ps  };
static arm_matrix_instance_f32 P_matrix,P_p_matrix,P_temp1_matrix,P_temp2_matrix;

//À©Õ¹¿¨¶ûÂüÔöÒæÏà¹Ø¾ØÕó
static float32_t               K[12],   K_temp1[12],   K_temp2[12],   K_temp3[9];
static arm_matrix_instance_f32 K_matrix,K_temp1_matrix,K_temp2_matrix,K_temp3_matrix;                                                                                                                                                                                                                                                                                                                       
                                                                                                                                                                                                                                                                                                                       

//IMU¸üÐÂ
void imu_update(float gx, float gy, float gz, float ax, float ay, float az)
{
          float norm1,norm2,norm3;
          gx = gx*gyro_qekf;
                gy = gy*gyro_qekf;
          gz = gz*gyro_qekf;
                ax = ax*accel_qekf;
                ay = ay*accel_qekf;
                az = az*accel_qekf;
          //IMU³õʼ»¯
  
            //¹ý³ÌÔëÉùЭ·½²î¾ØÕó
      Q_matrix.numRows=4;
            Q_matrix.numCols=4;
                        Q_matrix.pData  =Q;
               
                        //¹Û²âÔëÉùЭ·½²î¾ØÕó
                        R_matrix.numRows=3;
                        R_matrix.numCols=3;
                        R_matrix.pData  =R;
               
                        //ËĽ׵¥Î»¾ØÕó
                        I_matrix.numRows=4;
                        I_matrix.numCols=4;
                        I_matrix.pData  =I;
               
                        //ÉÏÒ»´ÎºóÑéµÄËÄÔªÊý
                        q_matrix.numRows=4;                    
                        q_matrix.numCols=1;                    
                        q_matrix.pData  =q;
                       
                  //ÏÈÑé¹À¼ÆµÄËÄÔªÊý
                        q_p_matrix.numRows=4;
                  q_p_matrix.numCols=1;
                  q_p_matrix.pData  =q_p;
                       
                        //ÁÙʱµÄËÄÔªÊý£ºq = q_p + q_e
                        q_e_matrix.numRows=4;
                        q_e_matrix.numCols=1;
                        q_e_matrix.pData  =q_e;
                                               
                        //״̬תÒƾØÕó
                        F_matrix.numRows=4;        
                        F_matrix.numCols=4;        
                        F_matrix.pData  =F;  
                       
                  //״̬תÒƾØÕóµÄתÖÃ
                        F_t_matrix.numRows=4;
                  F_t_matrix.numCols=4;
                        F_t_matrix.pData  =F_t;
               
                        //¹Û²âµÄÏòÁ¿
                        Z_matrix.numRows=3;                  
                        Z_matrix.numCols=1;                  
                        Z_matrix.pData  =Z;   
               
                  //¹Û²âµÄÏòÁ¿ÔÚÔØÌå×ø±êϵϵķÖÁ¿
                  Z_p_matrix.numRows=3;
                        Z_p_matrix.numCols=1;
                        Z_p_matrix.pData  =Z_p;
                       
                        //ÁÙʱµÄ¹Û²âÏòÁ¿£ºZ_e = Z - Z_p
                  Z_e_matrix.numRows=3;
                        Z_e_matrix.numCols=1;
                        Z_e_matrix.pData  =Z_e;
                       
                        //Ñſ˱ȾØÕó
                        H_matrix.numRows=3;        
                        H_matrix.numCols=4;        
                        H_matrix.pData  =H;
               
                  //Ñſ˱ȾØÕóµÄתÖÃ
                  H_t_matrix.numRows=4;
                        H_t_matrix.numCols=3;
                        H_t_matrix.pData  =H_t;
                       
                        //ϵͳЭ·½²î¾ØÕó
                        P_matrix.numRows=4;                  
                        P_matrix.numCols=4;                  
                        P_matrix.pData  =P;                  

      //ÏÈÑé¹À¼ÆµÄЭ·½²î¾ØÕó:P_p = P_2 + Q
      P_p_matrix.numRows=4;  
                        P_p_matrix.numCols=4;  
                        P_p_matrix.pData  =P_p;
                       
                        //ÁÙʱµÄ¾ØÕó£ºÐÞÕý P_1 = K * H
                        P_temp1_matrix.numRows=4;      
                        P_temp1_matrix.numCols=4;      
                        P_temp1_matrix.pData  =P_temp1;
                       
                        //ÁÙʱµÄ¾ØÕó£ºÐÞÕý P_2 = I - P_1
                        P_temp2_matrix.numRows=4;
                        P_temp2_matrix.numCols=4;
                        P_temp2_matrix.pData  =P_temp2;
                       
                        //À©Õ¹¿¨¶ûÂüÔöÒæ:K = K_3 + R                                          
                        K_matrix.numRows=4;           
                        K_matrix.numCols=3;           
                        K_matrix.pData  =K;   
      
                        //ÁÙʱµÄÔöÒ棺K_1 = P_p * H_t
      K_temp1_matrix.numRows=4;      
      K_temp1_matrix.numCols=3;      
      K_temp1_matrix.pData  =K_temp1;

      //ÁÙʱµÄÔöÒ棺K_2 = H * P_p
      K_temp2_matrix.numRows=3;      
      K_temp2_matrix.numCols=4;      
      K_temp2_matrix.pData  =K_temp2;
      
                        //ÁÙʱµÄÔöÒ棺K_3 = K_2 * H_t
      K_temp3_matrix.numRows=3;   
      K_temp3_matrix.numCols=3;   
      K_temp3_matrix.pData  =K_temp3;
               
                //ËÄÔªÊý¸üÐÂÑſ˱ȾØÕó¼°ÆäתÖþØÕó¼ÆËã
                F[0] = 1.0f       ;F[1] =-gx*dt*0.5f;F[2] =-gy*dt*0.5f;F[3] =-gz*dt*0.5f;//ÇóF
                F[4] =gx*dt*0.5f;F[5] =  1.0f        ;F[6] = gz*dt*0.5f;F[7] =-gy*dt*0.5f;
                F[8] =gy*dt*0.5f;F[9] =-gz*dt*0.5f;F[10]= 1.0f         ;F[11]= gx*dt*0.5f;
                F[12]=gz*dt*0.5f;F[13]= gy*dt*0.5f;F[14]=-gx*dt*0.5f;F[15]= 1.0f         ;
    //ÇóFµÄתÖÃF_t
                arm_mat_trans_f32(&F_matrix,&F_t_matrix);
                //Э·½²î¾ØÕóÔ¤²â
                arm_mat_mult_f32(&F_matrix,&P_matrix,&P_temp1_matrix);  
    arm_mat_mult_f32(&P_temp1_matrix,&F_t_matrix,&P_temp2_matrix);
          arm_mat_add_f32(&P_temp2_matrix,&Q_matrix,&P_p_matrix);
                //ËÄÔªÊýÔ¤²â
                arm_mat_mult_f32(&F_matrix,&q_matrix,&q_p_matrix);//Ô¤²âq_p
                //Ô¤²âÁ¿Ñſ˱ȾØÕó¼°ÆäתÖþØÕó¼ÆËã
                H[0 ]=-2*q_p[2]; H[ 1]= 2*q_p[3]; H[ 2]=-2*q_p[0];  H[ 3]= 2*q_p[1];//ÇóH
                H[4 ]= 2*q_p[1]; H[ 5]= 2*q_p[0]; H[ 6]= 2*q_p[3];  H[ 7]= 2*q_p[2];
                H[8 ]= 2*q_p[0]; H[ 9]=-2*q_p[1]; H[10]=-2*q_p[2];  H[11]= 2*q_p[3];
    arm_mat_mult_f32(&H_matrix,&P_p_matrix,&K_temp2_matrix);
                arm_mat_trans_f32(&H_matrix,&H_t_matrix);//ÇóHµÄתÖÃH_t
                arm_mat_mult_f32(&K_temp2_matrix,&H_t_matrix,&K_temp3_matrix);
                arm_mat_add_f32(&K_temp3_matrix,&R_matrix,&K_temp3_matrix);
                arm_mat_inverse_f32(&K_temp3_matrix,&K_temp3_matrix);
                arm_mat_mult_f32(&P_p_matrix,&H_t_matrix,&K_temp1_matrix);
                arm_mat_mult_f32(&K_temp1_matrix,&K_temp3_matrix,&K_matrix);
               
                //¼ÓËٶȹ۲âÁ¿µ¥Î»»¯
                arm_sqrt_f32(ax*ax+ay*ay+az*az,&norm1);
                Z[0]=ax/norm1;
                Z[1]=ay/norm1;
                Z[2]=az/norm1;
                //¼ÓËÙ¶ÈÏòÁ¿ÏòÁ¿Ô¤²â
                Z_p[0]=2*(q_p[1]*q_p[3] - q_p[0]*q_p[2]);                        
                Z_p[1]=2*(q_p[0]*q_p[1] + q_p[2]*q_p[3]);       
                Z_p[2]=(q_p[0]*q_p[0] - q_p[1]*q_p[1] - q_p[2]*q_p[2] + q_p[3]*q_p[3]);
                arm_sqrt_f32(Z_p[0]*Z_p[0]+Z_p[1]*Z_p[1]+Z_p[2]*Z_p[2],&norm2);      //µ¥Î»»¯
                Z_p[0]/=norm2;                                                                        
                Z_p[1]/=norm2;                                                                        
                Z_p[2]/=norm2;
                //ÐÞÕýq
                arm_mat_sub_f32(&Z_matrix,&Z_p_matrix,&Z_e_matrix);
                arm_mat_mult_f32(&K_matrix,&Z_e_matrix,&q_e_matrix);
                q_e[3]=0;
                arm_mat_add_f32(&q_p_matrix,&q_e_matrix,&q_matrix);
                //ÐÞÕýP
                arm_mat_mult_f32(&K_matrix,&H_matrix,&P_temp1_matrix);
                arm_mat_sub_f32(&I_matrix,&P_temp1_matrix,&P_temp2_matrix);
                arm_mat_mult_f32(&P_temp2_matrix,&P_p_matrix,&P_matrix);
                //¹éÒ»»¯ËÄÔªÊý
                arm_sqrt_f32(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3],&norm3);
                q[0]/=norm3;
                q[1]/=norm3;
                q[2]/=norm3;
                q[3]/=norm3;
                pitch_qekf=-asin(2.f * (q[1]*q[3]-q[0]*q[2])) * 57.3f;
                roll_qekf =atan2(2.f * q[2]*q[3] + 2.f * q[0]*q[1], q[0]*q[0]-q[1]*q[1]-q[2]*q[2]+q[3]*q[3]) * 57.3f;       
}
下面是main.c：
#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "usart.h"
#include "mpu6050.h"   
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "Qekf.h"


int main(void)
{       
       
        float pitch,roll,yaw;                 //Å·À­½Ç
        short gyro_x_adc, gyro_y_adc, gyro_z_adc;
        short accel_x_adc, accel_y_adc, accel_z_adc; //ÍÓÂÝÒÇԭʼÊý¾Ý
       
        NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //ÉèÖÃNVICÖжϷÖ×é2:2λÇÀÕ¼ÓÅÏȼ¶£¬2λÏìÓ¦ÓÅÏȼ¶
        uart_init(115200); //´®¿Ú³õʼ»¯Îª9600
        delay_init(); //ÑÓʱ³õʼ»¯
        MPU_Init();        //³õʼ»¯MPU6050
        Qekf_Init();
        while(mpu_dmp_init())
        {
                printf("11");
                delay_ms(200);
        }
        while(1)
        {
//           if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
//           {
              MPU_Get_Accelerometer(&accel_x_adc,&accel_y_adc,&accel_z_adc); //µÃµ½¼ÓËٶȼÆԭʼÊý¾Ý
              MPU_Get_Gyroscope(&gyro_x_adc,&gyro_y_adc,&gyro_z_adc);        //µÃµ½ÍÓÂÝÒÇԭʼÊý¾Ý
                    imu_update(gyro_x_adc,gyro_y_adc,gyro_z_adc,accel_x_adc,accel_y_adc,accel_z_adc); // qekfËã·¨
//                                printf("pitch£º%.2f  roll£º%.2f\r\n", pitch,roll);
              printf("pitch_qekf£º%.2f  roll_qekf£º%.2f\r\r\r\n", pitch_qekf,roll_qekf);
             
//           }
        }        
}

wye11083

溢出了吧。

DJCX

wye11083 发表于 2022-9-22 11:57
溢出了吧。
(引用自2楼)

可以再多说一下嘛，困扰我好几天了

三年模拟

楼主的问题可以这样转述，一个人中午吃饭吃的好好的，突然暴毙了，问坛友原因？你不描述下吃的啥？怎么吃的？

孤独飞行

不是告诉你了么“nan”好难

wye11083

DJCX 发表于 2022-9-22 15:32
可以再多说一下嘛，困扰我好几天了
(引用自3楼)

/0，asin/acos/atan一个非定义阈的数，etc。。你看看你里面一堆norm有没有可能变成0。毕竟你突然拿起来，z轴饱和，其它轴数值可能很小。要不然就是数值溢出。

Andrewz

传感器卡死了.重启一下.