Simulate motion for sensor fusion

Simulate motion for sensor fusion

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.

construction2

The basic components of a simulation are:

  • Define the environment: temperature, air pressure, magnetic field, etc. These can often be left at default values.
  • Create a trajectory object to manage the simulation.
  • Define movement and rotation of an object within the environment, link them to the trajectory object.
  • Tell the trajectory object to smooth the trajectory as shown above.
  • Tell the trajectory object to compute sensor outputs based on the simulated movement and environment.
  • Tell the trajectory object to dump outputs.

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.

ClassHierarchy1

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.

ClassHierarchy2

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.

 

Michael Stanley
Michael Stanley
Mike Stanley develops advanced algorithms and applications for MCUs and sensors, including sensor fusion and sensor data analytics. He is a founding member of the MEMS Industry Group’s Accelerated Innovation Community and a contributor to the IEEE Standard for Sensor Performance Parameter Definitions (IEEE 2700-2014). He is co-author of a chapter on intelligent sensors in “Measurement, Instrumentation, and Sensors Handbook” (volume two), and speaks on sensor topics. When the Arizona temperature drops below 100 degrees, you'll find Mike flying his F450 quadcopter . Follow him @SensorFusion.

Comments are closed.

Buy now