rodralez / NaveGo

NaveGo: an open-source MATLAB/GNU Octave toolbox for processing integrated navigation systems and performing inertial sensors analysis.
Other
581 stars 212 forks source link

Are quaternion corrections wrong? #120

Closed scott198510 closed 2 years ago

scott198510 commented 2 years ago

% Quaternion corrections % Crassidis. Eq. 7.34 and A.174a. antm = [0.0 qua(3) -qua(2); -qua(3) 0.0 qua(1); qua(2) -qua(1) 0.0]; % ??????

In your code, % Quaternion corrections % Crassidis. Eq. 7.34 and A.174a. antm = [0.0 qua(3) -qua(2); -qua(3) 0.0 qua(1); qua(2) -qua(1) 0.0];

antm = -skewm((qua(1) qua(2) qua(3))

    qua = qua + 0.5 .* [qua(4)*eye(3) + antm; -1.*[qua(1) qua(2) qua(3)]] * kf.xp(1:3);
    qua = qua / norm(qua);       % Brute-force normalization

But according to Crassidis. Eq. 7.34 and A.174a. no minus sign in "antm"

My code: % Quaternion corrections % Crassidis. Eq. 7.34 and A.174a. antm =skewm((qua(1) qua(2) qua(3)) %We can get antm = [0.0 -qua(3) qua(2); qua(3) 0.0 -qua(1); -qua(2) qua(1) 0.0]; qua = qua + 0.5 . [qua(4)eye(3) + antm; -1.[qua(1) qua(2) qua(3)]] kf.xp(1:3); qua = qua / norm(qua); % Brute-force normalization

There is just a minus sign difference between the code and your paper/book. Can you give me some explanations? Thks!

@

scott198510 commented 2 years ago

@rodralez @paolozoccarato

rodralez commented 2 years ago

Hello,

You are right. According to Crassidis' equations, antm should be positive. But, I think I took this portion of code from Crassidis MATLAB examples (http://www.acsu.buffalo.edu/~johnc/second_ed/), instead of transforming his equations to MATLAB code. Moreover, if antm is set as positive, NaveGo diverges. In summary, antm sign should be kept as negative for now.