#include "MovingAvarageFilter.h"
//Setup Motion Detect Averages
MovingAvarageFilter accnorm_avg(5);
MovingAvarageFilter accnorm_test_avg(7);
MovingAvarageFilter accnorm_var(7);
MovingAvarageFilter motion_detect_ma(10);
//======================================================================
bool MotionDetect() {
float gyro[3];
float accnorm;
int accnorm_test, accnorm_var_test, omegax, omegay, omegaz, omega_test, motionDetect;
gyro[0] = gx;
gyro[1] = gy;
gyro[2] = gz;
float ax_n = ax/9.807;
float ay_n = ay/9.807;
float az_n = az/9.807;
/*###################################################################
#
# acceleration squared euclidean norm analysis
#
# some test values previously used:
# if((accnorm >=0.96) && (accnorm <= 0.99)){
# <= 0.995
#
################################################################### */
//accnorm = (ax*ax+ay*ay+az*az);
accnorm = (ax_n*ax_n+ay_n*ay_n+az_n*az_n);
if((accnorm >=0.9) && (accnorm <= 1.3)){ //was 1.07 and .9
accnorm_test = 0;
} else {
accnorm_test = 1; }
/** take average of 5 to 10 points **/
float accnormavg = accnorm_avg.process(accnorm);
float accnormtestavg = accnorm_test_avg.process(accnorm_test);
/*####################################################################
#
# squared norm analysis to determine suddenly changes signal
#
#####################################################################*/
//accnorm_var.process(sq(accnorm-accnorm_avg.getAvg()));
// was 0.0005
if(accnorm_var.process(sq(accnorm-accnormavg)) < 0.0005) {
accnorm_var_test = 0;
} else {
accnorm_var_test = 1; }
//Serial.print("ACCEL TEST: ");
//Serial.print(accnorm); Serial.print(", "); Serial.print(accnorm_test);
//Serial.print(", "); Serial.println(accnorm_var_test);
/*###################################################################
#
# angular rate analysis in order to disregard linear acceleration
#
# other test values used: 0, 0.00215, 0.00215
################################################################### */
if ((gyro[0] >=-0.02) && (gyro[0] <= 0.02)) {
omegax = 0;
} else {
omegax = 1; }
if((gyro[1] >= -0.02) && (gyro[1] <= 0.02)) {
omegay = 0;
} else {
omegay = 1; }
//if((gyro[2] >= -0.005) && (gyro[2] <= 0.005)) {
if((gyro[2] >= -0.005) && (gyro[2] <= 0.005)) {
omegaz = 0;
} else {
omegaz = 1; }
if ((omegax+omegay+omegaz) > 0) {
omega_test = 1;
} else {
omega_test = 0; }
//Serial.print("GYRO TEST:"); Serial.print(gyro[2]);Serial.print(", ");
//Serial.print(omegax); Serial.print(", "); Serial.print(omegay);
//Serial.print(", "); Serial.print(omegaz);
//Serial.print(", "); Serial.println(omega_test);
/*
###################################################################
#
# combined movement detector
#
####################################################################
*/
if ((accnormtestavg + omega_test + accnorm_var_test) > 0) {
motionDetect = 1;
} else {
motionDetect = 0; }
/*
##################################################################
*/
//motion_detect_ma.process(motionDetect);
if(motion_detect_ma.process(motionDetect) > 0.5) {
return true;
} else {
return false;
}
}
Hi Tim
Happy Holidays and at the risk of being politically incorrect Merry Christmas. Yeah, I think that the yaw drift when it is stationary is something that is inherent with Kalman filters. If you look at the BNO055 IMU, it doesn't update values unless the IMU is moving. That is probably why.
Mike
omega_test = omegax | omegay | omegaz;
accnormtestavg + omega_test + accnorm_var_test) > 0
omega_test = omegax | omegay | omegaz;
// ...
motionDetect = accnormtestavg | omega_test | accnorm_var_test;
I ran a simple test using Brian's Teensy Backpack (T3.2 plus MPU9250). I aligned to x-axis pointer to close to magnetic North and let the filters run for awhile and then started movement up to about 90degs and then brought it back to North. This way zero degrees should be close for both filters. Going to run another test Brian's compass sketch and do a comparison.
View attachment 15418
UPDATE: I added in the TiltCompass code and did a quick comparison of the yaw. Essential the yaw from the tiltcompass code matches the yaw output from the Mahoney filter. This is when its flat of course.
Filter.update(uBloxData.iTOW,uBloxData.velN,uBloxData.velE,uBloxData.velD,uBloxData.lat*PI/180.0f,uBloxData.lon*PI/180.0f,uBloxData.hMSL,Imu.getGyroY_rads(),-1*Imu.getGyroX_rads(),Imu.getGyroZ_rads(),Imu.getAccelY_mss(),-1*Imu.getAccelX_mss(),Imu.getAccelZ_mss(),Imu.getMagX_uT(),Imu.getMagY_uT(),Imu.getMagZ_uT());
Wait... Conducting IMU static calibration...
Static Calibration Bias Values:
axb: -0.647463
ayb: -0.115094
azb: -4.477447
gxb: 0.000910
gyb: 0.001690
gzb: 0.000004
hxb: -13.498309
hyb: 41.154903
hzb: -26.354893
Wait... Conducting IMU static calibration...
Static Calibration Bias Values:
axb: 0.157945
ayb: -0.456676
azb: -3.711194
gxb: -0.000832
gyb: 0.000367
gzb: 0.000229
hxb: -31.889704
hyb: 10.246424
hzb: -47.167191
So the library expects the IMU input in the NED convention?
Also, is the any available paper/presentation/literature concerning the EKF in this library?
I ask because I am very interest in adding baro as an altitude input to fuse of the GPS and IMU.
Thanks
Bruce