Last year, I introduced the Accelerated Innovation Community, sponsored by the MEMS Industry Group, to this space (see “Open source sensor fusion“). At the time, we were contributing our sensor fusion library to the MIG GitHub site. Kionix and PNI Sensor Corp. have contributed to the site, and today we are making another contribution in the form of our trajectory simulation library.
When my team first started exploring sensor fusion topics a few years ago, we needed some way to validate the results of the algorithms we were looking at. This was before the company had a gyro, and we didn’t have any kind of motion capture system available to us. So, we started investigating simulation capabilities. We wanted something where we could programmatically define a 6DOF trajectory of an object, including translation in X,Y,Z space, as well as full rotation. Then we needed sensor models that would take that simulated motion and generate simulated sensor readings. Those could then be used as inputs of our sensor fusion simulations. The outputs of those simulations could then be compared with the original trajectory orientation data to see how well we did.
We found a few tools in the literature, but could not use them for one reason or another. The IMUSim tool by A.D. Young, M.J. Ling and D.K. Arvind was of particular interest, but we had trouble replicating the required software environment. We needed something that could easily be transferred to other engineers, and decided to write our own MatlabTM library to do the job. The result was a library of routines that I collectively refer to as TSim (short for trajectory SIMulator).
The basic idea is to have a good library of functions and canned routines that can easily be pulled together to succinctly specify motion in six dimensions. The program listing below shows how I cobbled together the definition for the motion you see animated at the top of this posting.
% Copyright (c) 2012-2015, Freescale Semiconductor path(path, '../tool'); close all; clc; outputDir = 'animation_example1_outputs'; % constant definitions sample_rate = 100; % sensor sample rate ts = 1/sample_rate; % Define the environment env = Env(Env.ENU); r = 5*pi(); % Adata for this test is angular velocity % X Y Z Adata = [... 0.0, r, 0.0; 0.0, r, 0.0; 0.0, 0.0, 0.0; r , 0.0, 0.0; r , 0.0, 0.0; 0.0, 0.0, 0.0; 0.0, 0.0 r; 0.0, 0.0, r]; Atime = [0; 1; 3; 4; 6; 7; 8; 9]; % Pdata is position data % time X Y Z Pdata = [... 0.0, 0.0, 0.0; 0.0, 0.0, 0.0; 1.0, 0.0, 0.2; 1.0, 1.0, 0.4; 0.0, 1.0, 0.6; 0.0, 0.0, 0.8; 1.0, 0.0, 1.0; 1.0, 1.0, 1.2; 0.0, 1.0, 1.4; 0.0, 0.0, 1.6]; Ptime = 0:9; % Compute parameters for a low pass filter % Cutoff frequency=1Hz % frequency = 200Hz, 200 taps % This filter takes several seconds to run, but does a nice job of % ensuring that our waveforms look reasonable. % Note that it DOES introduce phase delay (which we don't care about) [ N, D ] = LPF( 1, 200, 200 ); t = CompositeTrajectory('Traj1'); t = t.set_av('linear', Atime, Adata); t = t.set_position('spline', Ptime, Pdata ); t = t.compute(0.01, 0.005, N, D); t.plot_all([1;1;1]); t.animate(5, 'animation_example1');
The listing includes two tables. One defines data points of position and time. The other specifies angular velocity versus time. You probably recall from your college physics 101 that acceleration is the derivative of velocity which is the derivative of position. Specify any one of the three, and the other two can (within a constant value) also be determined. Angular acceleration, velocity and orientation work the same way.
You might think that these tables are pretty crude, but that would be ignoring the power that Matlab gives us to interpolate between points. We used a spline function above to create the smooth trajectory shown below.
The basic components of a simulation are:
Because this is a very constrained problem, we’re able to encapsulate most of the work in the trajectory library itself. That short listing above was all it took to define the trajectory, dump sensor outputs and create the animation above.
The TSim library makes heavy use of object oriented programming. Trajectory object construction is shown above. Integration of those trajectories with sensor models is shown below.
The whole mess is documented in gory detail in the TSim user manual, which is included in the distribution, which you can download now at https://github.com/memsindustrygroup/TSim.
We used TSim as a testbench to characterize our 4.x sensor fusion libraries last year, and are planning to repeat the process for the recently announced 5.00 fusion libraries in coming months. Our goal is that by making the library open, we can encourage the sensor fusion community to use a common collection of stimulus/response datasets for characterization purposes.
And if you would like to know more about the MIG Accelerated Innovation Community, please just drop me a line. We’re always looking for new members.
Michael Stanley “works” on fun sensors and systems topics.