Setting up a kalman filter to fuse Accelerometer, Gyroscope and GPS - Getting the parameters right

Dear Matlab community,

I am fairly new to Matlab (used it only in university a long time ago).

I have collected sensor data (Accelerometer, Gyroscope & GPS) with a smartphone and would like to perform a sensor fusion to show a path in 3D which is more detailed than just plotting the GPS data.

I have managed to set up the code based on a Matlab example. However I am really struggling with getting the initial parameters (Acceleration bias, gyro bias, State Covariance matrix...) and the sensor noises right.

Could somebody give me pointers on how to derive the correct values for these parameters from my collected sensor data?

Here's my setup for initialization:

%initialize fusionfilter and set initial state
filt = insfilter('AsyncIMU');
filt.IMUSampleRate = imuFs;
refloc = [SurfGPS(1,2:4)];
filt.ReferenceLocation = refloc;
% filt.VelocityNoise = [0.05 0.05 0.05];
% filt.AccelerationNoise = [0.05 0.05 0.05];
% filt.GyroscopeBiasNoise = [1e-12 1e-12 1e-12];
% filt.AccelerometerBiasNoise = [1e-12 1e-12 1e-12];

initstate = ones(28,1);
initstate(1:4) = [0.7071 0 0 0.7071];
initstate(5:7) = SurfGyro(1,2:4);
initstate(8:10) = SurfGPS(1,9:11);
initstate(11:13) =  SurfGPS(1, 12:14);
initstate(14:16) =  [0 0 0];
initstate(17:19) = [0.012 0.012 0.012];
initstate(20:22) = [0.012 0.012 0.012];
initstate(23:25) = [0 0 0];
% % 
filt.State = initstate;

And here are the noise values I take for predicting:

%set sensor noises
Rmag = 80;
Rvel = 0.64;
Racc  = 800;
Rgyro = 1e-4;
Rpos  = 5;

Here are my current results (blue line: fusion of sensors; red line: AsyncIMU with only GPS data fused):

I would be really grateful for tips and pointers on how to set up my system.

Answer 1

[Pat Williamson]

Hi Ingo Boegemann,

For your question specifically, it would be great to know which example you based this code on, as some of the variables you list are undefined in the code snippet, i.e. “SurfGPS” and “SurfGyro”. Also, I see you are using the “insfilter”, but you may have more success using the “insEKF” filter. See this documentation page for more information, especially the example toward the bottom.

Unfortunately, I don’t think I have enough information to fully answer your question as posed, but I think the information below can be helpful for working with estimation filters in general.

One approach to improving your estimation filter is to compare the filter's estimated state to a truth dataset. A truth dataset might mean simulated data, data collected with your sensors for a pre-determined motion, or data collected with a higher fidelity sensor system. By leveraging a truth dataset as a ground truth for tuning your filter, you can improve your confidence in the filter before using it on field data.

This Detect Multipath GPS Reading Errors Using Residual Filtering in Inertial Sensor Fusion example shows an approach for improving your filter settings by comparing to truth data for GPS and IMU readings.

For data collected using a cell phone, this Estimate Phone Orientation Using Sensor Fusion example walks through some steps for tuning the filter for GPS data from a cell phone. But, this is only using data for orientation-only estimation. This example leverages the “tune” function to tune the filter to the truth data.

To your question about Accelerometer and Gyro biases, these are based on the sensors themselves. They can be determined from the sensor data sheet or with a sensor calibration experiment.

I hope this helps!