Changes for hamilton quaternion convention. #34

jgoppert · 2016-12-10T19:54:46Z

It is about time we got this switched over. Can we get the necessary changes in to EKF2 @priseborough . Closes #29.

The changes to the unit test should show you all that will need to be changed in EKF2.

coveralls · 2016-12-10T19:55:37Z

Coverage remained the same at 100.0% when pulling a6bd1ee on hamilton into 74a120f on master.

coveralls · 2016-12-10T19:58:36Z

Coverage remained the same at 100.0% when pulling a9cedcb on hamilton into 74a120f on master.

priseborough · 2016-12-10T21:48:30Z

@jgoppert I want to keep the ecl EKF able to run as a standalone library without reliance on other PX4 libraries. I will have a look at the changes to conventions implemented in this PR and see how much work is involved to do this for all EKF operations.

jgoppert · 2016-12-11T01:23:58Z

OK, my goal would be to have ecl using matrix for math like the rest of the px4 code base to reduce the firmware size and make sure we have independent unit testing for the math.

Let me know if there is anything I can do on my end to make it easier for you to use or more portable.

coveralls · 2016-12-11T01:27:20Z

Coverage remained the same at 100.0% when pulling a9cedcb on hamilton into 74a120f on master.

MaEtUgR · 2016-12-12T10:15:16Z

The adaption to an existing quaternion convention is very welcome to me as I had problems with this library's quaternion convention in the past. The multiplication is the most important piece of the adaption and I'm really glad to see it change but I hope you are aware that other calculations like for example the dcm conversion also need to be compliant to the convention to have the perfectly predictable behavior.

I'm not sure if this is already the case and just wanted to mention. If you wish I would gladly take a look at all this soon.

jgoppert · 2016-12-13T15:27:57Z

@MaEtUgR I believe we have covered all conversions in the unit tests. For instance if you look at the changes in this diff to unit testing you see the impact of the quaternion multplication changes. Basically, it just inverts the multiplication order. https://github.com/PX4/Matrix/pull/34/files#diff-f27a9c721212dea87f37cd226eb7bd04L324

MaEtUgR

I checked multiplication and rotation matrix conversion with this paper: http://www.iri.upc.edu/people/jsola/JoanSola/objectes/notes/kinematics.pdf

MaEtUgR · 2016-12-13T20:59:15Z

@priseborough as it was already mentioned this should only imply opposite order of all quaternion multiplications...

coveralls · 2016-12-15T13:14:38Z

Coverage remained the same at 100.0% when pulling 1760a9e on hamilton into 47c0a93 on master.

coveralls · 2016-12-17T03:32:10Z

Coverage remained the same at 100.0% when pulling 2069851 on hamilton into 47c0a93 on master.

coveralls · 2016-12-17T19:32:17Z

Coverage remained the same at 100.0% when pulling 5072a18 on hamilton into 47c0a93 on master.

jgoppert · 2016-12-30T00:10:43Z

Now also closes #30.

MaEtUgR

as a suggestion you could also do:

invert() {
  this = this.inversed();
}

to keep this logic in one place

MaEtUgR · 2017-01-08T03:13:36Z

matrix/Quaternion.hpp

-        q(1) *= -1;
-        q(2) *= -1;
-        q(3) *= -1;
+        Type n = q.dot(q);


q.dot(q) gives you the norm squared but you want to devide by the norm here.
I would just use the norm method that we already have here n = q.norm() which would use this correct implementation: https://github.com/PX4/Matrix/blob/master/matrix/Vector.hpp#L73

Should have called the variable nsq to be clear. But for inverse believe it is conjugate over norm sq.

It is over the norm (without square) because the definition of the inverse is:
q * q^-1 = q^-1 * q = q_I where the identity quaternion q_I = [1 0 0 0] has unit length.
You can infer from the quaternion multiplication that |q^-1| = 1/|q|.

The matlab implementation is over norm and you can also look up a definition somwhere else for example https://www.j3d.org/matrix_faq/matrfaq_latest.html for example says:

How do I calculate the inverse of a quaternion? ---------------------------------------------------- This is equivalent to calculating the conjugate of the quaternion, if the quaternion is normalized (or a unit quaternion). In all other cases, the magnitude of the inverse is 1/|q|.

Here is a check using sympy with our quaternion product. You cannot assume unit length here. This works for unit length and non-unit length quaternions: https://github.com/jgoppert/iekf_analysis/blob/master/Quaternion%20Inverse%20Check.ipynb. I'm using non-unit quaternions for the IEFK estimator I'm developing, so it is important that it works for both.

Also, look again here: https://www.mathworks.com/help/aerotbx/ug/quatinv.html

Thanks, I'll have a look at it asap.
The part with not assuming unit length is clear, I only have to recheck the norm squared thing.

I checked, you are right! Sorry, my fault.

MaEtUgR · 2017-01-08T03:18:30Z

matrix/Quaternion.hpp

-        ret(1) = -q(1);
-        ret(2) = -q(2);
-        ret(3) = -q(3);
+        Type n = q.dot(q);


same as last comment.
btw a test to identify this issue would be:

q = [.5 0 0 0] q.inv() == [2 0 0 0]

LorenzMeier · 2017-01-12T01:51:29Z

matrix/Matrix.hpp

@@ -290,8 +290,8 @@ class Matrix

    void print() const
    {
-        char buf[200];
-        write_string(buf, 200);
+        char buf[1000];


That's pretty evil on an embedded system. We need to find a better way than this.

It's only for debug prints but nevertheless I agree. What would be your proposition? Dynamic allocation?

Yeah, I think dynamic allocation would be a good way to control it, make print take a length arg with a default of 200? We could also possibly do it intelligently with field width etc based on the size of matrix.

Ok, but if dynamic then using the fields for the exact size. Otherwise we have two disadavantages.

dagar · 2017-01-12T03:21:57Z

@jgoppert @priseborough we currently have matrix cloned twice in PX4 which is a bit confusing. Could we just use the version that comes in through ecl?

 db4374882bba122cb55370c5ea5efacbb8aa11c1 src/lib/ecl/matrix (v1.0.2-4-gdb43748)
 34fccdd6806fbdf7635a6365d7ef112f3bbaab9a src/lib/matrix (v1.0.2-5-g34fccdd)

jgoppert · 2017-01-12T03:32:56Z

@dagar I'm usually bumping matrix a lot as I work on the estimators. I don't think we want to make IEKF and LPE dependent on ecl, it would slow down my workflow. Currently it seems ekf2 is grabbing the px4 version of matrix anyways because it blows up when I pull in the hamilton quaternion patch. ecl should probably only have a copy of matrix lib for test purposes and use installed version or version in parent repo where possible which it seems to be doing anyways, but it is confusing having the other copy sitting there. I think this happens because we compile ecl as a px4 module and the default matrix lib path comes ahead of the local path in the includes.

Too many things added to this pr!

MaEtUgR · 2017-01-17T10:25:48Z

matrix/Matrix.hpp

@@ -290,8 +290,8 @@ class Matrix

    void print() const
    {
-        char buf[1000];
-        write_string(buf, 1000);
+        char * buf = new char[15*N*M];


Do you really want to spend 15 characters for every number? Seems excessive to me... but maybe you have other applications.

MaEtUgR · 2017-01-17T10:31:14Z

matrix/Matrix.hpp

@@ -499,8 +499,8 @@ bool isEqual(const Matrix<Type, M, N> &x,


    if (!equal) {
-        char buf_x[100];
-        char buf_y[100];
+        char * buf_x = new char[100];


Why is it then not dynamic here? We already generate the char array on the heap for print.
The easiest solution could be something like:

printf("not equal:\n x:\n"); x.print(); printf("\ny:\n"); y.print();

jgoppert · 2017-01-19T05:13:11Z

I just compressed the commits so this is easier to review.

jgoppert · 2017-01-20T10:08:26Z

It seems like coveralls stopped getting new builds. https://coveralls.io/github/PX4/Matrix

jgoppert · 2017-02-03T23:05:10Z

This is cleaned up again to the minimal diff.

MaEtUgR

I appreciate that you narrowed down the diff again to the original topic that I already checked and recommended some time ago.

The lines and issues reviewed are not part of this pr anymore

MaEtUgR · 2017-03-29T11:27:01Z

Is there any concern with merging this or why is it still pending?

coveralls · 2017-04-14T05:19:09Z

Coverage remained the same at 100.0% when pulling e69c33c on hamilton into 471e96f on master.

jgoppert · 2017-04-14T12:00:53Z

@MaEtUgR We would need to get ekf2 converted over to this convention.

jgoppert · 2017-04-24T00:49:46Z

@priseborough Can you help me get this into ekf2 so we can close it. You should just have to modify your derivation script for the new quaternion multiplication definition.

priseborough · 2017-04-24T02:14:08Z

OK, let's set up a call and you can walk me through the changes. I do wish to keep the ecl EKF portable and usable outside the PX4 framework.

jgoppert · 2017-04-24T02:44:22Z

@priseborough great, I will contact you via skype.

MaEtUgR · 2017-04-24T09:33:03Z

Switching multiplications should not be too much of a hassle, let me know if I can help anywhere to speed things up 👍

MaEtUgR · 2017-08-29T20:32:13Z

@jgoppert @priseborough @LorenzMeier Can we take this in now please? I want to push the switch to the useful hamilton convention now. It's pending forever and if we are not switching soon the current way will also be used in a lot more interfaces (ask @julianoes and @mrivi ) and we will have the PX4 quaternion convention in future student text books.

Sorry for making this so urgent but otherwise nothing will ever happen. If noone writes back I'll merge and start converting multiplication to make submodule updates possible...

priseborough · 2017-08-30T11:18:53Z

I will create a PR for the ecl library that uses the revised multiplication order and reference it to this PR.

priseborough · 2017-08-31T09:07:53Z

I've been through the ecl and have a branch with the required changes https://github.com/PX4/ecl/tree/pr-ekfQuatMultOrder

I'm happy for this PR to be merged.

MaEtUgR · 2017-08-31T09:10:08Z

Let's start testing and adapting in PRs for all the clients.

ndepal · 2017-09-25T08:04:34Z

matrix/Quaternion.hpp

+ * M_n = C_nb * M_b * C_nb^(-1) (matrix rotation)
+ *
+ * or similarly
+ * v_b = q_nb^1 * [0;v_n] * q_nb


A bit late to the party, but there is a - missing, should be v_b = q_nb^-1 ....

I agree, there's another ^(-1) missing. Can you quickly review #48 please?

Again late to the party, it's merged. Thanks for spotting 👍

jgoppert closed this Dec 11, 2016

jgoppert reopened this Dec 11, 2016

MaEtUgR mentioned this pull request Dec 13, 2016

Support hamilton quaternion #29

Closed

MaEtUgR approved these changes Dec 13, 2016

View reviewed changes

MaEtUgR previously requested changes Jan 8, 2017

View reviewed changes

LorenzMeier previously requested changes Jan 12, 2017

View reviewed changes

MaEtUgR reviewed Jan 17, 2017

View reviewed changes

jgoppert force-pushed the hamilton branch from f790e78 to a31c845 Compare January 19, 2017 05:11

jgoppert force-pushed the hamilton branch from a31c845 to 3bb8e6b Compare January 20, 2017 06:59

jgoppert force-pushed the hamilton branch from 29e9a3b to a1d9708 Compare February 3, 2017 21:39

jgoppert force-pushed the master branch from f1da300 to 230e847 Compare February 3, 2017 21:45

jgoppert force-pushed the hamilton branch 2 times, most recently from 88e4767 to 2c614c6 Compare February 3, 2017 22:58

jgoppert force-pushed the hamilton branch from 8f3e058 to 6f0768e Compare February 4, 2017 14:57

MaEtUgR approved these changes Mar 27, 2017

View reviewed changes

Switch to Hamilton quaternions and add Cholesky decomposition.

e69c33c

jgoppert force-pushed the hamilton branch from 6f0768e to e69c33c Compare April 14, 2017 05:16

MaEtUgR merged commit e595ebb into master Aug 31, 2017

MaEtUgR deleted the hamilton branch August 31, 2017 09:09

MaEtUgR mentioned this pull request Sep 4, 2017

Hamilton Quaternion Multiplication Order PX4/PX4-Autopilot#7908

Merged

MaEtUgR mentioned this pull request Sep 23, 2017

Quaternion based attitude control PX4/PX4-Autopilot#8003

Merged

6 tasks

ndepal reviewed Sep 25, 2017

View reviewed changes

This was referenced Oct 16, 2017

Matrix Print Buffer and Quaternion Fixes #50

Merged

Quaternion multiplication #46

Closed

MaEtUgR mentioned this pull request Jun 1, 2018

Questions about quaternion computation in attitude control PX4/PX4-Autopilot#9573

Closed

MaEtUgR mentioned this pull request Aug 31, 2019

Fixing and simplifying mavlink odometry handling PX4/PX4-Autopilot#12793

Merged

Changes for hamilton quaternion convention. #34

Changes for hamilton quaternion convention. #34

Conversation

jgoppert commented Dec 10, 2016 • edited Loading

coveralls commented Dec 10, 2016

coveralls commented Dec 10, 2016

priseborough commented Dec 10, 2016

jgoppert commented Dec 11, 2016 • edited Loading

coveralls commented Dec 11, 2016

MaEtUgR commented Dec 12, 2016 • edited Loading

jgoppert commented Dec 13, 2016 • edited Loading

MaEtUgR left a comment

Choose a reason for hiding this comment

MaEtUgR commented Dec 13, 2016

coveralls commented Dec 15, 2016

coveralls commented Dec 17, 2016

coveralls commented Dec 17, 2016

jgoppert commented Dec 30, 2016

MaEtUgR left a comment • 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

jgoppert Jan 9, 2017 • edited Loading

Choose a reason for hiding this comment

MaEtUgR Jan 12, 2017 • edited Loading

Choose a reason for hiding this comment

MaEtUgR Jan 12, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MaEtUgR Jan 12, 2017 • edited Loading

Choose a reason for hiding this comment

jgoppert Jan 12, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dagar commented Jan 12, 2017

jgoppert commented Jan 12, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jgoppert commented Jan 19, 2017

jgoppert commented Jan 20, 2017

jgoppert commented Feb 3, 2017

MaEtUgR left a comment

Choose a reason for hiding this comment

MaEtUgR commented Mar 29, 2017

coveralls commented Apr 14, 2017

jgoppert commented Apr 14, 2017

jgoppert commented Apr 24, 2017

priseborough commented Apr 24, 2017

jgoppert commented Apr 24, 2017

MaEtUgR commented Apr 24, 2017

MaEtUgR commented Aug 29, 2017 • edited Loading

priseborough commented Aug 30, 2017

priseborough commented Aug 31, 2017

MaEtUgR commented Aug 31, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jgoppert commented Dec 10, 2016 •

edited

Loading

jgoppert commented Dec 11, 2016 •

edited

Loading

MaEtUgR commented Dec 12, 2016 •

edited

Loading

jgoppert commented Dec 13, 2016 •

edited

Loading

MaEtUgR left a comment •

edited

Loading

jgoppert Jan 9, 2017 •

edited

Loading

MaEtUgR Jan 12, 2017 •

edited

Loading

MaEtUgR Jan 12, 2017 •

edited

Loading

MaEtUgR Jan 12, 2017 •

edited

Loading

jgoppert Jan 12, 2017 •

edited

Loading

jgoppert commented Jan 12, 2017 •

edited

Loading

MaEtUgR commented Aug 29, 2017 •

edited

Loading