求助mpu6050姿态解算时打印输出nan (amobbs.com 阿莫电子技术论坛) -

DJCX 发表于 2022-9-22 10:36:33

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

本帖最后由 DJCX 于 2022-9-22 16:57 编辑

姿态解算算法用的是基于四元数的扩展卡尔曼滤波算法，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={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= {Rs_a,0    ,0 ,
                                 0 ,Rs_a ,0 ,
                                 0 ,0    ,Rs_a };

//ËÄ½×µ¥Î»¾ØÕó
static arm_matrix_instance_f32 I_matrix;
static float32_t             I={1,0,0,0,
                                 0,1,0,0,
                                 0,0,1,0,
                                 0,0,0,1};

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

//×´Ì¬·½³ÌÑÅ¿Ë±È¾ØÕó¼°Æä×ªÖÃ
static float32_t             F, F_t;
static arm_matrix_instance_f32 F_matrix,F_t_matrix;

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

//¹Û²â·½³ÌÑÅ¿Ë±È¾ØÕó¼°Æä×ªÖÃ
static float32_t             H, H_t;
static arm_matrix_instance_f32 H_matrix,H_t_matrix;

//×´Ì¬Ð·½²îÏà¹Ø¾ØÕó
#define Ps 0
static float32_t             P_p, P_temp1, P_temp2,P={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, K_temp1, K_temp2, K_temp3;
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 = 1.0f    ;F =-gx*dt*0.5f;F =-gy*dt*0.5f;F =-gz*dt*0.5f;//ÇóF
F =gx*dt*0.5f;F =1.0f    ;F = gz*dt*0.5f;F =-gy*dt*0.5f;
F =gy*dt*0.5f;F =-gz*dt*0.5f;F= 1.0f       ;F= gx*dt*0.5f;
F=gz*dt*0.5f;F= gy*dt*0.5f;F=-gx*dt*0.5f;F= 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=-2*q_p; H[ 1]= 2*q_p; H[ 2]=-2*q_p;H[ 3]= 2*q_p;//ÇóH
H= 2*q_p; H[ 5]= 2*q_p; H[ 6]= 2*q_p;H[ 7]= 2*q_p;
H= 2*q_p; H[ 9]=-2*q_p; H=-2*q_p;H= 2*q_p;
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=ax/norm1;
Z=ay/norm1;
Z=az/norm1;
//¼ÓËÙ¶ÈÏòÁ¿ÏòÁ¿Ô¤²â
Z_p=2*(q_p*q_p - q_p*q_p);
Z_p=2*(q_p*q_p + q_p*q_p);
Z_p=(q_p*q_p - q_p*q_p - q_p*q_p + q_p*q_p);
arm_sqrt_f32(Z_p*Z_p+Z_p*Z_p+Z_p*Z_p,&norm2);    //µ¥Î»»¯
Z_p/=norm2;
Z_p/=norm2;
Z_p/=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=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*q+q*q+q*q+q*q,&norm3);
q/=norm3;
q/=norm3;
q/=norm3;
q/=norm3;
pitch_qekf=-asin(2.f * (q*q-q*q)) * 57.3f;
roll_qekf =atan2(2.f * q*q + 2.f * q*q, q*q-q*q-q*q+q*q) * 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£º%.2froll£º%.2f\r\n", pitch,roll);
      printf("pitch_qekf£º%.2froll_qekf£º%.2f\r\r\r\n", pitch_qekf,roll_qekf);

//    }
}
}

wye11083 发表于 2022-9-22 11:57:14

溢出了吧。

DJCX 发表于 2022-9-22 15:32:48

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

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

三年模拟 发表于 2022-9-22 15:56:46

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

孤独飞行 发表于 2022-9-22 16:28:49

不是告诉你了么“nan”好难{:titter:}

wye11083 发表于 2022-9-22 17:46:26

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

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

Andrewz 发表于 2022-9-22 23:39:10

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

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

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