Forum Rule: Always post complete source code & details to reproduce any issue!
Page 2 of 28 FirstFirst 1 2 3 4 12 ... LastLast
Results 26 to 50 of 683

Thread: uNav INS

  1. #26
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    Great, thx. Saw in the code where you check TOW to decide whether to update or not. Neat.

  2. #27
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    537
    Quote Originally Posted by Don Kelly View Post
    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/JanFe...0(secured).pdf
    https://web.stanford.edu/group/scpnt...ONNTM_2003.pdf

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

  3. #28
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    Brian,
    Here's another good reference on tightly vs loosely-coupled, more along the lines of how I've heard it defined. Guess the references aren't consistent!

    INS/GPS Integration Architectures, George Schmidt and Richard Phillips, 2010

    www.dtic.mil/get-tr-doc/pdf?AD=ADA581020

  4. #29
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    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?

  5. #30
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    537
    Quote Originally Posted by Don Kelly View Post
    Brian,

    Trying to think how id take a step back and build back into the full 15 state.
    Im 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 id 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 id 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 id 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, id 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.

  6. #31
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    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. Im 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

  7. #32
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    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.

  8. #33
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    So im back to thinking along your line of thought Brian. Thinking ill 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.

  9. #34
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    4,121
    Don-Brian

    Think I mentioned last year that I found a implementation of the Kalman Filter for quaternions (4-state). The C++ files are located at https://github.com/matthew-t-watson/Picopter. I am not sure his thesis is still on-line but if you want it I have a copy, http://www.recantha.co.uk/blog/?p=6346. I am in the process of converting it to use with the MPU-9250 library for reference purposes. When I get it working I will post it somewhere.

    Mike

  10. #35
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    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

  11. #36
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    4,121
    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

  12. #37
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    4,121
    Ok. Yaw is terrible but I can't figure it out. Here is what I have so far.

    Mike
    Attached Files Attached Files

  13. #38
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    537
    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.

  14. #39
    Junior Member
    Join Date
    Feb 2018
    Posts
    16
    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;

  15. #40
    Junior Member
    Join Date
    Feb 2018
    Posts
    16
    Quote Originally Posted by brtaylor View Post
    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;

  16. #41
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    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!

    uNavINS_MPU9250_I2CV5.zipuNavINS_LayoutV1.txt

  17. #42
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    4,121
    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.

  18. #43
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    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

  19. #44
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    4,121
    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

  20. #45
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    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

  21. #46
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    4,121
    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

  22. #47
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    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.000000000 0, 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.000000001 1, -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.000000001 1, -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.000000001 1, -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.000000001 1, -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

  23. #48
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    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.

  24. #49
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    417
    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.

  25. #50
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    4,121
    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 by mjs513; 03-27-2018 at 11:27 PM.

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •