Using GNSS-INS-SIM to Simulate IMU Only Navigation of a Left Hand Turn
About a month ago, Aceinna published an open-source Python simulation toolkit for developers that use Inertial Measurement Units (IMU), GPS, and related navigation sensors. In this blog, we will examine how to build a basic motion trajectory and then simulate how much position drift a purely inertial solution would incur tracking the simulated trajectory using “free integration”. The elegance of pure inertial navigation also known as “free integration” is simple — it requires no external dependencies. We choose the application of tracking a left hand turn because the example is of real world value in autonomous driving applications.
The source code for this blog is found at https://github.com/Aceinna/gnss-ins-sim, and the blog is split into three topics:
- Theory of Free Integration Algorithm
- Generating a Motion Trajectory
- Simulating Position, Velocity, and Attitude Drift Error
Theory of Free Integration
In many ways 6-axis free integration of inertial sensor data to compute position and orientation is one of the most basic and obvious uses of accelerometers and rate-sensors (gyros). Free integration is also one of the most performance demanding algorithms in terms of IMU sensor accuracy. However, with proper IMU selection, simulation, and careful deployment pure inertial, “free-integration” can be a powerful navigation tool when other sensors such as LIDAR, Cameras, or GPS are not accurate due to the environmental factors.
The free integration algorithm is conceptually fairly simple. The 3-axis rate output is integrated to orientation. The orientation is used to rotate the measured body frame accelerations into the Earth frame, and then gravity is removed. The resulting Earth frame accelerations are double integrated to position. This is diagramed below:
GNS-INS-SIM implements this as an example algorithm to simulate. The file demo_free_integration.py shows exactly how to invoke this algorithm. The implementation of the free integration itself is contained in the demo_algorithms directory. The algorithm mechanization was chosen for its numerical accuracy. For detailed questions on how this mechanization works please leave a comment.
Generating a Motion Trajectory
GNS-INS-SIM makes it simple to generate a realistic motion trajectory. The motion trajectory is specified by a CSV file, called a motion definition file. The motion definition file starts with the specification of initial conditions, and then is followed by rows of values representing motion commands. There are several different motion command types. The motion command definitions are found in imu_sim.py in the sim directory. Motion command type 1 specifies the rate of change in the “body” frame for each of the Euler angle rates and the body frame velocities.
Here is a simple example motion definition file that generates a realistic left hand turn using Command type 1 below:
ini lat (deg),ini lon (deg),ini alt (m),ini vx_body (m/s),ini vy_body (m/s),ini vz_body (m/s),ini yaw (deg),ini pitch (deg),ini roll (deg)
37.37965348, -121.939627004,0,0,0,0,132.3,0.0,0.0
command type,yaw (deg),pitch (deg),roll (deg),vx_body (m/s),reserved,reserved,command duration (s),reserved
1,0,0,0,1.0,0,0,6.25
1,-27,0,0,0,0,0,3.2
1,0,0,0,0.0,0,0,2.0The first row of numbers specifies the initial conditions as the column names indicate. The next three rows of numbers specifies the motion profile. In this case accelerating at 1 m/s² for 6.25 seconds, and then turning left by 90 degrees over 3.2 seconds, followed by an additional 2.0 seconds of driving. This is pretty similar to how an average driver might cross a busy intersection, and it requires about a 12 seconds period of pure inertial navigation (i.e., “free integration”).
GNSS-INS-SIM can generate a ‘KML’ file which is an XML file format that Google Maps can conveniently read and turn into breadcrumbs. Below is the corresponding motion trajectory on Google maps.
Simulating Position, Velocity, and Attitude Drift Error
With the motion profile and algorithm ready, we can run the simulation. In this blog we have chosen the default Aceinna IMU381 error model. The error model is initialized with typical bias errors for this sensor — 0.5mG and 5deg/Hr respectively. GNSS-INS-SIM makes it convenient to run the simulation with a random distribution of errors. For the example, we will run 10 trials so we can plot them easily. In real work, a developer might chose to run 1000 or more trials to better understand error bounds. GNSS-INS-SIM then returns a summary set of statistics. It prints these both to the command line as well as stores the error summary in a text file ‘summary.txt’.
For this example of the left hand turn, the predicted result is an average drift error of around 9cm with a standard deviation of 10cm. GNS-INS-SIM also generates KML files for each run which are plotted below on Google Maps as blue lines over the reference trajectory in red in order to help visualize the accumulated drift. These plots show that free inertial navigation can be quite accurate for short periods of time.
The example results file summary is shown below:
----------------statistics for simulation attitude (Euler, ZYX) from algo--Max error: [ 0.03683596 0.03072644 0.03834808] deg--Avg error: [ 0.01396051 0.01227379 -0.00541937] deg--STD of error: [ 0.01401866 0.01539313 0.01432695] deg----------------statistics for simulation position from algo--Max error: [ 0.10531077 0.20204374 0.05338844] m--Avg error: [-0.02886209 -0.07964527 0.02510121] m--STD of error: [ 0.06367006 0.08720146 0.01585162] m----------------statistics for simulation velocity from algo--Max error: [ 0.03322665 0.04136518 0.01040273] m/s--Avg error: [-0.01493868 -0.00902974 0.00649778] m/s--STD of error: [ 0.01853551 0.01721704 0.00252517] m/s
