uNav INS

Brian,
I noticed you called the 15-State "loosely-coupled." I've always viewed the loosely-coupled approaches as being when you're combining a fast-running Mahony/Madgwick (or like your 7-State) that processes IMU sensor data and combining that with a slower-running KF or EKF that pulls in GPS measurements. So I was thinking of the 15-State as more tightly-coupled EKF with the Kalman propagation step running (using IMU sensor data) very fast and the Kalman update step running (using GPS data) much slower, but still coupled together in a single EKF via the EKF dynamics.

Here are a couple of good references describing the different levels of coupling:
http://www.insidegnss.com/auto/JanFeb07GNSSSolutions (secured).pdf
https://web.stanford.edu/group/scpnt/gpslab/pubs/papers/Alban_IONNTM_2003.pdf

For the second reference, it's really only through the introduction.
 
Brian,

Trying to think how i’d take a step back and build back into the full 15 state.
I’m thinking of using the 15-state code as basis, but stepping back and first get 3 states working, then 9, then 15.

The 15 states are 3 posn, 3 vel, 3 angle, 3 gyro bias, 3 accel bias.

So for the first step, i thought i’d reduce the order to 3 states (the three angles) and get that working well. So the KF would be running just just the propagation stage. So i’d have to change the State transition matrix to its reduced 3 by 3 order, along with fixing the order of most of the other vectors and matrices.

Then i’d add back in the 3 posn and 3 vel states, making it a 9 state. This would add back in the
KF update stage.

One this works, i’d add back in the 3 gyro bias and 3 accel bias states, and get that working.

Any thoughts on this, is it too much trouble?
 
Brian,

Trying to think how i’d take a step back and build back into the full 15 state.
I’m thinking of using the 15-state code as basis, but stepping back and first get 3 states working, then 9, then 15.

The 15 states are 3 posn, 3 vel, 3 angle, 3 gyro bias, 3 accel bias.

So for the first step, i thought i’d reduce the order to 3 states (the three angles) and get that working well. So the KF would be running just just the propagation stage. So i’d have to change the State transition matrix to its reduced 3 by 3 order, along with fixing the order of most of the other vectors and matrices.

Then i’d add back in the 3 posn and 3 vel states, making it a 9 state. This would add back in the
KF update stage.

One this works, i’d add back in the 3 gyro bias and 3 accel bias states, and get that working.

Any thoughts on this, is it too much trouble?

I'm not 100% sure how the 15 state EKF is only working with 3 of the 4 quaternion states, so I would guess that you would want to do: 4 state, 10 state, and 15 state. Seems like it would be a good buildup.

As I've said, I'm working on a 4 state linear KF, then I'd like to re-derive and program the 7 state EKF, then the 15 state followed by some higher state ones. The difference being my 4 state linear KF has a different state transition and observation matrix than a 4 state EKF would.

I'll post new and/or updated code as I progress through deriving and testing each version.
 
As best i can tell, the 15-state uses the quaternion to get the euler angles, but the euler angles themselves are included as states, not the quaternions. So the 15 states are
- 3 posn
- 3 velocity
- 3 angles
- 3 accel biases
- 3 gyro biases

When i look at the 7-state, it appears to use the quaternions directly as states
- 4 quaternions
- 3 biases

So it seems they are two different approaches in the selection of their states. I’m leaning towards the 15-state approach of using the angles as states rather than the quaternion, but still using the quaternion to calculate the angles. So i could (assuming it works!) reduce the 15 states down to the three angles, then go to 9 (adding posn and vel states), then go to 15 (add the 6 bias states). No idea which way is easiest tho!!

Don
 
Looking at the 15-state cpp file again, i think you’re correct, the 15 state is directly using the angle elements of the quaternion as states 7,8,9, not the euler angles. So they are only using 3 of the 4 quaternion values in the 15 state.
 
So i’m back to thinking along your line of thought Brian. Thinking i’ll go back to the 7-state, but reduce it to 4 states (the for quaternions), and get that going solidly first, then add back on from there.
 
Mike,
Sounds cool. I don’t see his thesis out there, looks like link is gone. may I get copy (don.kelly@mac.com)?

Also, see my recent post over at the uNavAHRS blog. Changed the initialization of Fsys on the 7-State and so far it seems to be working better. Looking to see if others see the same after changing Fsys!

Don
 
Just sent it to your email. I will take a look at changing Fsys as well. Got the new version working with the 9250 but getting some strange results. It seems if that I have to reset the MPU9250 a few times to get good yaw readings. Its like the magnetometer connection gets flaky. I will post the code shortly to see if anyone else sees this or its my sensor.

Mike
 
Ok. Yaw is terrible but I can't figure it out. Here is what I have so far.

Mike
 

Attachments

  • EKF 7-State.zip
    6.6 KB · Views: 168
My thought process for the INS is to extend the 7 state EKF (https://github.com/bolderflight/uNavAHRS) integrating to NED velocities and NED positions (from a starting location). The measurement update would be the NED velocities and positions from a GPS. Likely I'll have to do some processing to convert GPS LLA to an NED delta from a starting location. I'm thinking that using NED positions rather than LLA will allow us to keep the whole filter using float values, which should make it better for running on microcontrollers other than Teensy 3.5/3.6. The downside is loss of accuracy once a certain distance away from the starting location (loss of floating point precision and, possibly, curvature of the earth). Probably not a huge impact for most UAV / drone users though.
 
I want to use EKF_15State_quat,but I have some problem,why select F and G matrix like this?could you tell me some reference paper ,so I can to understand it easy.
// JACOBIAN
F = mat_fill(F, ZERO_MATRIX);
// ... pos2gs
F[0][3] = 1.0; F[1][4] = 1.0; F[2][5] = 1.0;
// ... gs2pos
F[5][2] = -2 * g / EARTH_RADIUS;

// ... gs2att
temp33 = sk(f_b, temp33);
atemp33 = mat_mul(C_B2N, temp33, atemp33);

F[3][6] = -2.0*atemp33[0][0]; F[3][7] = -2.0*atemp33[0][1]; F[3][8] = -2.0*atemp33[0][2];
F[4][6] = -2.0*atemp33[1][0]; F[4][7] = -2.0*atemp33[1][1]; F[4][8] = -2.0*atemp33[1][2];
F[5][6] = -2.0*atemp33[2][0]; F[5][7] = -2.0*atemp33[2][1]; F[5][8] = -2.0*atemp33[2][2];

// ... gs2acc
F[3][9] = -C_B2N[0][0]; F[3][10] = -C_B2N[0][1]; F[3][11] = -C_B2N[0][2];
F[4][9] = -C_B2N[1][0]; F[4][10] = -C_B2N[1][1]; F[4][11] = -C_B2N[1][2];
F[5][9] = -C_B2N[2][0]; F[5][10] = -C_B2N[2][1]; F[5][11] = -C_B2N[2][2];

// ... att2att
temp33 = sk(om_ib, temp33);
F[6][6] = -temp33[0][0]; F[6][7] = -temp33[0][1]; F[6][8] = -temp33[0][2];
F[7][6] = -temp33[1][0]; F[7][7] = -temp33[1][1]; F[7][8] = -temp33[1][2];
F[8][6] = -temp33[2][0]; F[8][7] = -temp33[2][1]; F[8][8] = -temp33[2][2];

// ... att2gyr
F[6][12] = -0.5;
F[7][13] = -0.5;
F[8][14] = -0.5;

// ... Accel Markov Bias
F[9][9] = -1.0 / TAU_A; F[10][10] = -1.0 / TAU_A; F[11][11] = -1.0 / TAU_A;
F[12][12] = -1.0 / TAU_G; F[13][13] = -1.0 / TAU_G; F[14][14] = -1.0 / TAU_G;

//fprintf(stderr,"Jacobian Created\n");

// State Transition Matrix: PHI = I15 + F*dt;
temp1515 = mat_scalMul(F, imu_dt, temp1515);
PHI = mat_add(I15, temp1515, PHI);

// Process Noise
G = mat_fill(G, ZERO_MATRIX);
G[3][0] = -C_B2N[0][0]; G[3][1] = -C_B2N[0][1]; G[3][2] = -C_B2N[0][2];
G[4][0] = -C_B2N[1][0]; G[4][1] = -C_B2N[1][1]; G[4][2] = -C_B2N[1][2];
G[5][0] = -C_B2N[2][0]; G[5][1] = -C_B2N[2][1]; G[5][2] = -C_B2N[2][2];

G[6][3] = -0.5;
G[7][4] = -0.5;
G[8][5] = -0.5;

G[9][6] = 1.0; G[10][7] = 1.0; G[11][8] = 1.0;
G[12][9] = 1.0; G[13][10] = 1.0; G[14][11] = 1.0;
 
My thought process for the INS is to extend the 7 state EKF (https://github.com/bolderflight/uNavAHRS) integrating to NED velocities and NED positions (from a starting location). The measurement update would be the NED velocities and positions from a GPS. Likely I'll have to do some processing to convert GPS LLA to an NED delta from a starting location. I'm thinking that using NED positions rather than LLA will allow us to keep the whole filter using float values, which should make it better for running on microcontrollers other than Teensy 3.5/3.6. The downside is loss of accuracy once a certain distance away from the starting location (loss of floating point precision and, possibly, curvature of the earth). Probably not a huge impact for most UAV / drone users though.
want to use EKF_15State_quat,but I have some problem,why select F and G matrix like this?could you tell me some reference paper ,so I can to understand it easy.
// JACOBIAN
F = mat_fill(F, ZERO_MATRIX);
// ... pos2gs
F[0][3] = 1.0; F[1][4] = 1.0; F[2][5] = 1.0;
// ... gs2pos
F[5][2] = -2 * g / EARTH_RADIUS;

// ... gs2att
temp33 = sk(f_b, temp33);
atemp33 = mat_mul(C_B2N, temp33, atemp33);

F[3][6] = -2.0*atemp33[0][0]; F[3][7] = -2.0*atemp33[0][1]; F[3][8] = -2.0*atemp33[0][2];
F[4][6] = -2.0*atemp33[1][0]; F[4][7] = -2.0*atemp33[1][1]; F[4][8] = -2.0*atemp33[1][2];
F[5][6] = -2.0*atemp33[2][0]; F[5][7] = -2.0*atemp33[2][1]; F[5][8] = -2.0*atemp33[2][2];

// ... gs2acc
F[3][9] = -C_B2N[0][0]; F[3][10] = -C_B2N[0][1]; F[3][11] = -C_B2N[0][2];
F[4][9] = -C_B2N[1][0]; F[4][10] = -C_B2N[1][1]; F[4][11] = -C_B2N[1][2];
F[5][9] = -C_B2N[2][0]; F[5][10] = -C_B2N[2][1]; F[5][11] = -C_B2N[2][2];

// ... att2att
temp33 = sk(om_ib, temp33);
F[6][6] = -temp33[0][0]; F[6][7] = -temp33[0][1]; F[6][8] = -temp33[0][2];
F[7][6] = -temp33[1][0]; F[7][7] = -temp33[1][1]; F[7][8] = -temp33[1][2];
F[8][6] = -temp33[2][0]; F[8][7] = -temp33[2][1]; F[8][8] = -temp33[2][2];

// ... att2gyr
F[6][12] = -0.5;
F[7][13] = -0.5;
F[8][14] = -0.5;

// ... Accel Markov Bias
F[9][9] = -1.0 / TAU_A; F[10][10] = -1.0 / TAU_A; F[11][11] = -1.0 / TAU_A;
F[12][12] = -1.0 / TAU_G; F[13][13] = -1.0 / TAU_G; F[14][14] = -1.0 / TAU_G;

//fprintf(stderr,"Jacobian Created\n");

// State Transition Matrix: PHI = I15 + F*dt;
temp1515 = mat_scalMul(F, imu_dt, temp1515);
PHI = mat_add(I15, temp1515, PHI);

// Process Noise
G = mat_fill(G, ZERO_MATRIX);
G[3][0] = -C_B2N[0][0]; G[3][1] = -C_B2N[0][1]; G[3][2] = -C_B2N[0][2];
G[4][0] = -C_B2N[1][0]; G[4][1] = -C_B2N[1][1]; G[4][2] = -C_B2N[1][2];
G[5][0] = -C_B2N[2][0]; G[5][1] = -C_B2N[2][1]; G[5][2] = -C_B2N[2][2];

G[6][3] = -0.5;
G[7][4] = -0.5;
G[8][5] = -0.5;

G[9][6] = 1.0; G[10][7] = 1.0; G[11][8] = 1.0;
G[12][9] = 1.0; G[13][10] = 1.0; G[14][11] = 1.0;
 
Made a couple of minor changes to Brian's original files, and got a version that's up and running. Pretty exciting! Note that the uNavINS cpp and h files are included as tabs and not as separate library install. I'm printing all the states (whole states, not error states) to the serial port at about 50Hz. You can use the attached TelemetryViewer file to check out a real-time plot of the whole states.

I have not done any tuning of the filter parameters (Q, R), so am planning to look at that next.

For some of you that have been using the new SPI-Master library, would that be the most efficient way for me to stream a bunch (~25) of internal EKF parameters for tuning and analysis purposes? Guessing that's more efficient than streaming to regular serial port. Sorry I'm behind on the SPI-Master stuff, but was focusing on getting this EKF running!

View attachment uNavINS_MPU9250_I2CV5.zipView attachment uNavINS_LayoutV1.txt
 
Hi Don. Sounds like you have been busy. Did you manage to get yaw stable? If you did what the heck did you do? Will give it a try later. Have choirs to do today. By the way I saw for the M8U there is a high data rate option, think its 30Hz :) Have to do a little more research on that one:)

Using SPI_MSTransfer would definitely be the way to go to offload the stream.
 
Mike,
Thx. I made several tweaks to get it going, mainly changed the signs of the sensor inputs to the filter and adjusted the indices on the Eigen "block" commands. Lots of time looking at the matrices (Fs, PHI, Gs, etc) to make sure they all made sense.

As far as yaw goes, this version will still have drift. This particular 15-state implementation uses magnetometer data only to define the initial offset between IMU-yaw and true heading. My plan is to add the full magnetometer code per the Curt Olsen routines (that Brian referenced) so that heading and yaw hopefully will be better maintained.

So I suppose I'll add the magnetometer code next, and then think about how I'd leverage SPI_MSTransfer to pull out inner parms of the EKF so that I can began some tuning analysis.

Any interest in helping out on the SPI interface? Maybe we could pull out a couple of parameters as a test case (like the raw states, x, and diagonals of covariance, P, perhaps!) to get it going...

Don
 
Don,
Be happy to give you a hand - have to finish one other project up so I can get my MPU9250 back up with the GPS then I can start playing with this for awhile. Give me a couple of days.

Ok "Curt Olsen routines (that Brian referenced)" - must have missed that one.

Mike
 
Great! I need a couple of days (at least!) to get the mag stuff incorporated too...

The Curt Olsen reference was on Brian's first posting from this thread:

"Also, much of the recent work on the filter in a BeagleBone Black environment, including adding magnetometers, has been done by Curt Olson: https://github.com/AuraUAS/navigation "

Don
 
Hi Don.
Not good news. Not sure what the issue is but after about a few seconds it just hangs? It gets an IMU interrupt but then nothing. I've been using same breakout board on a couple of other projects with no issues. I also checked it with the basic i2c sketch for the mpu-9250 and that works no problem.... Any suggestion.

Mike
 
Mike,

On my T3.6, it takes several seconds for all the initialization stuff to happen and print out, like this:


Serial Initialized

Loading Accel and Mag Biases and Scale Factors
Accel and Mag Biases and Scale Factors Loaded

Accel Biases and Scale Factors:
-0.043603, 1.001250
-0.043603, 1.000263
-0.340763, 0.994053

Mag Biases and Scale Factors:
16.742832, 1.019413
43.921291, 1.090688
-24.235130, 0.907284

Gyro Calibration Underway
Gyro Calibration Complete

Setup Complete

Then, if IMU and GPS data is being received, you should get this:

0.0147, -0.2491, -1.5423, 0.0000, 338.9598, 29.6102, -95.1096, 6.7920, -0.0160, 0.0090, -0.0130,-0.0379949504,0.0468452000,0.0083924688,0.0000000000, 0.0000000000,0.0000000000
0.0347, -0.2492, -1.5423, 0.0000, 338.9598, 29.6102, -95.1096, 6.7921, -0.0160, 0.0090, -0.0130,-0.0379949504,0.0468452000,0.0083924688,0.0000000011, -0.0000000005,0.0000008896
0.0547, -0.2487, -1.5417, -0.0005, 338.9594, 29.6102, -95.1096, 6.7923, -0.0161, 0.0092, -0.0125,-0.0379949504,0.0468452000,0.0083924688,0.0000000011, -0.0000000005,0.0000008896
0.0747, -0.2501, -1.5409, -0.0003, 338.9595, 29.6102, -95.1096, 6.7926, -0.0161, 0.0091, -0.0121,-0.0379949504,0.0468452000,0.0083924688,0.0000000011, -0.0000000005,0.0000008896
0.0947, -0.2514, -1.5395, -0.0002, 338.9596, 29.6102, -95.1096, 6.7928, -0.0159, 0.0092, -0.0118,-0.0379949504,0.0468452000,0.0083924688,0.0000000011, -0.0000000005,0.0000008896

I'm running it right now and getting this. Where is yours hanging up? Might have to put some prints in to see where yours stops. I'll think some more on it...

Don
 
Are you running on a T3.6? Mine did hang if I tried to print to serial too fast, but it's doing 50Hz just fine on my T3.6.
 
Mike,

Another possibility... Did you leave uNavINS cpp and h as tabs as I have them rather than as separate library? If you still have uNavINS as a separate library, there may be conflicts with which uNavINS it's trying to use.
 
Hi Don

I am using a T3.5. All the tabs are the same. I get updates for a 2-3 seconds (prints what you showed) and then it hangs right after I get a print that it received interrupt. Maybe its what you posted about printing too fast but I printed faster when testing the SPI_MSTransfer library. I will do a little debugging, I am running it at 120Mhz. I will play around with it a little bit to try and see exactly where it is hanging.

Thanks
Mike

UPDATE: Believe it or not its hanging right at readSensor, very weird.
 
Last edited:
Back
Top