uNav INS
- Thread starter brtaylor
- Start date
Hi All,
I have some questions! I've been using the Kalman filter as described within this post. However, I'm really struggling with it.
Did anyone post a stable version that works?
I'm wondering if I'm updating it frequently enough? I'm using a Teensy 3.2 that's doing other things at the same time but I'm managing to run the filter at approx 40-50Hz. Is that fast enough?
With the device sitting flat on my desk I'm getting wildly drifting pitch/roll/yaw and the values I'm getting from the filter don't make sense.
Like:
pitch: 3 to -30 degrees
roll: 2 to -115 degrees
yaw: 25 to -164 degrees
I've definitely calibrated the MPU9250 correctly - getting reasonable results from it. The values from it are pretty stable, so I don't understand how the filter can vary so wildly. The GPS inputs to the filter are relatively (but not totally) stable - within the normal variations. If GPS is lost, the filter goes waaay off really quickly, suggesting huge speeds and the like.
Any help would be greatly appreciated as this is the final stage of a major project!
Thanks,
Andrew
I have some questions! I've been using the Kalman filter as described within this post. However, I'm really struggling with it.
Did anyone post a stable version that works?
I'm wondering if I'm updating it frequently enough? I'm using a Teensy 3.2 that's doing other things at the same time but I'm managing to run the filter at approx 40-50Hz. Is that fast enough?
With the device sitting flat on my desk I'm getting wildly drifting pitch/roll/yaw and the values I'm getting from the filter don't make sense.
Like:
pitch: 3 to -30 degrees
roll: 2 to -115 degrees
yaw: 25 to -164 degrees
I've definitely calibrated the MPU9250 correctly - getting reasonable results from it. The values from it are pretty stable, so I don't understand how the filter can vary so wildly. The GPS inputs to the filter are relatively (but not totally) stable - within the normal variations. If GPS is lost, the filter goes waaay off really quickly, suggesting huge speeds and the like.
Any help would be greatly appreciated as this is the final stage of a major project!
Thanks,
Andrew
willie.from.texas
Well-known member
I tried but was never able to get the uNav-INS to work, even when injecting static GPS values into the filter. Have you tried the uNav Attitude and Heading Reference System (AHRS)? That uses the MPU9250 but not the GPS. I was able to get that to work, and was relatively stable as long as I didn't leave it running without moving the MEMs chip once in a while.
mjs513
Senior Member+
Finally catching up here.
Same comment as @willie.from.texas. Tried several variations of the Kalman filter with the MPU-9250 with no luck - a lot of yaw drifting even though pitch and roll may be fairly stable. Had better luck with the various AHRS filters.
Somewhere in this thread is a Sketch that actually contains about 4 of the different filters you can use. I will see if I can find it and repost it. Funny thing is that I had more luck with the Kalman filter implemented for the prop shield with the prop shield.
Same comment as @willie.from.texas. Tried several variations of the Kalman filter with the MPU-9250 with no luck - a lot of yaw drifting even though pitch and roll may be fairly stable. Had better luck with the various AHRS filters.
Somewhere in this thread is a Sketch that actually contains about 4 of the different filters you can use. I will see if I can find it and repost it. Funny thing is that I had more luck with the Kalman filter implemented for the prop shield with the prop shield.
brtaylor
Well-known member
Hi Andrew,
We flight test often with the version of the EKF implemented in our flight software without issues on both fixed wing and multi-rotor vehicles (https://github.com/bolderflight/RAPTRS/blob/master/software/src/soc/common/uNavINS.cpp). This version does not use the magnetometers and I'm sure the EKF gains are different (they're based on flight test data using a post-processed camera based system as a true reference for attitude). One piece of advice, the filters will not perform well when they are static. They require motion to warm up the filter and keep the filter operating well (without motion, the filter lacks the dynamic information needed and is dominated by GNSS noise).
Spent a bit of time reading through this thread. Interesting project! I’ve made a similar filter myself. I could be corrected if I misunderstand but the reason your filter diverges when you’re not moving is likely due to there not being enough measurements for attitude. So the attitude is only observable when moving. To get this kind of 15 state filter to remain converged when at a standstill is to have at least two attitude references. Typically the magnetic field and gravity vectors suffice. With these two measurements you are able to observe the attitude when not moving. Once moving you can change your measurement model back to what you have. EKF’s are very tolerant of this and allow you to change measurement models on the fly. As long as you remember to also change the measurement error model concurrently otherwise you end up with a hard to debug logic error - trust me!
I use a filter like this in my rockets using a teensy 3.6. They may wait on the pad for over an hour so my measurement model estimates the gravity and earth magnetic field from the state and filter them against the magnetometer and accelerometer measurements keeping the EKF happy and everything steady. Once launched, at burnout the rocket is ballistic and the accelerometer is useless as an attitude measurement so I change the measurement model to skip it and operate without it. With the rocket moving the EKF can now observe the attitude. I still include the magnetometer in the measurement as this improves the attitude estimate when tumbling back to the ground.
Rarely, when the tumble is Very severe I’ve had the filter either drift or diverge; I’m hoping to implement a GPS based attitude reference in the future. Hopefully that would fix things.
Anyhow you may already know all this, but it’s nice to read up on similar projects and maybe give a little input that may help.
I use a filter like this in my rockets using a teensy 3.6. They may wait on the pad for over an hour so my measurement model estimates the gravity and earth magnetic field from the state and filter them against the magnetometer and accelerometer measurements keeping the EKF happy and everything steady. Once launched, at burnout the rocket is ballistic and the accelerometer is useless as an attitude measurement so I change the measurement model to skip it and operate without it. With the rocket moving the EKF can now observe the attitude. I still include the magnetometer in the measurement as this improves the attitude estimate when tumbling back to the ground.
Rarely, when the tumble is Very severe I’ve had the filter either drift or diverge; I’m hoping to implement a GPS based attitude reference in the future. Hopefully that would fix things.
Anyhow you may already know all this, but it’s nice to read up on similar projects and maybe give a little input that may help.
Sent you a PM re: dissertation. Did you successfully convert it? Thanks!
@u_cap, I'm interested in it as well, let me know what you hear from Mike please.
On a separate note, I recently have got the gnss-sdr (GPS software defined radio) software running on a linux platform. Would be awesome to get it going on a Teensy, but am guessing it's going to be a few generations of Teensy in the future (multi-core, much faster CPU speeds) to get it working on a Teensy.
Don
On a separate note, I recently have got the gnss-sdr (GPS software defined radio) software running on a linux platform. Would be awesome to get it going on a Teensy, but am guessing it's going to be a few generations of Teensy in the future (multi-core, much faster CPU speeds) to get it working on a Teensy.
Don
mjs513
Senior Member+
Just sent you a response. Can not upload here either - too big.
Think this is the sketch. Not sure what state it is in. You will probably have to do some mods for your use.
View attachment MPU9250_FiltersGPS_V11a.zip
mjs513
Senior Member+
Morning Don
Just emailed it you. Have fun.
Have two ublox F9Ps sitting in a box that I haven't played with yet. Keep getting sidetracked
brtaylor
Well-known member
Love the F9P's - especially adding in CORS corrections, fast time to fix and low standard deviation.
Haven't done too much with this filter, there were a couple minor errors discovered, but mostly once it is tuned - seems to work really well. Some tests I did have it working favorably compared with much more expensive solutions. Hard to come up with a way of auto-tuning the filter though. Only half the problem is the noise in the IMU and GNSS, the filter tuning also depends a lot on the platform it's being installed on the environment conditions it's being used in.
Haven't done too much with this filter, there were a couple minor errors discovered, but mostly once it is tuned - seems to work really well. Some tests I did have it working favorably compared with much more expensive solutions. Hard to come up with a way of auto-tuning the filter though. Only half the problem is the noise in the IMU and GNSS, the filter tuning also depends a lot on the platform it's being installed on the environment conditions it's being used in.
mjs513
Senior Member+
I picked up a couple of these a long time ago: https://www.ardusimple.com/simplertk2b/, but haven't played with them but they do look promising. Been too busy with T4.0 and T4.1. Now with displays - never enough time.
If I remember right the biggest problem we were having was with the yaw drifting that we never seemed to resolve. Never was able to track down the issue. Would like to give it a try again at some point.
Brian - know you have been real busy with real work stuff but did you ever post an update to code that we were using anywhere?
brtaylor
Well-known member
Attached is a zip of the corrected code. It was being used with some CMake build tools, but should be easy enough to get it working as an Arduino library. Requires Eigen.
Yaw tracking has been good while moving. Here is a plot of a test I did where I was walking on a north-south aligned street and compared the EKF results with the DJI A3 flight control system, a VN-200 and a VN-300. The EKF was using an MPU-9250 IMU (hence the label) and a F9P GNSS receiver. The weird data dropouts on the VN-300, I think were due to a specific cable - this test was performed in February and in April, I tried to reproduce the data dropouts to debug with VectorNav engineers, but couldn't reproduce the issue, which is why I think it was an intermittent cabling fault between the Teensy and the VN-300. Prior to this test, all of the filters were warmed up by moving the platform in pitch, roll, heading circles and changing position for about 5 - 10 minutes.
willie.from.texas
Well-known member
UNav INS on the Teensy 4.1
In the ‘for what it’s worth’ department, I was able to get this code to work on the Teensy 4.1. I was primarily concerned with being able to save and retrieve the calibration data from the EEPROM. It works like a champ! The loop rate is averaging around 6.3 msec, so it’s not quite twice as fast as the Teensy 3.6.
In the ‘for what it’s worth’ department, I was able to get this code to work on the Teensy 4.1. I was primarily concerned with being able to save and retrieve the calibration data from the EEPROM. It works like a champ! The loop rate is averaging around 6.3 msec, so it’s not quite twice as fast as the Teensy 3.6.
brtaylor
Well-known member
Hi Brian
Sorry was had to disappear for awhile. Interesting that the MPU-9250 gives about the same results as the DJI but the VN-200/300 are off by about 45degs?
Well guess I have another project for the list now
Yeah, seems weird - it's too big of an error to be declination. I haven't had a chance yet to do a deep dive into what's going on.
mjs513
Senior Member+
@brtaylor
Well manage to get the new uNavINS incorporated into a sketch and running on the T4.0 mounted on a Teensy Backpack 9250 along with the ZED-F9P running in GPS mode. Still getting a slow yaw drift but better than in previous iterations.
Was wondering how sensitive Yaw is to GPS data such as vE/vN/vD? What I am seeing in the GPS data is that Lat/Lon is fairly stable to 6 decimals but velocities even when steady can go between 0-1m/s.
Will have to do a warm up process as you described:
From my just playing around with it think it might help.
Brian, Thanks for posting the update. Looking forward to trying this out!
I've just got my own GPS SDR running using a HackRF front end and Linux box for the SDR code. Would be awesome to get the SDR code running on the Teensy 4.1, but thinking it will need to wait until we get multi-core faster Teensys.
Don
I've just got my own GPS SDR running using a HackRF front end and Linux box for the SDR code. Would be awesome to get the SDR code running on the Teensy 4.1, but thinking it will need to wait until we get multi-core faster Teensys.
Don