|
本帖最后由 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[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);
// }
}
}
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
阿莫论坛20周年了!感谢大家的支持与爱护!!
你熬了10碗粥,别人一桶水倒进去,淘走90碗,剩下10碗给你,你看似没亏,其实你那10碗已经没有之前的裹腹了,人家的一桶水换90碗,继续卖。说白了,通货膨胀就是,你的钱是挣来的,他的钱是印来的,掺和在一起,你的钱就贬值了。
|