Madgwick filter theoretical problem

[dyso]

After reading through the paper and the library, I found something I did not understand. Maybe someone can help me.

How is it possible that the paper states that:

bx = sqrtf(hx*hx+hy*hy);

But the library says that

_2bx = sqrtf(hx*hx+hy*hy);

[J-M-L]

Just a guess - I’m on my smartphone so can’t look it up in details:

You should check the formulas but my assumption would be that h[sub]x[/sub] and h[sub]y[/sub] and h[sub]z[/sub] as calculated in the library are not the same as in the original paper but twice as much (probably using 2m_x 2m_y and 2m_z in the paper’s formulas and just m_x,m_y and m_z in code or something like that).

So if this is a correct assumption then

orig_h_x = 1/2 * h_x
orig_h_y = 1/2 * h_y

So when in the paper they are doing

orig_b_x = sqrt(orig_h_x²+ orig_h_y²)

It’s translates to

orig_b_x = sqrt(0.5².h_x²+ 0.5².h_y²) = sqrt(0.5²) * sqrt(h_x²+ h_y²)

Which is orig_b_x = 0.5 * sqrt(h_x²+ h_y²)

Hence this would explain the
_2bx = sqrt(h_x²+ h_y²)

If that’s true then for coherence they would have been better calling the library h_x and h_y variables _2hx and _2hy.

Of course it would mean then that the earth’s magnetic field vector components are off but It does not matter when discussing just the direction of the vector if all the components are multiplied by a constant (the vector is longer or shorter but the direction is the same).

If I remember well the equations involve a factor 2 or 4 but not just b_x or b_y so may be there is an advantage in reducing the number of computations doing it this way.

Just some food for thoughts on what to double check, I might be totally wrong

[dyso]

Sorry for the late reply. I just saw your answer now.

So I had a similar assumption, but if this is correct, then the paper is wrong. The variable mx, my, mz are taken directly from raw readings of the IMU.
Therefore, equation 29 or 31 from the paper must be wrong.

I checked all the steps. The only one that did not make sense to me was this one. I have simulated the Madgwick filter from the beginning to the end symbolically using MATLAB to ensure that I understood the steps taken on the library.

I have also declared:

m = [0, mx, my, mz]

and using the quaternion multiplication of:

h = q (x) m (x) q*

I become the correct h:

h = [0, hx, hy, hz]

Also the gradient decent algorithm corrective step are correct.

(Maybe I misunderstood what you said)

[J-M-L]

I’m still on the run and just on my iPhone

Did you check if check if hx, hy and hz as calculated in the library are the same as in the original paper? (If they used 2mx 2my and 2mz in the paper's formulas and just mx,my and mz in code then the formula would indeed calculate 2.bx ).

[dyso]

Yeah, nothing on the paper regarding 2*m. I made a small summary that you will find in the attachments (AHRSlibrary.pdf). I will also upload a symbolic MATLAB script that you can use for visual aid and the paper from Madgwick.

I somehow cannot believe that the library is wrong. It was checked by so many people. Unfortunately, I am not at home the couple of weeks, so I cannot test if it would make a difference if I change the library.

MATLAB script:

% Estimation of IMU and MARG orientation using a gradient descent algorithm
% Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan
% June 29 - July 1, 2011

clear;
clc;

syms q1 q2 q3 q4 gx gy gz ax ay az mx my mz bx bz by hx hy hz

%% Symbolic Parameters
Beta = 0.1; %Eq.(33)
% SR = 500;

q = [q1 q2 q3 q4]; %Eq.(9)
cong_q = [q1 -q2 -q3 -q4]; % Conjugate of q

% q1 = q(1);
% q2 = q(2);
% q3 = q(3);
% q4 = q(4);

gyro = [0 gx gy gz]; %Eq.(1)
wg = [0 0 0 1]; %Eq.(10)
a = [0 ax ay az]; %Eq.(11)
b = [0 bx 0 bz]; %Eq.(14)
m = [0 mx my mz]; %Eq.(15)
h = [0 hx hy hz]; %Eq.(31)

%% Symbolic Calculations

% Gyroscope
qdotg =0.5*quatmul(q,gyro); %Eq.(2)

% Accelerometer
fg1 = quatmul(cong_q,wg); %Eq.(6),(12)
fg = quatmul(fg1,q) - transpose(a); %Eq.(6),(12)
fg = quatsimp(fg,q1,q2,q3,q4); %Eq.(6),(12)
fg(1) = []; %Eq.(6),(12)
Jg = jacobian(fg, [q1, q2, q3, q4]); %Eq.(13)
Jg_t_fg = transpose(Jg)*fg; %Eq.(21)

% Magnetometer
h1 = quatmul(q,m); %Eq.(31)
h = quatmul(h1, cong_q); %Eq.(31)
h = quatsimp(h,q1,q2,q3,q4); %Eq.(31)
h(1) = []; %Eq.(31)
hx = h(1); hy = h(2); hz = h(3); %Eq.(31)
b = [0 sqrt(hx^2+hy^2) 0 hz]; %Eq. (32)
bx = b(2); bz = b(4); %Eq. (32)

fb1 = quatmul(cong_q,b); %Eq.(6),(16)
fb = quatmul(fb1,q) - transpose(m); %Eq.(6),(16)
fb = quatsimp(fb,q1,q2,q3,q4); %Eq.(6),(16)
fb(1) = []; %Eq.(6),(16)
Jb = jacobian(fb, [q1, q2, q3, q4]); %Eq.(17)
Jb_t_fb = transpose(Jb)*fb; %Eq.(21)

% Error function
ftot = Jg_t_fg + Jb_t_fb; %Eq.(21)
fnorm = ftot * (1/sqrt(ftot(1)^2+ftot(2)^2+ftot(3)^2+ftot(4)^2)); %Eq.(30)

% Update
qdot = qdotg - Beta*fnorm; %Eq.(30)

% qnew = qdot./SR;
% q = qdot./SR;

% roll = atan2(q1*q2+q3*q4,0.5-q2^2-q3^2);
% pitch = asin(-2.0*(q2*q4-q1*q3));
% yaw = atan2(q2*q3+q1*q4, 0.5-q3^2-q4^2);


%% Functions for Quaternions (not important to understand the process)
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

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

AHRSlibrary.pdf (195 KB)

madgwick_internal_report.pdf (1.47 MB)

[dyso]

Did you figure this one out? I would still love to know.

[J-M-L]

The point you make is that in the paper b_x is defined as the square root of (h_x² + h_y²) and in the library they have the same square root but call that two times bx.

I had just a high level quick look and they have different definition of h_x,y,z

In the paper, this looks like this and they use two_mx

In the library, they just use mx

The expressions are different. Have you checked if they actually match or if they would actually have a factor 2 difference?

it would make sense if you can calculate directly 2bx since the matrix does not use just bx, but requires 2bx and 4bx. So it would save one multiplication.

(also if h_z was twice as big and the square root twice as big as well, then the direction of the earth’s magnetic field would still be correct. The vector would just be twice as long but if you are only interested in the direction....)

[dyso]

I tried posting the answer but there is a limit of 9000 characters. I had to create a new PDF.

What I did is compare ∇f with h. Anyway, here it is.

Madgwick.pdf (189 KB)