Adaptative estimation of external acceleration

[j_perezdelarray]

This discussion is related to the 6DOF sensorfusion algorithm only and the potential to include an adaptative estimation of external acceleration.

According to Sensorfusion strategy (AN5023), the accelerometer measurement includes gravity, external acceleration and sensor noise. External acceleration and sensor noise are computed as noise variances and input errors in sensorfusion strategy (indirect kalman). External acceleration variance is defined as Qa,k. The accelerometer sensor model is defined in terms  of the deviation of the accelerometer measurement magnitude from the 1g sphere with noise variances being proportional in terms of a constant k determined  from geometrical arguments to a value of 3 (norm based method?).Lower bounds of the noise estimates are applied for robustness of the model in cases measurements rely solely on gravity value.

In the 9DOF mode, the filter weights the magnetometer & accelerometer depending on external interferences (accelerations / magnetic) but in the 6DOF the filter cannot rely in magnetometer data. If I have well understood the filter working principle (not evident!), smaller weights are given to the accelerometer output with respect to the gyroscope output when there is external acceleration. If the accelerometer output is not near 1g it is determined that there is external acceleration and smaller weights are given to the accelerometer output by increasing the measurement noise variance.

After some reading about indirect kalman filters I bumped onto an interesting approach to handle external acceleration ("Orientation estimation using a quaternion-based indirect kalman filter with adaptative estimation of external acceleration", Young Soo Suh), I wondered if this alternative method could be implemented in sensorfusion by directly estimating external acceleration from the filter residual. The main advantage of the proposed method is that the direction of external acceleration is estimated and used for compensation. For example, suppose there is only external acceleration in the x axis. In the accelerometer norm-based method, smaller weights are given to all 3 axis accelerometer outputs and thus valuable information contained in y and z axis accelerometer output is lost. With an adaptative method, smaller weights are given only to x axis thus y and z outputs are still used in the filter.

This is just another way of the many existing to craft sensor fusion models but I was wondering if, a minor modification of the sensorfusion strategy, could improve the external acceleration stimate.

[j_perezdelarray]

Ok. So if i'm not wrong what sensorfusion is applying is a norm-based algorithm (eq. 11, AN5023) of the acceleration / noise variances with lower bounds to avoid 0 values. This norm-based algorithm is already pretty robust against external acceleration except is equivalent to giving less weights to all 3 axis accelerometer outputs independantly of the direction of the external acceleration: if external acceleration is applied only to x axis, and by giving less weights on y and z axis accelerometer output, some useful information in y and z axis accelerometer output is lost.

According to the paper above (I attach it to avoid searching the web), by using an adaptive weighing algorithm, we can increase acceleration variance in the perturbed axis only "x", and thus y and z axis accelerometer information is not lost.

Assuming that external acceleration variance calculation in sensorfusion is a norm-based algorithm: By taking small K constant values, y axis element of the acceleration variance becomes small and thus y axis accelerometer information is used more compared with a high K case. The price paid is that x axis element of the acceleration variance becomes also small and the filter becomes sensitive to x axis external acceleration.

Looking at the test data from the paper (table III, page 9) it looks apparently that the estimation error in the perturbed axis is reduced without affecting the other axes.

So basically, I would now need a little hand here to implement and test this adaptive accel variance algorithm in sensorfusion. The algorithm is described in section IV, page 4. What it does is an estimation of the external acceleration from the residual accelerometer measurement update. The algorithm is described in equations (34) and (35), page 5. I attach the paper to avoid web searching.

Any ideas on how this could be implemented? I wouldn't mind giving it a try.

[j_perezdelarray]

Is the constant K with value 3 related to "fQvGQa = 3.0F * ftmp * ftmp;" ?

[michaelestanley]

Javier,

Can you give me more context for which constant K?  I assume you are NOT talking about the Kalman gain matrix K.

As to the "3" term in the expression above, I believe it is just a fudge factor that my co-developer found worked well.

Mike

[j_perezdelarray]

I'm talking about constant K in equations (10) and (11) in section 2.2 of document AN5023. It refers to the sum of acceleration and accelerometer noise variances, defined as 3 times (constant K)  the square of the difference of the magnitude of the accelerometer measurement from the 1 g sphere

[michaelestanley]

Yes, that's it.

[michaelestanley]

You've been doing your homework!

You're right in that there are lots of ways to adjust the varying trade-offs that go into inertial sensor fusion.  We actually started out with a more complex Kalman filter in the early days, and gradually threw out terms that didn't offer sufficient improvements as a function of memory and power.  I suspect the adjustment you mention might not have visible effects on the performance of the filter.  But by all means, if you've got time to spare, try it out and let us know the results.

Good luck!

MIke

[j_perezdelarray]

Indeed! Unfortunately (or not) I like to get to the very bottom of things...and this topic is fascinating.

I'm not sure if we will have the time (in the short term) to try everything out but we will for sure in the mid term. I'm sure current architecture of the filter is very optimized. This is why we first have to tweak some of the filter parameters so as to evaluate any potential improvements (even if out of the box configuration has worked fine). We will first try:

1) Fusion rate

2) Sensor noise levels

3) Software and hardware sampling rates

4) Sensor variances

5) Computed noise variances

I'll let you know the behavior of these tests in our application.