Incorrect F matrix, Velocity Update about navego HOT 11 CLOSED

X is just cross product surely, Neither matrix product, nor hadamard product!!!
you can do as this,
OMEGA =skewm (omega_en_n + 2 *omega_ie_n )；
fnc = fn -OMEGA * v + gn;

% fn: 3x1, omega_en_n:3x1, omega_ie_n:3x1; gn:3x1
% then call the function skewm, you can get OMEGA that is 3x3
% OMEGA * v 3x3 x 3x1 -> 3x1
% so you get fnc that is 3x1 ,

from navego.

Hi thank you for correcting me.

I have verified Prof. Groves, 2nd Edition.. Still the below equations are not matching... Please look into below points and correct me...

1. fnc = fn - (omega_en_n + 2 *omega_ie_n ) X v + gn; X is cross product or can also be taken as skew symmetric matrix of v. But in matlab SW, fnc = fn - v X (omega_en_n + 2 *omega_ie_n ) + gn; see equation 5.53 in Grooves book. Post multiplication with skew symmetric matrix will not give same result with pre multiplication.
2. F matrix in equation 14.63, positive sign for Cbn at velocity row but in code negative sign for Cbn in velocity row.
3. Similarly for G matrix.

from navego.

Hello,

NaveGo has been modified since that article was published. This document is kind of outdated. Currently, it is a mistake to state that NaveGo's mathematical model is based on our previous articles (Gonzalez et al, 2015, and Gonzalez et al, 2015b). I have modified the README.md file to reflect this situation.

Now, NaveGo mathematical model is develop mostly based on the book by Prof. Groves Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (2nd Ed.)

from navego.

Hi thank you for correcting me.

I have verified Prof. Groves, 2nd Edition.. Still the below equations are not matching... Please look into below points and correct me...

1. fnc =  fn - (omega_en_n + 2 *omega_ie_n ) X v + gn;   X is cross product or can also be taken as skew symmetric matrix of v.  But in matlab SW,  fnc =  fn - v X  (omega_en_n + 2 *omega_ie_n )  + gn;  see equation 5.53 in Grooves book.    Post multiplication with skew symmetric matrix will not give same result with pre multiplication.

Hello,

NaveGo equation seems to be right since it mimics Eq. 5.53 from Groves. Bear in mind that omega_ie_n and omega_en_n are skew-symmetric matrices (upper case omega in Groves), so cross product is replaced by a normal product (https://en.wikipedia.org/wiki/Skew-symmetric_matrix#Cross_product).

2. F matrix in equation 14.63, positive sign for Cbn at velocity row but in code negative sign for Cbn in velocity row.

3. Similarly for G matrix.

Well, I think this change in sign is due to a mix of models between Groves' and Titterton's. NaveGo will keep the Groves formula, so negative sign is discarded.

from navego.

@rodralez hello, Doctor
I have an question about why you edit the code to that " omega_ie_n and omega_en_n are skew-symmetric matrices "
Even that, in att_update.m:
input wb is 3x1, but when you do that:
wb = ( wb - DCMbn' * (omega_ie_n + omega_en_n));

% you get wb is 3x3, This is meaningless， even though there is no error in code.
% wb 3x1 incremental turn-rates in body-frame, not skew-symmetric matrices.
% I think wb should be a vector, not a matrix.

% att_update: updates attitude using quaternion or DCM.
%
% INPUT
% wb, 3x1 incremental turn-rates in body-frame (rad/s).
% DCMbn, 3x3 body-to-nav DCM.
% qua, 4x1 quaternion.
% omega_ie_n, 3x1 Earth rate (rad/s).
% omega_en_n, 3x1 Transport rate (rad/s).
% dt, 1x1 IMU sampling interval (s).
% att_mode, attitude mode string.
% 'quaternion': attitude updated as quaternion. Default value.
% 'dcm': attitude updated as Direct Cosine Matrix.

from navego.

@rodralez meanwhile I think the output of omega_ie_n and omega_en_n should be two vectors, not two matrices
then when we use it in specified position, we can call skewm to get a matrix.
Otherwise, we will change the dimension of the matrix as described above

from navego.

Hi Scott, yes this is a bug. I fixed it.

Thank you very much for your help!

Kind regards.

from navego.

Sorry to dredge this up again, but I cannot resolve some of the entries in ins-gnss/F_update.m with my copy of the Groves book (second edition). Specifically, the contents of F12 in the code appear to be -F12 from the book.
From the code

a11 =    0 ; a12 =         1/RO; a13 = 0;
a21 = -1/RO; a22 =            0; a23 = 0;
a31 =    0 ; a32 = -tan(lat)/RO; a33 = 0;
F12 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

From the book

Similarly, F13 in the code is different as well.
From the code

a11 =                             -Om*sin(lat); a12 = 0; a13 =           -Ve/(RO^2);
a21 =                                        0; a22 = 0; a23 =            Vn/(RO^2);
a31 =  -Om*cos(lat) - (Ve/((RO)*(cos(lat))^2)); a32 = 0; a33 = (Ve*tan(lat))/(RO^2);
F13 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

From the book

The placement of DCMbn matrix in the F matrix is different as well.
From the code

% Eq. 14.63 from Groves
F = [F11 F12 F13 DCMbn Z  ;
     F21 F22 F23 Z     DCMbn  ;
     F31 F32 F33 Z     Z      ;
     Z   Z   Z   Fgg   Z      ;
     Z   Z   Z   Z     Faa    ;
    ];

And from the book

A minor difference is seen in F23. Element 3,3 in the code
a33 = Ve^2/(RN+h)^2 + Vn^2/(RM+h)^2 - 2*g0/res;

and from the book

I suspect the code should read
a33 = Ve^2/(RM+h)^2 + Vn^2/(RN+h)^2 - 2*g0/res;

from navego.

I now see that Groves uses a different Kalman error state than NaveGo. NaveGo swapped the gyro and accelerometer bias elements. That changes the math quite a bit and might account for the differences I see above.

from navego.

Swapping bg and ba in the Kalman state vector does account for the different placement of DCMbn in the NaveGo.

As for the missing minus sign in the NaveGo F12 and F13, I confirmed that the Groves F12 and F13 matrices are consistent with his derivation starting from 14.52. In particular, 14.53 includes two minus signs which he does not explicitly carry forward.

He apparently collects the minus signs and then starts working on

I really wish he had carried the minus sign forward as that would make it easier to follow along.

from navego.

It turns out that 'fixing' the F12 and F13 matrices by using the having them correspond with Groves does not change the simulation output.

from navego.