Incorrect F matrix, Velocity Update

[venkat057]

Hi, i found following mismatch/incorrect updation of matlab software and reference "An Approach to bench-marking of loosely coupled low-cost navigation systems"

1. Equation (17C) + sign @ fc + S() +... but in code vel_update function fnc = fn - S() + ... (i.e fnc = fn - Coriolis +fn)
2. F matrix in equation 22, negative sign for Cbn at attitude row but in code negative sing for Cbn in velocity row.
3. Similarly for G matrix,equation 23, negative sign for Cbn at 1st row but in code negative sing for Cbn in second row. Please check and update or comment..!?

[rodralez]

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.)

[venkat057]

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.

[scott198510]

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 ,

[rodralez]

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.

[scott198510]

@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.

[scott198510]

@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

[rodralez]

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

Thank you very much for your help!

Kind regards.

[dkokron]

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;

[dkokron]

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.

[dkokron]

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.

[dkokron]

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