关于pitch角的值解算问题
Y1ZzLu opened this issue · 0 comments
Y1ZzLu commented
在#include "QuaternionEKF.h"中的pitch解算中
QEKF_INS.Pitch = 57.29578f * asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2]));
在陀螺仪接近垂直时
我解算得到的asinf的输入值达到了1.05而不是1，超出了定义asin定义域的范围，请问一下可能时参数的原因嘛
我使用的陀螺仪时icm42688
初始化使用代码IMU_QuaternionEKF_Init(10, 0.001, 10000000, 1, 0.001f,0);
更新使用代码
IMU_QuaternionEKF_Update(gyro_value.x / 180.0 * M_PI, gyro_value.y / 180.0 * M_PI, gyro_value.z / 180.0 * M_PI, accel_value.x * 9.8, accel_value.y * 9.8, accel_value.z * 9.8);
其余armdsp库为手动实现，应该不会有问题
但其中逆矩阵解算为esp-dsp库，如果出现无逆矩阵，会输出全零矩阵