slow convergence and yaw drift In 9DOF Kalman Fusion

[jiaoqing]

The euler angle performs slowly to convergence  when I rotate the device and then put it  stay stationary, both yaw、pitch and roll. What's more,The yaw drift is obvious when the device is stationary.

How can I adjust the covariance of  magnetic disturbance and linear acceleration ,increase or decrease?

FQWA_9DOF_GBY_KALMAN   1E-4F

FQWD_9DOF_GBY_KALMAN   0.5F

Is magnetic disturbance time constants concerned about the problem?

FCD_9DOF_GBY_KALMAN

Thanks for help!

[michaelestanley]

Jiao,

Which version of the library are you using?  The parameter names you provided don't look right.

In Version 5.00, the 9-axis Kalman parameters are:

// gyro sensor noise covariance units deg^2

#define FQVY_9DOF_GBY_KALMAN            1E4F

// gyro offset random walk units (deg/s)^2

#define FQWB_9DOF_GBY_KALMAN            5E-1F

// geomagnetic inclination angle random walk units deg/s

#define FQWDLT_9DOF_GBY_KALMAN          2E1F

// minimum magnetometer and magnetic disturbance

#define FQVBQD_MIN_9DOF_GBY_KALMAN      7E0F

// minimum and maximum gyro offset in deg/s

#define FGYRO_OFFSET_MIN_9DOF_GBY_KALMAN        -5.0F

#define FGYRO_OFFSET_MAX_9DOF_GBY_KALMAN        5.0F

Generally, you tighten mag parameters and loosen gyro parameters to put more emphasis on the magnetic samples.

BUT - as I've noted in other posts, I generally do not expect to see a lot of drift on 9-axis.  Maybe a few degrees.  Are you sure you are not looking at 6-axis results?

Mike

[jiaoqing]

Hi,Michael：

     Thanks for help.I think I use the early version before version 5.00,and the main problem is the  slow speed of convergence especially when I put the device down after moving it violently.The whole parameter as follows:

// COMPUTE_9DOF_GBY_KALMAN constants

// *********************************************************************************

// kalman filter noise variances

#define FQVA_9DOF_GBY_KALMAN 2E-6F // accelerometer noise g^2 so 1.4mg RMS

#define FQVM_9DOF_GBY_KALMAN 0.1F // magnetometer noise uT^2

#define FQVG_9DOF_GBY_KALMAN 0.3F // gyro noise (deg/s)^2

#define FQWB_9DOF_GBY_KALMAN 1E-9F // gyro offset drift (deg/s)^2: 1E-9 implies 0.09deg/s max at 50Hz

#define FQWA_9DOF_GBY_KALMAN 1E-4F // linear acceleration drift g^2 (increase slows convergence to g but reduces sensitivity to shake)

#define FQWD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance drift uT^2 (increase slows convergence to B but reduces sensitivity to magnet)

:::::::::::::

// linear acceleration and magnetic disturbance time constants

#define FCA_9DOF_GBY_KALMAN 0.5F // linear acceleration decay factor

#define FCD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance decay factor

// maximum geomagnetic inclination angle tracked by Kalman filter

#define SINDELTAMAX 0.9063078F // sin of max +ve geomagnetic inclination angle: here 65.0 deg

#define COSDELTAMAX 0.4226183F // cos of max +ve geomagnetic inclination angle: here 65.0 deg

------------------------------------------------------------------------

the measurement noise is calculated as follows,when I increase the mag covariance parameter FQWD_9DOF_GBY_KALMAN ,the measurement noise also increase, does it effective to put more emphasis to magnetic samples?

	pthisSV->fQvMM = FQVM_9DOF_GBY_KALMAN + FQWD_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * pthisSV->fdeltatsq * DEFAULTB * DEFAULTB * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);

Should I try the library of version 5.00 for better performance?

Thanks!

Jiao.

[michaelestanley]

Actually, if you increase the covariance numbers on the mag, it will emphasize the gyro more.  Basically, you are telling the algorithm that the magnetometer is less accurate when you increase it's covariances.

You did not mention which MCU you are using.  If it happens to be K64F or K22F, then I highly recommend you checkout VERSION 7.00 - released just this morning!  Go to kex.nxp.com, Select "Build an SDK", and then create a new configuration based on one of these two boards.   Make sure to check "ISSDK' and "FreeRTOS".