Step 1: make an INS. Step 2: profit
Updated: Feb 2
So I made an inertial navigation system over the Christmas break. Oh, keep the judgemental eyes to yourself. I didn’t go out on Christmas break because of the pandemic (though I would’ve stayed in regardless). Now, those that have either been following my work through my blogs or otherwise, probably know what that is. For those that don’t, here’s the definition. A GNSS-aided inertial navigation system (INS) uses a GNSS sensor (commonly called a GPS) for the position and velocity measurements.
At the moment, GNSS-INS systems are quite expensive with prices starting from 700-1000 USD (for instance, this one costs 2500 USD). This is usually well outside the budget of the rookie roboticist, not just in India, but probably in the rest of the world as well. They provide really high-quality solutions, but the average roboticist doesn't actually need that. Imagine a world where only luxury/sports cars with high price tags were sold. You could either buy one or walk everywhere, and there was no in-between. That is what it feels like to a rookie roboticist (like me) when I see these uber-expensive systems.
Okay, but why do you need it?
GNSS-INS integration is crucial for taking a lab-grown mobile robotics project to the outdoor environment. The outdoor environment does not provide the niceties of the lab where one could use MOCAP or IR tracking for pose feedback. One could certainly use LIDAR and computer vision, but those too have their limitations (try using them in a large empty parking lot). Besides, the use of a GNSS-INS does not preclude the use of LIDAR and CV, it is in fact used to aid them (or be aided by them).
Aren’t there ROS packages for this already?
Ah Yes, the robot-localization package. You see, this backend requires an IMU sensor, which provides the raw accelerations and rotations as well as Euler angles w.r.t the earth frame. Such IMU sensors use accelerometer-gyroscope fusion for orientation estimation. This can be counterproductive when the vehicle undergoes large accelerations, such as during a turn.
This isn’t an issue for slow ground vehicles, however, it limits the applicability of the system. Some of them are also susceptible to magnetic interferences, as they don’t have position-velocity feedback. Furthermore, the ROS localization package does not account for the time-delay in the GPS data, or for that matter, any other data, which needs to be compensated for highly dynamic applications. In simple terms, it is assumed that the GPS position and velocity corresponds to the vehicle’s current position and velocity. In reality, the reported position corresponds to a position ~100-250 ms in the past (depending on the GPS update rate).
A good state-estimation system is a prerequisite for good control, as I have explained in section 3.2.2 of my control systems tutorial. Commercial grade GNSS-INS systems can have update rates as high as 1000Hz with RTK positioning to provide centimeter-level accuracy. However, they are often very expensive and outside the budget of the average weekend roboticist. The systems are expensive due to multiple reasons; the high solution rate as well as solution quality, the fact that this is a low volume market (as compared to say, mobile phones), and third, the after-sales service. Even without the after-sales service, these systems would still be expensive, because someone has to pay the software engineers that write the code for these high-quality systems.
Well, how hard can it be?
So I set out to develop an INS of my own. I was trying to keep the cost of the system low and hence did not use multiple IMUs and RTK/multi-band/multi-constellation GPS. I have made an INS before as a sub-component for my mini-self driving car project. However, the INS in that project made certain assumptions, such as that of planar motion, which reduced its’ applicability to other vehicles. This version makes fewer assumptions and is therefore applicable to a much wider range of vehicle types, such as UAVs. This also meant higher computational cost and thus a costlier microcontroller. I started testing with a hand-made set-up since making the PCB outright could incur an unnecessary cost if I messed up the connections.
I then designed the PCB and the enclosure around it. The enclosure provides environmental protection, prevents airflow around the system from disturbing the barometer reading, and protects the barometer from direct sunlight exposure (which can result in the air temperature spiking inside the pressure sensor's chamber). The GPS is put on top of a small ground plane, although I feel that the size of this ground plane may be insufficient and will likely need to be increased in the future.
The fusion system has 2 parts to it; the attitude heading reference system and the “nav-filter”. Besides this, there is also a dynamic notch filter that uses real-time FFT to find the noise frequencies in gyroscope and accelerometer data and removes them.
The AHRS itself uses an extended Kalman filter which uses the accelerometer and magnetometer to correct the orientation. This is akin to the “IMU” used by the ROS localization package. However, its’ core job is to boot-strap the “Nav-filter” when it is started or reset. The reason why I haven’t used direct measurements to find initial orientation for boot-strap purposes is that the initial alignment errors significantly affect the loop-closure error for small trips. The AHRS is given some time to settle before the “Nav-filter” gets boot-strapped to ensure that the initial alignment is as accurate as possible.
The “Nav-filter” uses an extended Kalman filter to predict the orientation quaternion, velocity, position, accelerometer biases, gyroscope biases, earth’s magnetic field, body-frame magnetic field, and planar wind vector if an airspeed sensor is available. The earth frame and body frame magnetic field predictions can be used for rejecting short term interferences, although that isn’t being done as of right now (I am working on it).
The reason why the Nav filter's orientation output is unaffected by the accelerations is that it uses the GPS position and velocity vector to correct the orientation and not the acceleration vector. While this isn't exactly how it works, a simplified explanation is as follows: the gyroscope is used to predict the orientation, and the orientation is used to calculate the delta velocity vector using the acceleration vector. The GPS can also be used to calculate the same. We then fuse the two delta velocities based on their respective errors (as is done in a Kalman filter). This corrects the velocity vector, but by the same token, also corrects the orientation since an error in the velocity vector can be attributed to an error in the orientation. Furthermore, any angular error between the fused velocity output and the predicted velocity output is due to the gyro biases, and any magnitude error is due to the accelerometer biases. Thus, this can then be used to correct the orientation as well as sensor biases. The actual filter can fuse data from the magnetometer, optical flow sensor, airspeed sensor, and rangefinder if their data is available (although for now it is just an IMU+magnetometer+barometer+GPS setup).
I tested this system both for short term loop closure and long term consistency. The first two graphs show the results for the long term consistency check, and the next two graphs show the short loop closure tests. All the dimensions are in meters. Note that in both cases, there was high magnetic interference at the start of the trip due to the presence of high tension powerlines as well as the subway power lines near the starting point. The long consistency checking trip was also through an area with significant tree foilage and buildings on both sides (I will likely update the graphs with google-maps in the background for reference. For now, just trust me and assume that the output matched the ground truth within reasonable margin). The frame of reference for these graphs is ENU, so north is on the Y-axis and east is on the X-axis.
skewed view from the side showing the position (XYZ)
Top-down view showing only the XY position
Skewed view of small loop closure test (XYZ)
Top-down view of the small loop closure test (XY)
For the loop-closure, the testing wasn't done in a very controlled manner as with my self-driving-car project, and that's mostly because I didn't have the real-estate to do so. I had put this system on my (full-sized)car and tested it on a public road so I can't stop the car "exactly" where I started the trip. Thus it is hard to report numbers just yet. However, I will be conducting more testing, probably with a slightly different setup and shorter trips.
Step 1: make an INS. Step 2: profit… or not?
There is still room for improvement. The barometer reports a higher than actual altitude when the vehicle is moving due to the lower air pressure on the side of the car. This can be fixed by depending more on the GPS, but I would prefer not to do that due to the poor GPS reception quality. According to this study, I also have to tune the Q and R matrices myself since the gyroscope's Anderson-Darling test shows that the gyroscope data is not gaussian distributed. Nonetheless, I would consider version 1 of this project to be "done".
While this version produces fairly high-rate solutions and could be tuned to provide high-quality solutions, it wasn’t intended to be the final version. The cost of the microcontroller used here is quite high (20$) and it isn’t even fully utilized. Even when running at a 1KHz solution rate, the maximum computation load on the controller was 50%, with the average being 25-30%. The purpose of this version was to be a testing platform to improve the software without worrying about the computation speed.
The aim of the next version will be to reduce not only the absolute cost but also the relative cost. The relative cost, as I define it, is the "bang for the buck". Essentially, 500$ (this number is arbitrary) for a GNSS-INS seems expensive. If the same thing could also solve a bunch of other problems such as hardware abstraction, low-level control, and inter-agent communications, it wouldn't seem as expensive anymore. It would be a fully integrated system, requiring you to spend less money and time on setting up the rest of the stuff yourself. Version two will aim to do just that.