MATLAB: Madgwick filter – Quaternion Multiplication

madgwick filterMATLABquaternionquaternion 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

Best Answer

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?
Related Question