Madgwick filter - Quaternion Multiplication

I am trying to replicate the Madgwick filter just to learn from it. I am stuck at the multiplication to become the objective function.

This is what I got:

% Let:
q = [q1 q2 q3 q4];
qstar = [q1 -q2 -q3 -q4];  % conjugate of q
a = [0 ax ay az];
g = [0 0 0 1];

% fg(q,a) = qstar ⦻ g ⦻ q - a


Answer:

fg(q,a) =
                                0
           2*q2*q4 - 2*q1*q3 - ax
           2*q1*q2 - ay + 2*q3*q4
   q1^2 - q2^2 - q3^2 + q4^2 - az

This is the answer from the paper:

There are other variables that should not be there. Can someone tell me why this is?

Here is the full code:

syms q1 q2 q3 q4 gx gy gz ax ay az mx my mz bx bz by h hx hy dx dy dz sx sy sz

q = [q1 q2 q3 q4];
cong_q = [q1 -q2 -q3 -q4];

gyro = [0 gx gy gz];
a = [0 ax ay az];
b = [0 bx 0 bz];
d = [0 dx dy dz];
s = [0 sx sy sz];
m = [0 mx my mz];
wg = [0 0 0 1];

% Gyroscope
qdotg = quatmul(q,gyro);

% Function and Jacobian for Accelerometer
fg1 = quatmul(cong_q,wg);
fg = quatmul(fg1,q) - transpose(a);
fg(1) = [];
Jg = jacobian(fg, [q1, q2, q3, q4]);

% Function and Jacobian for Magnetometer
fb = quatmul(cong_q,b);
fb = quatmul(fb,q) - transpose(m);
fb(1) = [];
Jb = jacobian(fb, [q1, q2, q3, q4]);

% Total function
Jg_t_fg = transpose(Jg)*fg;
Jb_t_fb = transpose(Jb)*fb;

ftot = Jg_t_fg + Jb_t_fb;

fnorm = ftot * (1/sqrt(ftot(1)^2+ftot(2)^2+ftot(3)^2+ftot(4)^2));

function quatprod = quatmul(r, k)
% r * k

quatprod1 = r(1)*k(1) - r(2)*k(2) - r(3)*k(3) - r(4)*k(4);
quatprod2 = r(1)*k(2) + r(2)*k(1) + r(3)*k(4) - r(4)*k(3);
quatprod3 = r(1)*k(3) - r(2)*k(4) + r(3)*k(1) + r(4)*k(2);
quatprod4 = r(1)*k(4) + r(2)*k(3) - r(3)*k(2) + r(4)*k(1);

quatprod = [quatprod1;quatprod2;quatprod3;quatprod4];
end

A similar problem occurs also for the other functions.

If anything is missing, let me know and I will add it. Thank you.

Reference: http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf

[Neel Mehta]

I figured it out!

The answer I got is correct:

This is the reason:

So:

Simple maths.

However, I have a new question. How can I implement this rule:

into the function I created?

[James Tursa]

Not sure what you are asking. You want the Symbolic Toolbox to automatically simplify and/or select a preferred expression using the unity constraint? How will it know which one you prefer?

[Neel Mehta]

Hi James, I was trying to figure this one out and I still haven't really managed to do this till now. So I have thought of this: Lets create a function called quatsimp(q) that simplifies quaternions.

function simp = quatsimp(q)

end

In this case, if there is a line with 4 cubed quaternion components for example:

That we can say that, we shall implement the quaternion normalisation rule.

So in this case the function will take the case and convert it to:

and using substitution will give us

I really want to do this symbolic to prove a point.

I hope this makes sense.

Answer 1

[James Tursa]

I guess you could generate the following three expressions symbolically:

original

original + 1 - ()

original + () - 1

and then have some logic to pick the "simplest" from the three outcomes. Maybe "simplest" is simply the shortest one when converted to a char string?

[Neel Mehta]

Hi James, thanks for the input.

Hmm, I think you are on the right track. I created this function where i say, if and and and exist, then do that what you mentioned. Namely:

original

function simp = quatsimp(q,q1,q2,q3,q4)
j = 1 - q1^2 - q2^2 - q3^2 - q4^2;
q = expand(q);

for i = 1:length(q)
    if (has(q(i), q1^2) && has(q(i), q2^2) && has(q(i), q3^2) && has(q(i), q4^2))
        q(i) = q(i) + j;       
    else
        q(i) = q(i);
    end

simp = q;
end
end

The issue is that, if there are other symbolic components that are multiplied with the quaternion squared, then this wont work. e.g.

bx*q1^2 - 2*bz*q1*q3 + bx*q2^2 + 2*bz*q2*q4 - bx*q3^2 - bx*q4^2 - mx
              2*bx*q2*q3 - 2*bx*q1*q4 - my + 2*bz*q1*q2 + 2*bz*q3*q4
bz*q1^2 + 2*bx*q1*q3 - bz*q2^2 + 2*bx*q2*q4 - bz*q3^2 + bz*q4^2 - mz

Here the issue will look as follows:

bx*q1^2 - mx + bx*q2^2 - bx*q3^2 - bx*q4^2 - q1^2 - q2^2 - q3^2 - q4^2 - 2*bz*q1*q3 + 2*bz*q2*q4 + 1
                                              2*bx*q2*q3 - 2*bx*q1*q4 - my + 2*bz*q1*q2 + 2*bz*q3*q4
bz*q1^2 - mz - bz*q2^2 - bz*q3^2 + bz*q4^2 - q1^2 - q2^2 - q3^2 - q4^2 + 2*bx*q1*q3 + 2*bx*q2*q4 + 1

This is due to a factorization problem:

Do you know a way around this?

fyi. It works fine for the first issue I posted.

[Neel Mehta]

So I managed to do a function that simplifies a symbolic expression of a 4 dimension quaternion. The only problem I have with it is that it is way too long. If anyone knows how to reduce it, I'm open for any ideas.

function [simp] = quatsimp(q,q1,q2,q3,q4)
j = 1 - q1^2 - q2^2 - q3^2 - q4^2;
q = simplify(q);
q = expand(q);

if q(1) == 0
    c1 = 0;
else
    c1 = symvar(q(1));
    c1(c1 == q1) = [];
    c1(c1 == q2) = [];
    c1(c1 == q3) = [];
    c1(c1 == q4) = [];
end

if q(2) == 0
    c2 = 0;
else
    c2 = symvar(q(2));
    c2(c2 == q1) = [];
    c2(c2 == q2) = [];
    c2(c2 == q3) = [];
    c2(c2 == q4) = [];
end

if q(3) == 0
    c3 = 0;
else
    c3 = symvar(q(3));
    c3(c3 == q1) = [];
    c3(c3 == q2) = [];
    c3(c3 == q3) = [];
    c3(c3 == q4) = [];
end

if q(4) == 0
    c4 = 0;
else
    c4 = symvar(q(4));
    c4(c4 == q1) = [];
    c4(c4 == q2) = [];
    c4(c4 == q3) = [];
    c4(c4 == q4) = [];
end

for i = 1:length(c1)
    if q(1) == 0
        f1 = 0;
    else
        f = coeffs(q(1), c1(i));
        f(1) = [];
        f1(i) = f;
    end
end

for i = 1:length(c2)
    if q(2) == 0
        f2 = 0;
    else
        f = coeffs(q(2), c2(i));
        f(1) = [];
        f2(i) = f;
    end
end

for i = 1:length(c3)
    if q(3) == 0
        f3 = 0;
    else
        f = coeffs(q(3), c3(i));
        f(1) = [];
        f3(i) = f;
    end
end

for i = 1:length(c4)
    if q(4) == 0
        f4 = 0;
    else
        f = coeffs(q(4), c4(i));
        f(1) = [];
        f4(i) = f;
    end
end

d1 = f1.*c1;
d2 = f2.*c2;
d3 = f3.*c3;
d4 = f4.*c4;

g1 = -simplify(sum(d1)-q(1));
g2 = -simplify(sum(d2)-q(2));
g3 = -simplify(sum(d3)-q(3));
g4 = -simplify(sum(d4)-q(4));

if f1 == 0
    f1 = 0;
else
    for i = 1:length(f1)
        if (has(f1(i), q1^2) && has(f1(i), q2^2) && has(f1(i), q3^2) && has(f1(i), q4^2))
            f1(i) = f1(i) + j;
        else
            f1(i) = f1(i);
        end
    end
end

for i = 1:length(f2)
    if (has(f2(i), q1^2) && has(f2(i), q2^2) && has(f2(i), q3^2) && has(f2(i), q4^2))
        f2(i) = f2(i) + j;       
    else
        f2(i) = f2(i);
    end
end

for i = 1:length(f3)
    if (has(f3(i), q1^2) && has(f3(i), q2^2) && has(f3(i), q3^2) && has(f3(i), q4^2))
        f3(i) = f3(i) + j;       
    else
        f3(i) = f3(i);
    end
end

for i = 1:length(f4)
    if (has(f4(i), q1^2) && has(f4(i), q2^2) && has(f4(i), q3^2) && has(f4(i), q4^2))
        f4(i) = f4(i) + j;       
    else
        f4(i) = f4(i);
    end
end

d1 = f1.*c1;
d2 = f2.*c2;
d3 = f3.*c3;
d4 = f4.*c4;

h1 = expand(simplify(sum(d1)+g1));
h2 = expand(simplify(sum(d2)+g2));
h3 = expand(simplify(sum(d3)+g3));
h4 = expand(simplify(sum(d4)+g4));

h = [h1;h2;h3;h4];

for i = 1:length(h)
    if (has(h(i), q1^2) && has(h(i), q2^2) && has(h(i), q3^2) && has(h(i), q4^2))
        h(i) = h(i) + j;       
    else
        h(i) = h(i);
    end

simp = h;
end

end

[James Tursa]

What about this strategy. Generate these expressions and pick the "shortest" result:

original

subs(original,q1,sqrt(1-q2^2-q3^2-q4^2))

subs(original,q2,sqrt(1-q1^2-q3^2-q4^2))

subs(original,q3,sqrt(1-q1^2-q2^2-q4^2))

subs(original,q4,sqrt(1-q1^2-q2^2-q3^2))

Maybe this will allow all of those extra factors you have to be accounted for.

EDIT

Actually I just ran a short test on your sample expressions and realized that you have individual q1, q2, q3, and q4 variables in the expression ... not just the squared variables. So this isn't going to work as stated either.

Looks like you only want the squared terms potentially simplified?

[Neel Mehta]

Exactly, only the squared terms. I think the strategy you gave could create other complications.

I also realized that the function I wrote will only work if the coefficient of the terms are purely q squared.

So probably this wont work:

Maybe there is a way to fix this issue using a loop.

However, for the Madgwick paper, the function I created is sufficient. But I was thinking, maybe it would be cool to create a symbolic expression quaternion simplification function for the MATLAB community.

Maybe just a quick expaination of my code:

1. Simplify and expand the vector.
2. Gather all symbolic variables in the matrix and delete,
3. Find the coefficients of the vector components (q(1) to q(4))
4. Gather all the rest that do not belong to a symbolic variable and save them (I called them g).
5. Using a for loop and if statement, if all are present, then add
6. Multiply them back to their symbolic variable and add g.
7. Put them back into a vector (h).
8. Run through loop to check for missing .
9. Done

[James Tursa]

One more strategy to consider. Pick the shortest of:

original

simplify(original)

simplify(subs(original,q1^2,1-q2^2-q3^2-q4^2))

simplify(subs(original,q2^2,1-q1^2-q3^2-q4^2))

simplify(subs(original,q3^2,1-q1^2-q2^2-q4^2))

simplify(subs(original,q4^2,1-q1^2-q2^2-q3^2))

Then look for terms containing common factors that are not quaternion elements and group them together. E.g., this function:

EDIT for vectorization

% Simplify symbolic expression containing quaternion elements by
% using the identity q1^2 + q2^2 + q3^2 + q4^2 = 1.
% This code is vectorized.
% Programmer: James Tursa
% Date: 2020-12-02
function Q = simplifyq(q)
% symbols we know about
syms q1 q2 q3 q4
% first cut at substitutions
qsubs = {q1,q1;
         q1^2,1-q2^2-q3^2-q4^2;
         q2^2,1-q1^2-q3^2-q4^2;
         q3^2,1-q1^2-q2^2-q4^2;
         q4^2,1-q1^2-q2^2-q3^2};
% vectorization loop
Q = expand(simplify(q));
for m=1:numel(Q)
    q = Q(m);
    % find the substitution with the shortest length
    s = q;
    sc = char(s); sc(sc==' ')=[];
    sn = length(sc);
    for k=1:size(qsubs,1)
        t = simplify(subs(q,qsubs{k,1},qsubs{k,2})); % substitute from above list
        tc = char(t); tc(tc==' ')=[];
        tn = length(tc);
        if( tn < sn )
            s = t;
            sn = tn;
        end
    end
    % now look for factors that we can combine
    v = symvar(s); % get the variables
    isq = @(s)s==q1||s==q2||s==q3||s==q4; % helper function
    [C,T] = coeffs(s); % get coefficients and terms
    S = 0; % initialize our final answer
    for k=1:numel(v) % for each variable
        if( ~isq(v(k)) ) % if variable isn't a quaternion element
            b = arrayfun(@(x)ismember(v(k),symvar(x)),T); % which terms contain v(k)
            S = S + prod(factor(sum(C(b).*T(b)))); % product of factors
            C(b) = []; T(b) = []; % eliminate these terms we already used
        end
    end
    Q(m) = S + sum(C.*T); % add in anything left over
end
end

produces this:

>> e
e =
 bx*q1^2 - 2*bz*q1*q3 + bx*q2^2 + 2*bz*q2*q4 - bx*q3^2 - bx*q4^2 - mx
               2*bx*q2*q3 - 2*bx*q1*q4 - my + 2*bz*q1*q2 + 2*bz*q3*q4
 bz*q1^2 + 2*bx*q1*q3 - bz*q2^2 + 2*bx*q2*q4 - bz*q3^2 + bz*q4^2 - mz
>> simplifyq(e)
ans =
 - mx - bx*(2*q3^2 + 2*q4^2 - 1) - 2*bz*(q1*q3 - q2*q4)
       2*bz*(q1*q2 + q3*q4) - 2*bx*(q1*q4 - q2*q3) - my
   2*bx*(q1*q3 + q2*q4) - bz*(2*q2^2 + 2*q3^2 - 1) - mz
>> f
f =
 bx*q1^2 - mx + bx*q2^2 - bx*q3^2 - bx*q4^2 - q1^2 - q2^2 - q3^2 - q4^2 - 2*bz*q1*q3 + 2*bz*q2*q4 + 1
                                               2*bx*q2*q3 - 2*bx*q1*q4 - my + 2*bz*q1*q2 + 2*bz*q3*q4
 bz*q1^2 - mz - bz*q2^2 - bz*q3^2 + bz*q4^2 - q1^2 - q2^2 - q3^2 - q4^2 + 2*bx*q1*q3 + 2*bx*q2*q4 + 1
>> simplifyq(f)
ans =
 - mx - bx*(2*q3^2 + 2*q4^2 - 1) - 2*bz*(q1*q3 - q2*q4)
       2*bz*(q1*q2 + q3*q4) - 2*bx*(q1*q4 - q2*q3) - my
   2*bx*(q1*q3 + q2*q4) - bz*(2*q2^2 + 2*q3^2 - 1) - mz

Maybe this can get you what you want most of the time. I'm not sure how to force the symbolic engine to have the products listed as +bx*(etc) instead of -bx*(etc)

[Neel Mehta]

Hi James, thanks for that code. It looks great. I am not too sure what you mean with:

" I'm not sure how to force the symbolic engine to have the products listed as +bx*(etc) instead of -bx*(etc)"

The answer looks correct...

However, I tested it with this:

        2*q2*q4 - 2*q1*q3 - ax
        2*q1*q2 - ay + 2*q3*q4
q1^2 - q2^2 - q3^2 + q4^2 - az

and it did not work. This is the result:

-ax
-ay
-az

Also, a simple other problem that you forgot is to insert expand and simplify in the beginning of your code. Namely:

q = expand(simplify(q));

But I must admit, your code looks much nicer! If I wasn't in the last 3 weeks of my thesis, I would work on it. I will definetely look through it properly to learn from this strategy after the thesis. Thanks!

Edit: I got what you meant. I'm not too sure either. Maybe using collect? But I don't think it is important.

[James Tursa]

You might have picked up a version that I was editing. Download it again and try. Here is what I get:

>> g
g =
         2*q2*q4 - 2*q1*q3 - ax
         2*q1*q2 - ay + 2*q3*q4
 q1^2 - q2^2 - q3^2 + q4^2 - az
>> simplifyq(g)
ans =
     2*q2*q4 - 2*q1*q3 - ax
     2*q1*q2 - ay + 2*q3*q4
 - 2*q2^2 - 2*q3^2 - az + 1

[Neel Mehta]

Oh, my bad! Yeah, works great! You just need to put the simplify and expand in the beginning:

% Simplify symbolic expression containing quaternion elements by
% using the identity q1^2 + q2^2 + q3^2 + q4^2 = 1.
% This code is vectorized.
% Programmer: James Tursa
% Date: 2020-12-02
function Q = simplifyq(q)
% symbols we know about
syms q1 q2 q3 q4
q = expand(simplify(q)); % This is missing.
% first cut at substitutions
qsubs = {q1,q1;
         q1^2,1-q2^2-q3^2-q4^2;
         q2^2,1-q1^2-q3^2-q4^2;
         q3^2,1-q1^2-q2^2-q4^2;
         q4^2,1-q1^2-q2^2-q3^2};
% vectorization loop
Q = q;
...

so that it will work for this type of case:

q1*(bx*q1 - bz*q3) - mx - q3*(bx*q3 + bz*q1) + q2*(bx*q2 + bz*q4) - q4*(bx*q4 - bz*q2)
q2*(bx*q3 + bz*q1) - my - q1*(bx*q4 - bz*q2) + q3*(bx*q2 + bz*q4) - q4*(bx*q1 - bz*q3)
q1*(bx*q3 + bz*q1) - mz + q3*(bx*q1 - bz*q3) + q2*(bx*q4 - bz*q2) + q4*(bx*q2 + bz*q4)

I just get this answer:

bx*(q1^2 + q2^2 - q3^2 - q4^2) - mx - 2*bz*(q1*q3 - q2*q4)
          2*bz*(q1*q2 + q3*q4) - 2*bx*(q1*q4 - q2*q3) - my
bz*(q1^2 - q2^2 - q3^2 + q4^2) - mz + 2*bx*(q1*q3 + q2*q4)

However, thats just a simple issue for a great code. Thanks again for your help! :-)

[James Tursa]

Thanks. Inserted your addition into my code above so it has this change. Now I get:

>> h
h =
 q1*(bx*q1 - bz*q3) - mx - q3*(bx*q3 + bz*q1) + q2*(bx*q2 + bz*q4) - q4*(bx*q4 - bz*q2)
 q2*(bx*q3 + bz*q1) - my - q1*(bx*q4 - bz*q2) + q3*(bx*q2 + bz*q4) - q4*(bx*q1 - bz*q3)
 q1*(bx*q3 + bz*q1) - mz + q3*(bx*q1 - bz*q3) + q2*(bx*q4 - bz*q2) + q4*(bx*q2 + bz*q4)
>> simplifyq(h)
ans =
 - mx - bx*(2*q3^2 + 2*q4^2 - 1) - 2*bz*(q1*q3 - q2*q4)
       2*bz*(q1*q2 + q3*q4) - 2*bx*(q1*q4 - q2*q3) - my
   2*bx*(q1*q3 + q2*q4) - bz*(2*q2^2 + 2*q3^2 - 1) - mz

[Neel Mehta]

Looks good, James. It's definetely better than mine since it also factorizes automatically. If you want, you could also figure out a problem with the naming convention of the quaternion components that are simplified. Since typical ones are:

etc...

That's why I did this:

function [simp] = quatsimp(q,q1,q2,q3,q4)
...
end

So that if anyone has named their qauternion components differently, then they can write it in.

However, I don't think it's important.

So, to sum it all, I'm gonna use your code for my thesis, since it is shorter and the output looks nicer.

Thank you for your help. I really appreciate it.

[James Tursa]

OK, here is a version that attempts to discover the symbols used for the quaternion elements first before simplifying.

% Simplify symbolic expression containing quaternion elements by
% using the identity q1^2 + q2^2 + q3^2 + q4^2 = 1.
% This code is vectorized.
% Programmer: James Tursa
% Date: 2020-12-11
function Q = simplifyq(q)
% possible quaternion element variables
syms q0 q1 q2 q3 q4 Q0 Q1 Q2 Q3 Q4 qw qx qy Qw Qx Qy QW QX QY qW qX qY
qz = sym('qz'); % need to do qz a special way
qZ = sym('qZ'); % need to do qZ a special way
Qz = sym('Qz'); % need to do Qz a special way
QZ = sym('QZ'); % need to do QZ a special way
% figure out which symbols are present
Q = expand(simplify(q));
V = symvar(sum(Q(:))); % get the variables
q0_present = ismember(q0,V) || ismember(Q0,V);
q4_present = ismember(q4,V) || ismember(Q4,V);
qn_present = ismember(q0,V) || ismember(q1,V) || ismember(q2,V) || ismember(q3,V) || ismember(q4,V);
Qn_present = ismember(Q0,V) || ismember(Q1,V) || ismember(Q2,V) || ismember(Q3,V) || ismember(Q4,V);
qx_present = ismember(qw,V) || ismember(qx,V) || ismember(qy,V) || ismember(qz,V);
qX_present = ismember(qW,V) || ismember(qX,V) || ismember(qY,V) || ismember(qZ,V);
Qx_present = ismember(Qw,V) || ismember(Qx,V) || ismember(Qy,V) || ismember(Qz,V);
QX_present = ismember(QW,V) || ismember(QX,V) || ismember(QY,V) || ismember(QZ,V);
qsum = qn_present + Qn_present + qx_present + qX_present + Qx_present + QX_present;
if( (q0_present && q4_present) || qsum > 1 )
    error('Ambiguous quaternion elements found');
elseif( qsum == 0 )
    warning('No quaternion elements found');
    return;
elseif( ismember(q0,V) )
    P1 = q0; P2 = q1; P3 = q2; P4 = q3;
elseif( ismember(Q0,V) )
    P1 = Q0; P2 = Q1; P3 = Q2; P4 = Q3;
elseif( qn_present )
    P1 = q1; P2 = q2; P3 = q3; P4 = q4;
elseif( Qn_present )
    P1 = Q1; P2 = Q2; P3 = Q3; P4 = Q4;
elseif( qx_present )
    P1 = qw; P2 = qx; P3 = qy; P4 = qz;
elseif( qX_present )
    P1 = qW; P2 = qX; P3 = qY; P4 = qZ;
elseif( Qx_present )
    P1 = Qw; P2 = Qx; P3 = Qy; P4 = Qz;
elseif( QX_present )
    P1 = QW; P2 = QX; P3 = QY; P4 = QZ;
else
    error('Internal coding error ... contact author');
end
% first cut at substitutions
qsubs = {P1,P1;
         P1^2,1-P2^2-P3^2-P4^2;
         P2^2,1-P1^2-P3^2-P4^2;
         P3^2,1-P1^2-P2^2-P4^2;
         P4^2,1-P1^2-P2^2-P3^2};
isq = @(s)s==P1||s==P2||s==P3||s==P4; % helper function
% vectorization loop
for m=1:numel(Q)
    q = Q(m);
    % find the substitution with the shortest length
    s = q;
    sc = char(s); sc(sc==' ')=[];
    sn = length(sc);
    for k=1:size(qsubs,1)
        t = simplify(subs(q,qsubs{k,1},qsubs{k,2})); % substitute from above list
        tc = char(t); tc(tc==' ')=[];
        tn = length(tc);
        if( tn < sn )
            s = t;
            sn = tn;
        end
    end
    % now look for factors that we can combine
    v = symvar(s); % get the variables
    [C,T] = coeffs(s); % get coefficients and terms
    S = 0; % initialize our final answer
    for k=1:numel(v) % for each variable
        if( ~isq(v(k)) ) % if variable isn't a quaternion element
            b = arrayfun(@(x)ismember(v(k),symvar(x)),T); % which terms contain v(k)
            S = S + prod(factor(sum(C(b).*T(b)))); % rewrite as prod of factors
            C(b) = []; T(b) = []; % eliminate these terms we already used
        end
    end
    Q(m) = S + sum(C.*T); % add in anything left over
end
end

[Neel Mehta]

Wow! That's a great effort. You should upload it on the file exchange. I'm sure alot of people would be very thankful.