Forum Rule: Always post complete source code & details to reproduce any issue!
Page 1 of 2 1 2 LastLast
Results 1 to 25 of 38

Thread: uNav INS

  1. #1
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    234

    uNav INS

    All, I've ported the University of Minnesota 15 state EKF to Arduino:
    https://github.com/bolderflight/uNavINS

    Beware, it's currently very beta and a work in progress. This filter was designed by Adhika Lie at the University of Minnesota and, the PowerPC and BeagleBone Black versions, are the primary navigation filter used for research there since 2012 for the quaternion version of the filter (there had been an Euler version of the filter used earlier). This filter loosely integrates IMU and GPS measurements. The IMU is propagated in a time update to estimate the inertial state (corrected IMU measurements, orientation, LLA position, NED velocity). When GPS measurements are available, they are used as a measurement update to correct the inertial state and estimate the IMU biases.

    Known current issues with this port of the filter are:
    1. Accelerometer and gyro biases are not being updated
    2. Set methods are needed for setting the sensor characteristics
    3. Get methods are needed for covariance in order to measure the filter covariance
    4. Versions of this filter use magnetometer for increased heading accuracy. These should be incorporated.

    This algorithm has been ported from it's original source:
    https://github.com/UASLab/OpenFlight...15state_quat.c

    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

  2. #2
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    5,181
    Nice, looking after some code to get a time stamp tied to GPS PPS and it looks promising - (6X) less overhead to compute than micros and ties to cycle counter gives it resolution derived from F_CPU ( 240,000,000 on T_3.6 ). With PPS interrupt it repeats to some few cycles difference per second ( 16 cycles is Max-Min in 1200 seconds ) - shows the error in the core crystal perhaps +/- 2K or so { current average is 239,998,104 against PPS and unadjusted RTC counter was +2K). Logging this with averaging as the cycles per second will change based on onboard CPU crystal environment and I've see it at +/- 10 cycles from that average for hours - and it should have just improved coding.

    I see in the code it just takes "tnow = (float) micros()/1000000.0f;". At 240 MHz the CycleCounter rolls over every 17.85 seconds. If you could see a clock tied to cycle counter updated with PPS what would be needed to track time? I've got the Adafruit Ultimate unit and the few GPS reports I saw it shows the time sync's to time.000 which would tie very accurately to the PPS based timing. Question - how are IMU data points interleaved when it takes longer to get GPS string ( 6+ms serial transfer time) than more frequent IMU updates?
    Last edited by defragster; 01-03-2018 at 11:54 PM. Reason: Overstated Error Variance versus current

  3. #3
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    544
    Hi Brian.
    Wow. Just a little more complicated. Any way I am starting making my way through this and the first thing is defining the error characteristics of navigation parameters. This is a little different than the our normal calibration where with get the offsets and scale factors, you can pretty much use what you have currently for the calibration.

    The noise characteristics for the accelerometer aren't too bad to get but characterizing the gryo I would think would lead you to doing a Allan Variance on the gyro. This would then be dependent on the range and bandwidth selected for the gyro. The one thing that always got me was do you do the characteristics before or after the calibration values are applied, i.e., use raw values, probably raw values.

    If you want I can take a cracked at this piece of it.

    Mike

  4. #4
    Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    68
    Thx Brian, looking forward to checking it out. Out of pocket for a couple of days tho! Don

  5. #5
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    544
    Hi Brian

    Spent the day playing around with allan variance. Finally finished collecting the data (847K points). I have attached a few plots for you. Still need to go through them and see what I can come up with for the program. Ran the tests at 16g, 2000dps with dlpf of 20. If you want another set let me know.

    The file is too big to load so here is a link to the file: https://drive.google.com/file/d/1oaI...ew?usp=sharing
    Last edited by mjs513; 01-06-2018 at 01:51 AM.

  6. #6
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    5,181
    mjs513 - the plots don't show?

    In my quest for a fast unified timer I tapped the Serial receive pin with an isr() to capture the first bit transition of incoming GPS serial message. That time based on F_CPU CycleCounter normalized (?) with the GPS PPS signal gives a fast way to time tag events - GPS reception and IMU data notification (same tap on MPU9250 interrupt gets uniform time). Shows 6X faster than micros() read and will resolve +/- a few dozen nanoseconds rather than +/- a uS.

    A WIP coming together - I have the Adafruit Ult GPS DonK sent me.

    Will that Adafruit unit be able to provide the data points needed to feed this 15 state EKF? Seems to be missing an accel field I heard?

    This shows 30 hours of cycle counts (averaged as the crystal freq moves) on Y against the +/- of the incoming PPS signal on T_3.6 at 240 MHz.
    Click image for larger version. 

Name:	ppsccnt.png 
Views:	16 
Size:	17.5 KB 
ID:	12528
    Last edited by defragster; 01-06-2018 at 02:10 AM.

  7. #7
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    544
    @defragster - did you download the zip and open the pdfs? If not I will upload the reports individually.

  8. #8
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    5,181
    Quote Originally Posted by mjs513 View Post
    @defragster - did you download the zip and open the pdfs? If not I will upload the reports individually.
    I'll try that - it wasn't up when I refreshed before my post.

    <edit>: Indeed the zip downloaded and is valid
    Last edited by defragster; 01-06-2018 at 03:00 AM.

  9. #9
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    234
    Quote Originally Posted by defragster View Post
    Nice, looking after some code to get a time stamp tied to GPS PPS and it looks promising - (6X) less overhead to compute than micros and ties to cycle counter gives it resolution derived from F_CPU ( 240,000,000 on T_3.6 ). With PPS interrupt it repeats to some few cycles difference per second ( 16 cycles is Max-Min in 1200 seconds ) - shows the error in the core crystal perhaps +/- 2K or so { current average is 239,998,104 against PPS and unadjusted RTC counter was +2K). Logging this with averaging as the cycles per second will change based on onboard CPU crystal environment and I've see it at +/- 10 cycles from that average for hours - and it should have just improved coding.

    I see in the code it just takes "tnow = (float) micros()/1000000.0f;". At 240 MHz the CycleCounter rolls over every 17.85 seconds. If you could see a clock tied to cycle counter updated with PPS what would be needed to track time? I've got the Adafruit Ultimate unit and the few GPS reports I saw it shows the time sync's to time.000 which would tie very accurately to the PPS based timing. Question - how are IMU data points interleaved when it takes longer to get GPS string ( 6+ms serial transfer time) than more frequent IMU updates?
    That's a really good question and one that I would have to defer for the time being and think about more. My initial guess would be to apply the GPS measurement update to the IMU data closest in time to it and then re-propagate the time updates on the IMU data forward.

  10. #10
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    234
    Quote Originally Posted by defragster View Post
    mjs513 - the plots don't show?

    In my quest for a fast unified timer I tapped the Serial receive pin with an isr() to capture the first bit transition of incoming GPS serial message. That time based on F_CPU CycleCounter normalized (?) with the GPS PPS signal gives a fast way to time tag events - GPS reception and IMU data notification (same tap on MPU9250 interrupt gets uniform time). Shows 6X faster than micros() read and will resolve +/- a few dozen nanoseconds rather than +/- a uS.

    A WIP coming together - I have the Adafruit Ult GPS DonK sent me.

    Will that Adafruit unit be able to provide the data points needed to feed this 15 state EKF? Seems to be missing an accel field I heard?

    This shows 30 hours of cycle counts (averaged as the crystal freq moves) on Y against the +/- of the incoming PPS signal on T_3.6 at 240 MHz.
    Click image for larger version. 

Name:	ppsccnt.png 
Views:	16 
Size:	17.5 KB 
ID:	12528
    Probably not, if I recall correctly the MTK chipset only gives one GPS velocity measurement instead of a NED velocity.

  11. #11
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    234
    Quote Originally Posted by mjs513 View Post
    Hi Brian

    Spent the day playing around with allan variance. Finally finished collecting the data (847K points). I have attached a few plots for you. Still need to go through them and see what I can come up with for the program. Ran the tests at 16g, 2000dps with dlpf of 20. If you want another set let me know.

    The file is too big to load so here is a link to the file: https://drive.google.com/file/d/1oaI...ew?usp=sharing
    Awesome data, thanks! That'll really help inform some of the calibration programs on the MPU-9250 as well; looks like I should be using a longer time period for computing gyro bias for instance.

  12. #12
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    544
    Just for sake of completeness the gryo values I used was raw values divided by sensitivity. Not sure if I should of done that or not. Let me know if you want me to re-run the data. Don't think it will make much of a difference though. Takes less than a minute. Found a nifty program to do the analysis for me. Also, I can collect the data tonight for 2g/250dps if you would like.

    UPDATE: Ok I ran the 2g/250dps case with dlpf of 20. I also ran the allan variance on the magnetometer readings since I had them. Here is the link: https://drive.google.com/file/d/1pQM...ew?usp=sharing

    EDIT: I moved the zip files into a folder on google. The new link is: https://drive.google.com/open?id=1YQ...rF98A7PiT7OKNS . I have also added a excel spreadsheet comparing the gyro axes for the two ranges. For some reason something is not sitting right with me. @Brian if you get a chance can you check for me.

    Cheers
    Mike

    UPDATE: after looking at the gy data a more closely it looks like there was some sort of corruption for the gy axis. So I reran the 2g/250 case. I posted the results in the GitHub directory, https://drive.google.com/open?id=1YQ...rF98A7PiT7OKNS
    Last edited by mjs513; 01-07-2018 at 12:58 PM.

  13. #13
    Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    68
    Brian,
    Have been on travel, finally back and catching up... I have a couple of quick questions for you.

    - What's the output rate on your GPS, 5Hz?
    - Does the EKF require both GPS position and velocity measurements? It looks like it does from the code. I'm asking because I'm curious if I can run an Ultimate GPS board (which as best I can tell, only provides GPS LLA out). I just got a uBlox, so long-term will move to that.
    - Looks like the code runs the EKF at the GPS rate, which I'm guessing may be 5Hz. Can you also run the propagate step of the EKF at a much faster rate, say 400Hz (i.e., for drone use for example)?

    Thx!
    Don

  14. #14
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    234
    Quote Originally Posted by Don Kelly View Post
    Brian,
    Have been on travel, finally back and catching up... I have a couple of quick questions for you.

    - What's the output rate on your GPS, 5Hz?
    - Does the EKF require both GPS position and velocity measurements? It looks like it does from the code. I'm asking because I'm curious if I can run an Ultimate GPS board (which as best I can tell, only provides GPS LLA out). I just got a uBlox, so long-term will move to that.
    - Looks like the code runs the EKF at the GPS rate, which I'm guessing may be 5Hz. Can you also run the propagate step of the EKF at a much faster rate, say 400Hz (i.e., for drone use for example)?

    Thx!
    Don
    Hi Don,
    - The code was originally developed using a 1 Hz GPS, but I currently use a 5 Hz unit.
    - Yes, the EKF uses LLA position and NED velocities from the GPS.
    - The EKF time update is run at whatever rate the update function is called. The measurement update is carried out at the GPS rate, of course. I can't think of any issues running the time update at 400 Hz, not that I think that rate is needed for the dynamics, as long as the processor time for the update is fast enough.

    Brian

  15. #15
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    544
    looks like I should be using a longer time period for computing gyro bias for instance.
    What would be more interesting is if there was a way to redo the gyro bias periodically while the filter was still running. For instance if there is zero motion for short period of time the recalculate the biases. You can use a zero motion detect algorithm for that, I do have one of those that I use in some of my stuff

    Mike

  16. #16
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    234
    Quote Originally Posted by mjs513 View Post
    What would be more interesting is if there was a way to redo the gyro bias periodically while the filter was still running. For instance if there is zero motion for short period of time the recalculate the biases. You can use a zero motion detect algorithm for that, I do have one of those that I use in some of my stuff

    Mike
    I agree, I've started working on a framework for that in the MPU-9250 library, but haven't pushed it to github yet. The idea being that you could grab the IMU data at different levels of processing (uncompensated data through varying levels of correction and compensation) in case your EKF algorithm is also internally applying a bias correction. I was also thinking of trying to apply other corrections like detecting and ignoring magnetic field changes not associated with motion, etc.

  17. #17
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    544
    Hi Brian
    You might want to check out the following thread on magnetic disturbance, https://forum.pjrc.com/threads/33902...ic+disturbance. For zero motion detection here is what I am using, pretty sensitive. Changing the thresholds will control when zero motion is detected:
    Code:
    void FreeIMU::MotionDetect(float * val) {
    	
    	float gyro[3];
    	float accnorm;
    	int accnorm_test, accnorm_var_test, omegax, omegay, omegaz, omega_test, motionDetect;
    	
    	gyro[0] = val[3] * M_PI/180;
    	gyro[1] = val[4] * M_PI/180;
    	gyro[2] = val[5] * M_PI/180;
    	
        /*###################################################################
        #
        #   acceleration squared euclidean norm analysis
    	#   
    	#   some test values previously used:
    	#       if((accnorm >=0.96) && (accnorm <= 0.99)){
    	#										<= 0.995
        #
        ################################################################### */
        accnorm = (val[0]*val[0]+val[1]*val[1]+val[2]*val[2]);
        if((accnorm >=0.94) && (accnorm <= 1.03)){  
            accnorm_test = 0;
        } else {
            accnorm_test = 1; }
        
    	/** take average of 5 to 10 points  **/
        float accnormavg = accnorm_avg.process(accnorm);
        float accnormtestavg = accnorm_test_avg.process(accnorm_test);
    
        /*####################################################################
        #
        #   squared norm analysis to determine suddenly changes signal
        #
        ##################################################################### */
        //accnorm_var.process(sq(accnorm-accnorm_avg.getAvg()));
    	// was 0.0005
        if(accnorm_var.process(sq(accnorm-accnormavg)) < 0.0005) {
            accnorm_var_test = 0;
        } else {
            accnorm_var_test = 1; }
    
        /*###################################################################
        #
        #   angular rate analysis in order to disregard linear acceleration
        #
    	#   other test values used: 0, 0.00215, 0.00215
        ################################################################### */
        if ((gyro[0] >=-0.005) && (gyro[0] <= 0.005)) {
            omegax = 0;
        } else {
            omegax = 1; }
            
        if((gyro[1] >= -0.005) && (gyro[1] <= 0.005)) {
            omegay = 0;
        } else {
            omegay = 1; }
            
        if((gyro[2] >= -0.005) && (gyro[2] <= 0.005)) {
            omegaz = 0;
        } else {
            omegaz = 1; }
            
        if ((omegax+omegay+omegaz) > 0) {
            omega_test = 1;
        } else {
            omega_test = 0; }
    
    
        /* 
    	###################################################################
        #
        # combined movement detector
        #
        #################################################################### 
    	*/
        if ((accnormtestavg + omega_test + accnorm_var_test) > 0) {
            motionDetect = 1;
        } else {
            motionDetect = 0; }
    
        /* 
    	################################################################## 
    	*/   
        
        //motion_detect_ma.process(motionDetect);
        
        if(motion_detect_ma.process(motionDetect) > 0.5) {
    		val[11] = 1.0f;
        } else {
    		val[11] = 0.0f;
    	}
    }
    This is what I have for magnetic detection:
    Code:
      
    void  FreeIMU::initMagJamCal(){
    	for(int sample_size =0; sample_size < 200; sample_size++){
    		getValues(val);
    		sqr_mag = sqr_mag + sqrt(val[6]*val[6]+val[7]*val[7]+val[8]*val[8]);
    	}
    	MagJamCal_mean = sqr_mag/200;
    }
    and the test in the code:
    Code:
      
           #if defined(DISABLE_MAGJAM)
    	sqr_mag = sqrt(val[6]*val[6]+val[7]*val[7]+val[8]*val[8]);
    	if(sqr_mag < MagJamLwrLimit*MagJamCal_mean || sqr_mag > MagJamUprLimit*MagJamCal_mean) {
    		MagJamFlag = 1;
    	} else {
    		MagJamFlag = 0;
    		getYawPitchRollRadAHRS(ypr,q);
    		old_Yaw = ypr[0]*rad2degs;
    	}
    	val[12] = MagJamFlag ;
    val array is comprised of normalized values for ax, ay, az, gx, gy, gz and values for hx, hy and hz.

    Cheers
    Mike

  18. #18
    Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    68
    Mike and Brian,
    Trying to catch up with where you are, due to my travels last week...

    Got the 15-State running this afternoon with my Teensy3.6, MPU9250, and uBlox m8n receiver. I'm running at 1Hz for now, but will switch it to 5Hz.

    I went outside and think I got a good solid MPU calibration. When I start up the 15-state filter, the PRY angles are low (like -2.0, 0.1, 0.0), but over a 15 min period or so they all drift pretty badly. Did you all (or y'all as we say here in TX) increase accelerometer/gyro process noise or change any other parms to get the drift under control?

    Thx y'all!
    Don

  19. #19
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    544
    Hi Don,

    Don't think Brian has changed any of the sensor characteristic values in his 15-state filter. Based on all the tests I did they will probably have to be tweaked. I haven't really dug too deep into this version yet - was still working with the 7-state version trying to get parameters correct since Brian updated the 7-state library.

    Cheers
    Mike

  20. #20
    Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    68
    Started digging into the 15-State EKF code, and have a couple of observations. Exciting to see it run, and the code looks impressive! Would really like to hear from others on what they think about my observations.

    First, there are several approaches in implementing effective Kalman filters in real-time. I've been involved in a number of these over the years. One approach is to set up a timed loop, i.e. with a fixed deltaT for each loop. For example, I've been running my latest drone flight controller at a 250Hz loop time, which works well for doing the nav and motor commanding. I believe the PixHawk2.1 is running internally at 400Hz.

    The alternative approach is to use interrupts and to run the loop as fast as the microcontroller and MPU and GPS will allow. This is the approach used in this 15-State EKF. There are plusses and minuses to either approach, and it's interesting to see how they differ!

    A second observation for the 15-State EKF code is that there's no real setup of a real time clock (RTC) that's driven off the (super accurate) GPS time. GPS measurements are time-tagged super accurately with the internal accuracy of the GPS chip and system. But the IMU sensor readings (gyro, accel, mag) need to be accurately time-tagged so that the EKF can sync them with the GPS measurements. I guess I was thinking there might be the use of the PSS rising edge to create an super-accurate RTC, which would then be used to time-tag the IMU sensor readings.

    Anyway, wanted to throw that idea out there (defragster and I have been thinking about this!). As long as the microcontroller is VERY fast, and as long as there's not much else going on in the void loop (like serial printing), I'm guessing that just trying to time-tag IMU readings using micro() each loop is maybe accurate enough. Is that what others have found?

    When I run the 15-State with the default parameters, I'm getting significant PRY drift, so I'll start trying some different settings to see if I get them locked in. I may also at some point look at setting up a 400Hz fixed loop like (I think) the PixHawk2.1 is using. Anyway, very interesting code, thanks Brian for posting it!!

  21. #21
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    544
    Hi Don. Just my 2cents here. To be honest haven't started playing around with the 15State Kalman, still cutting my teeth you might say on the 7-state one. Anyway,
    First, there are several approaches in implementing effective Kalman filters in real-time. I've been involved in a number of these over the years. One approach is to set up a timed loop, i.e. with a fixed deltaT for each loop. For example, I've been running my latest drone flight controller at a 250Hz loop time, which works well for doing the nav and motor commanding. I believe the PixHawk2.1 is running internally at 400Hz.

    The alternative approach is to use interrupts and to run the loop as fast as the microcontroller and MPU and GPS will allow. This is the approach used in this 15-State EKF. There are plusses and minuses to either approach, and it's interesting to see how they differ!
    Like you said both approaches have its plusses and minuses. The way Brian implemented the MPU9250 code is by specifying a SRD. At SRD 1 you are running at about 500hz with 2 at about 333hz give or take and using the interrupt when the data is ready. Kind of like this approach. As for the PixHawk2.1 running at 400Hz, can't really address that one, but I know ardupilot has two loops set up - 100Hz and 50Hz. Think the IMU is in the 100Hz loop while the GPS is in the 50Hz loop. I believe I read somewhere that the PixHawk4 is updating the IMU data a 1Khz.

    I just ran the 7-state Kalman at SRD=1 and 2 with no detrimental effects compared to running it at SRD=9 (100Hz). For comparison I've run the Madgwick/Mahoney filters at 1Khz without a problem. Only thing I would say is do we really need to update at those kind of rates. Not a UAV person myself, my efforts been with ground vehicles.

    Anyway, wanted to throw that idea out there (defragster and I have been thinking about this!). As long as the microcontroller is VERY fast, and as long as there's not much else going on in the void loop (like serial printing), I'm guessing that just trying to time-tag IMU readings using micro() each loop is maybe accurate enough. Is that what others have found?
    As far as I can tell most of the Ublox modules like the M8N, M8U don't have a the PPS signal broken out like the Adafruit ultimate gps that @defragster is using for his tests. So the software would have to have an option to use either or.

    When I run the 15-State with the default parameters, I'm getting significant PRY drift, so I'll start trying some different settings to see if I get them locked in
    That's one of the reasons I have always shied away from the Kalman filter. Getting the parameters tuned in is a challenge from what I have seen.

    Thanks for listening to my ramblings.
    Mike

  22. #22
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    234
    Hi Don, interesting discussion.

    First, there are several approaches in implementing effective Kalman filters in real-time. I've been involved in a number of these over the years. One approach is to set up a timed loop, i.e. with a fixed deltaT for each loop. For example, I've been running my latest drone flight controller at a 250Hz loop time, which works well for doing the nav and motor commanding. I believe the PixHawk2.1 is running internally at 400Hz.
    My understanding is that it really depends on the dynamics of the vehicle and how well they're captured with your system equation used in the Kalman filter. In making a library, my attempt is to generalize the filter to run at whatever rate the user is trying to setup; hence measuring the length of the time step. The intention is to still have a fixed timestep, but make software modifications easier by not specifying it in the library. Coming from a fixed-wing aircraft world, most of our flight control software runs at 50 Hz; some unique projects, one that is controlling structural modes of an aircraft for instance, needed to run at 150 Hz. The navigation filter didn't need to run that fast, but since it was in the main loop and measured the time step, we could easily change the main loop timing without breaking the filter.

    A second observation for the 15-State EKF code is that there's no real setup of a real time clock (RTC) that's driven off the (super accurate) GPS time. GPS measurements are time-tagged super accurately with the internal accuracy of the GPS chip and system. But the IMU sensor readings (gyro, accel, mag) need to be accurately time-tagged so that the EKF can sync them with the GPS measurements. I guess I was thinking there might be the use of the PSS rising edge to create an super-accurate RTC, which would then be used to time-tag the IMU sensor readings.

    Anyway, wanted to throw that idea out there (defragster and I have been thinking about this!). As long as the microcontroller is VERY fast, and as long as there's not much else going on in the void loop (like serial printing), I'm guessing that just trying to time-tag IMU readings using micro() each loop is maybe accurate enough. Is that what others have found?
    It's a loosely coupled filter. When you're running the filter at a lower rate, I would think the timing between the GPS and IMU doesn't matter as much as running the filter at much higher rates. One option for timing is the MPU-9250 has an FSYNC input, where it can use a bit in its measurement data to indicate when FSYNC was pulsed. It's most useful if you're using the FIFO capability of the MPU-9250 and only gets you accurate to the nearest 1 ms, which is why I haven't worked on implementing FSYNC functionality yet (the Teensy could get down to 1 us accuracy as you point out).

    When I run the 15-State with the default parameters, I'm getting significant PRY drift, so I'll start trying some different settings to see if I get them locked in.
    Doesn't surprise me . The filter likely needs to be tuned for the MPU-9250 and the uBlox GPS receivers. It was originally built around the ADIS16405 IMU and a Novatel GPS receiver. There's also likely some bugs from porting the original and I'm not sure if I'm carrying enough numerical precision. I've developed and been playing with a 4 state KF AHRS and I'll be working my way back to the 7 state EKF AHRS and then the 15 state EKF INS. I'm trying some approaches out, like gathering IMU statistics during initialization, to make implementing these filters easier and am starting from the most intuitive filter I could think of (a 4 state linear KF) and gradually building up complexity from there.

  23. #23
    Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    68
    Mike,
    Shame the M8N doesn't have the PPS readily available. It's on Pin 3 of the Neo M8N, but my soldering skills aren't good enough to tap into off the board directly from that tiny connection!

    I'm guessing the reason that some UAVs are running the nav filters at 250-400 Hz is that this is a good speed for commanding the stepper motors. Just a guess tho! That way you can have a single speed loop and do the nav, PID, accept RC controller inputs, and run the ESCs for the motors, all in the same loop.

    Another thing I haven't figured out is why the gps has to be "read" (using gps.read ) at the start of each void loop in the 15-state. I need to dig in the uBlox code and see what that read does. Is it only reading if there's new data, is it parsing it each void loop too, why not only read gps data only when it's new (either with interrupt or PPS signal)?

    Anyway... this is definitely an interesting project for me!

  24. #24
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    234
    Quote Originally Posted by Don Kelly View Post
    Mike,
    Another thing I haven't figured out is why the gps has to be "read" (using gps.read ) at the start of each void loop in the 15-state. I need to dig in the uBlox code and see what that read does. Is it only reading if there's new data, is it parsing it each void loop too, why not only read gps data only when it's new (either with interrupt or PPS signal)?
    It's checking the serial buffer, parsing any serial data, and checking to see whether there is a complete GPS packet there. It will return "true" on the loop when a complete packet has been fully read and parsed off the serial buffer, not that I'm checking the return value in this example. All I'm doing in the example is keeping the GPS data current. Every IMU triggered frame, I send that data to the filter and, within the filter, if the TOW data has changed, that indicates the GPS data is new and the filter processes it as a measurement update.

  25. #25
    Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    68
    Brian,
    Funny you should mention starting with 4-State and then building to 7-State and then 15-State. I was originally working on a simple 6-State (3 posn, 3 vel) linear KF that I could loosely-couple with the GPS and Mahony/Madgwick output of the MPU9250. Put that on hold to mess around with this 15-State EKF!

    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.

    Good to know where the original parameters were built around. I'll try some things to see what happens! Thx!

    Don

Posting Permissions

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