Dynamic accelerometer calibration II

Picking up from where I got to before, I’m going to follow the path where the accelerometer raw data ellipsoidal space is near enough to spherical that I can forget the rx*_gain values.

The errors this will introduce should be relatively inconsequential: first the gains are only a per-cent or two; secondly, the critical factor here is that the sensors read zero when horizontal (i.e. corrected by offsets), but any minor gain errors will lead to angle and speed errors, but critically, these will still read zero when they should, and the errors will be handled by the PIDs.

On that basis, let’s see where that leads.  Here’s what we have to work with so far

qx = (rx + rxoff) * rxgain
qy = (ry + ryoff) * rygain
qz = (rz + rzoff) * rzgain

qx2 + qy2 + qz2 = 1

ex = 0
ey = 0
ez = 1

|ex|   |cos(pitch),         0,          -sin(pitch)||qx|
|ey| = |         0, cos(roll),           -sin(roll)||qy|
|ez|   |sin(pitch), sin(roll), cos(pitch).cos(roll)||qz|

pitch = atan(qx / qz)
roll = atan(qy / qz)

So without the gain excluded, we have the following – and we have 3 variables fewer to calculate.

qx = (rx' + rx'off)
qy = (ry' + ry'off)
qz = (rz' + rz'off)

Sat on the ground, the only force acting on Phoebe is vertical gravity, so the earth axis values should read (0, 0, 1) g, as should the offseted sensors once matrixed.

ex = 0 = (rx' + rx'off) * cos(pitch) - (rz' + rz'off) * sin(pitch)
ey = 0 = (ry' + ry'off) * cos(roll) - (rz' + rz'off) * sin(roll)
ez = 1 = (rz' + rz'off) * cos(pitch) * cos(roll) + (rx' + rx'off) * sin(pitch) + (ry' + ry'off) * sin(roll)

Replacing pitch and roll with the raw data gives

pitch = atan((rx' + rx'off) / (rz' + rz'off))
roll = atan((ry' + ry'off) / (rz' + rz'off))

That leaves us with only the 3 offset variables, r*off to calculate based upon the raw sensor data, r*.

(rx' + rx'off) * cos(pitch) = (rz' + rz'off) * sin(pitch)
(ry' + ry'off) * cos(roll) = (rz' + rz'off) * sin(roll)
(rz' + rz'off) * cos(pitch) * cos(roll) + (rx' + rx'off) * sin(pitch) + (ry' + ry'off) * sin(roll) = 1

Not the easiest of equations to solve as I don’t think I can simplify the atan’s sine’s and cosine’s, but I’ll have a go.

(rx' + rx'off) * cos(pitch) = (rz' + rz'off) * sin(pitch)

Dividing both sides by cos(pitch) leads to

(rx' + rx'off) = (rz' + rz'off) * tan(pitch)

Substituting for tan(pitch) leads to

(rx' + rx'off) = (rz' + rz'off) * (rx' + rx'off) / (rz' + rz'off)

Thus successfully proving that

 1 = 1

10/10 for effort, 0/10 for results!

2 thoughts on “Dynamic accelerometer calibration II

  1. What you have is:
    (rx’ + rx’off)² + (ry’ + ry’off)² + (rz’ + rz’off)² = 1

    And can be written as:

    
                                          | 2rx'off                         |
                                          | 2ry'off                         |
                                          | 2rz'off                         |
    rx² = | rx  ry  rz  -ry²  -rz²  1 | * | 1                               |
                                          | 1                               |
                                          | 1 - rx'off² - ry'off² - rz'off² |
    

    Since rx, ry, rz are from raw readings, you have the rx² = | rx ry rz -ry² -rz² 1 | part and the other is what you need to found out.

    Considering w as a matrix Nx1 of N rx² inputs, and H a matrix containing the | rx ry rz -ry² -rz² 1 | for each input and X the matrix you need to found out, you will have:

    w = H * X

    Then you can use the least squares to solve the “w = H*X” (b = ax) equation.
    After finding the X matrix the only thing you need to do is convert the results:

    rx’off = X[1] / 2
    ry’off = X[2] / (2 * X[4])
    rz’off = X[3] / (2 * X[5])

    source: http://www.pololu.com/file/download/LSM303DLH-compass-app-note.pdf?file_id=0J434, page 28

    • Gustavo, thank you (I think)!

      I have a lot of relearning to do it seems to use this method for per-flight accelerometer offset calibration, but it’s definitely worth doing! I’m hugely grateful, but at the same time, a little bit cautious my ancient math(s) knowledge isn’t up to this. Still, you don’t know if you don’t try.

      Andy

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.