Initial cut on full quaternion IMU conversion #2894

digitalentity · 2018-03-08T12:44:19Z

No description provided.

adrianmiriuta · 2018-03-08T18:14:28Z

why ?
QUATERNION_INIT_ZERO
why ?
quaternionMultiply

digitalentity · 2018-03-08T22:57:16Z

nice adoption !

@adrianmiriuta while it may look similar, this is not an adoption of your work, see my comment here betaflight/betaflight#5182 (comment)

quaternionMultiply

Read the code

digitalentity · 2018-03-09T01:26:09Z

@HaukeRa we're finally making some use of your implementation. Quaternion PID controller will follow.

adrianmiriuta · 2018-03-09T07:05:05Z

@digitalentity

Read the code

I did

More accurate quaternion integration

ledvinap · 2018-03-09T09:59:25Z

src/main/common/quaternion.h

+    t_fp_vector axis;
+    float angle;
+} fpAxisAngle_t;
+


With LTO, it may be better to define these function in .c file and let gcc decide when inlining is worth it ...

LTO will take out optimizations that GCC could do when inlining the functions (i.e. optimizing out multiplications where one operand is known to be zero). Also, in all cases a function call will be more expensive than inlined code at expense of marginal increase of text segment.

Inlining is possible with LTO and (IIRC) actual function may be eliminated when all calls are inlined.
So it is not worse than static inline in '.h'

(Not that sure) Using static inline, static will force one instance per source file. Gcc can still decide that it is not worth inlining, producing multiple copies of identical code ...

Zero-member optimization is interesting question, i'll have to look at it ...

ledvinap · 2018-03-09T10:03:28Z

src/main/common/quaternion.h

+    return sq(q->q0) + sq(q->q1) + sq(q->q2) + sq(q->q3);
+}
+
+static inline fpQuaternion_t * quaternionCrossProduct(fpQuaternion_t * result, const fpQuaternion_t * a, const fpQuaternion_t * b)


As @adrianmiriuta pointed out, this is quaternion multiplication (and vector cross product - cross product is defined only for 3D and 7D)

You're right. This is actually a Hamilton product, but for simplicity we can rename it to quaternionMultiply.

ledvinap · 2018-03-09T10:11:00Z

src/main/common/quaternion.h

+    vectQuat.q2 = vect->V.Y;
+    vectQuat.q3 = vect->V.Z;
+
+    quaternionConjugate(&refConj, ref);


(this is slowest way to rotate vector ... https://geometrictools.com/Documentation/RotationIssues.pdf, p9)

Agreed, I'll optimize it later. Are you suggesting converting to rotation matrix and applying it to a vector?

ledvinap · 2018-03-09T10:11:48Z

src/main/common/quaternion.h

+    return result;
+}
+
+static inline t_fp_vector * rotateVectorByQuaternion(t_fp_vector * result, const t_fp_vector * vect, const fpQuaternion_t * ref)


(maybe quaternionRotateVector ? )

ledvinap · 2018-03-09T10:12:37Z

src/main/common/vector.h

+
+// Floating point 3 vector.
+typedef struct fp_vector {
+    float X;


(normally field names are lower-case)

This is how it is now, will refactor in a separate PR.

ledvinap · 2018-03-09T11:56:54Z

src/main/flight/imu.c

+            t_fp_vector vCoG = { .V = { -cos_approx(courseOverGround), sin_approx(courseOverGround), 0.0f } };
+
+            // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame
+            rotateVectorByQuaternionInv(&vHeadingEF, &vNorth, &orientation);


it's vForward, not north (value is the same)

ledvinap · 2018-03-09T12:02:37Z

src/main/flight/imu.c

+
+                // integral error scaled by Ki
+                vectorScale(&vTmp, &vErr, imuRuntimeConfig.dcm_ki_mag * dt);
+                vectorAdd(&vIntegralMag, &vIntegralMag, &vTmp);


There should probably be only one integral error accumulator.
Two integrators may drift in opposite direction - total error will be small, but error in each integrator large, possibly causing problem.

Also integrator is here to remove gyro drift. Not sure, but it's gain is more related to gyro properties (maximum drift) than to reference source (and its noise)...

ledvinap · 2018-03-09T12:08:30Z

src/main/flight/imu.c

-        ey = (az * rMat[2][0] - ax * rMat[2][2]);
-        ez = (ax * rMat[2][1] - ay * rMat[2][0]);
+        vectorNormalize(&vAcc, accBF);
+        vectorCrossProduct(&vErr, &vAcc, &vEstGravity);


Possible improvement:
Cross product will be small when vectors are close to antiparallel (initial convergence problem). Using dot product &vAcc, &vEstGravity, it is easy to detect large error and update error estimation accordingly (for example normalize vErr when dot(&vAcc, &vEstGravity) < 0

I'll try that

It will usually apply only on initial guess. But it is also metric to detect 'large deviation' that is done explicitly to reset orientation in iNav ...

ledvinap · 2018-03-09T12:14:12Z

src/main/flight/imu.c

+
+    // Calculate quaternion delta:
+    // Theta is a axis/angle rotation. Direction of a vector is axis, magnitude is angle/2.
+    // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero.


The instability is strange - even sin_approx should be reasonably accurate for sin(x)/x
(BF approximation polynomials were designed with this in mind)

The problem is /x part. If our sin(x) was perfect it wouldn't matter, but since our sin(x) is an estimate, with x -> 0 result of sin_approx(x)/x diverges from actual value creating a problem.

Tried it, sin_approx(x)/x correctly converges to 1 and is within 1e-6 absolute error ...
(skipped normalization, valid from -pi to pi )

function y = sin_approx(x) if(1) sinPolyCoef3 =-1.666568107e-1; sinPolyCoef5 = 8.312366210e-3; sinPolyCoef7 = -1.849218155e-4; sinPolyCoef9 = 0; else sinPolyCoef3 = -1.666665710e-1; % Double: -1.666665709650470145824129400050267289858e-1 sinPolyCoef5 = 8.333017292e-3; % Double: 8.333017291562218127986291618761571373087e-3 sinPolyCoef7 = -1.980661520e-4; % Double: -1.980661520135080504411629636078917643846e-4 sinPolyCoef9 = 2.600054768e-6; % Double: 2.600054767890361277123254766503271638682e-6 endif x(x > .5*pi) = pi - x(x > .5*pi); x(x < -.5*pi) = -pi - x(x < -.5*pi); x2 = x .* x; y= x + x .* x2 .* (sinPolyCoef3 + x2 .* (sinPolyCoef5 + x2 .* (sinPolyCoef7 + x2 .* sinPolyCoef9)));

I prefer to be on a safe side here. Besides, I would still need to handle a case of no motion (gyroBF == 0)

Yes, I was just surprised. Approximation functions were designed to behave nicely around these points.

Actually, the approximation is faster and accurate enough. From my experiments even linear approximation is quite OK (when running IMU ar 300Hz and assuming 2k IMU as reference).

It's not about update rate, it's about delta rotation magnitude. At 300Hz IMU if quad is rotating 720deg/s (2.4deg per update). Linear approximation of sin(x) has only 0.03% error at this rotation. After 300 iterations (1 second) it will yield 0.8 deg error.

At 100Hz IMU and 720 deg/s rotation error after 1s would be 2 deg already.

In reality there are also measurement errors, delta time jitter etc. All this adds up to rapidly growing error at slow update rates. If we can take out at least interpolation error - I'd say it's worth the effort

I'm (slowly) looking into this ... IMO balance is necessary - there is no point using math many orders more accurate than input data / drift ...
At least it's useful to have both available ...

ledvinap · 2018-03-09T12:15:31Z

src/main/flight/imu.c

+    // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero.
+    // For near-zero cases we use the first 3 terms of the Taylor series expansion for sin/cos. We check if fourth term is less than machine precision -
+    // then we can safely use the "low angle" approximated version without loss of accuracy.
+    if (thetaMagnitudeSq * thetaMagnitudeSq / 24.0f < 1e-6f) {


you can sqrt both sides ... -> thetaMagnitudeSq < sqrt(24*1e-6)

sqrtf if more expensive than multiplication

But square root of constant is free ...

digitalentity · 2018-03-09T13:15:15Z

@ledvinap please have a look at 765495f

…o have orientation correspond to RPY angles

ledvinap · 2018-03-13T13:31:54Z

@digitalentity : Some error estimate is here: betaflight/betaflight#5182 (comment)

digitalentity · 2018-03-13T13:37:58Z

@ledvinap yep, that's one main reason why INAV is running IMU at gyro rate.

digitalentity · 2018-03-13T14:02:27Z

Finally magnetic declination is applied to compass and RPY angles now accurately represent internal orientation quaternion.

digitalentity · 2018-03-13T14:02:54Z

This should be now ready for testing

ledvinap · 2018-03-13T14:16:15Z

@digitalentity : There is some update rate that is 'good enough', with no measurable influence on IMU estimate (or error well below noise)

digitalentity · 2018-03-13T14:22:09Z

@ledvinap indeed it is, but proper rate is yet to be figured out.

ledvinap · 2018-03-13T14:23:04Z

src/main/flight/imu.c

@@ -277,6 +290,9 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
            // Ignore magnetic inclination
            vMag.z = 0.0f;

+            // Apply magnetic declination correction
+            rotationMatrixRotateVector(&vMag, &vMag, &magDeclinationRMat);


It may be easier (and actually cleaner) to use true north vector ( [cos(declination, sin(declination), 0] (?) ) as reference

@ledvinap you're a genius! It didn't occur to me that I can just figure our true North vector from declination 👍

digitalentity · 2018-03-14T14:18:18Z

Attitude and heading seems correct. Merging

stronnag · 2018-03-14T14:23:19Z

On March 14, 2018 2:18:20 PM UTC, Konstantin Sharlaimov ***@***.***> wrote: Attitude and heading seems correct. Merging -- You are receiving this because you are subscribed to this thread. Reply to this email directly or view it on GitHub: #2894 (comment)

In the field flying it now. No problems.

@ledvinap

* Initial cut on full quaternion/vector IMU conversion * More accurate quaternion integration * Refactor vector struct per @ledvinap suggection * Implement rotation matrix from axis/angle; Refactor mag declination to have orientation correspond to RPY angles * Use magnetic North vector as a reference

digitalentity added Experimental Don't merge labels Mar 8, 2018

digitalentity added this to the 1.9.1 milestone Mar 8, 2018

digitalentity force-pushed the de_quaternion_math branch from 4af9e39 to facc625 Compare March 8, 2018 13:58

ledvinap mentioned this pull request Mar 8, 2018

IMU quaternion (rmat phase out) betaflight/betaflight#5182

Closed

This was referenced Mar 9, 2018

Would like Angle Mode to work up to 90 degree angle #272

Closed

Quaternion-based attitude controller #1077

Closed

Quaternion based level PIDs #734

Closed

Initial cut on full quaternion/vector IMU conversion

ad5fdc0

More accurate quaternion integration

digitalentity force-pushed the de_quaternion_math branch from d480cca to ad5fdc0 Compare March 9, 2018 07:12

ledvinap reviewed Mar 9, 2018

View reviewed changes

Changes per @ledvinap review

765495f

digitalentity added 3 commits March 10, 2018 00:37

Minor optimization

2ab598a

Refactor vector struct per @ledvinap suggection

5d9926d

Implement rotation matrix from axis/angle; Refactor mag declination t…

2fe435e

…o have orientation correspond to RPY angles

Magnetic declination direction bugfix

22d0888

digitalentity added Testing Required and removed Don't merge Experimental labels Mar 13, 2018

ledvinap reviewed Mar 13, 2018

View reviewed changes

Use magnetic North vector as a reference

8561b57

digitalentity removed the Testing Required label Mar 14, 2018

digitalentity merged commit e174e5a into development Mar 14, 2018

digitalentity deleted the de_quaternion_math branch March 14, 2018 14:20

adrianmiriuta mentioned this pull request Mar 28, 2018

IMU quaternion v4 ButterFlight/butterflight#79

Closed

shellixyz mentioned this pull request Apr 11, 2018

Feature Request: "Heading Hold" for Flying Wings #1712

Closed

Initial cut on full quaternion IMU conversion #2894

Initial cut on full quaternion IMU conversion #2894

Conversation

digitalentity commented Mar 8, 2018

adrianmiriuta commented Mar 8, 2018

digitalentity commented Mar 8, 2018 • edited Loading

digitalentity commented Mar 9, 2018

adrianmiriuta commented Mar 9, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

digitalentity Mar 9, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

digitalentity Mar 9, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

digitalentity commented Mar 9, 2018

ledvinap commented Mar 13, 2018

digitalentity commented Mar 13, 2018

digitalentity commented Mar 13, 2018

digitalentity commented Mar 13, 2018

ledvinap commented Mar 13, 2018

digitalentity commented Mar 13, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

digitalentity commented Mar 14, 2018

stronnag commented Mar 14, 2018 via email

digitalentity commented Mar 8, 2018 •

edited

Loading

digitalentity Mar 9, 2018 •

edited

Loading

digitalentity Mar 9, 2018 •

edited

Loading