insfilterMARG parameter tuning - Kalman filter

Hello,

I am trying to do sensor fusion for pose estimation using the insfilterMARG filter. I am somewhat following the structure of this example, in which I have an IMU object to simulate the sensor values (accelerometer, gyroscope and magnetometer). For the GPS signal I directly use the values in meters (plus some added noise) from the trajectory, which comes from a waypointtrajectory (along with the orientation).

The Kalman loop looks something like this:

ori_est = quaternion();

pos_est = zeros(n,3);

stateIdx = stateinfo(fuse);

for ii=1:n

fuse.predict(acc(ii,:),avel(ii,:));

fuse.fusemag(mag(ii,:),Rmag);

correct(fuse, stateIdx.Position, pos(ii,:), Rpos);

[pos_est(ii,:),ori_est(ii,:)] = pose(fuse);

end

The performance of this filter is rather good for ideal sensors (no noise) and after manually tweaking the process noise values after trying different values and evaluating the results. Using the tune function has resulted in no success so far (even for simpler models), so manually finding the values has been the only option. However when simulating a realistic environment where the sensors are read with noise, these process noise values become even more important, and it is hard to manually find the optimal combination.

Therefore I am trying to use the tune function to find the optimal process noise values:

sensor_data = table(acc,avel,mag,pos,NaN(size(acc)),'VariableNames',{'Accelerometer','Gyroscope','Magnetometer',...

'GPSPosition','GPSVelocity'}); % Sensor values, velocity not available

g_truth = table(ori_true,pos_true,vel_true,repmat(magfieldNED,n,1),zeros(n,3),zeros(n,3),zeros(n,3),...

'VariableNames',{'Orientation','Position','Velocity','GeomagneticFieldVector',...

'DeltaAngleBias','DeltaVelocityBias','MagnetometerBias'}); % true values from the waypoint trajectory, try with no bias first

measurenoise = tunernoise('insfilterMARG');

tune(fuse,measurenoise,sensor_data,g_truth);

The tune function is not converging (I tried adding more iterations and changing the step forward and backward values with a config object. The output of the tune function is as follows:

After it ends the orientation estimation is unusable, even worse than manually finding close to optimal values, which are still not good enough.

Is this the expected behavior? Or is there any error in my understanding/code? I can post the rest of the code if need be.

Thanks in advance.

Answer 1

[Brian Fanous]

It's hard to tell what's going on with only these code snippets. Can you post more?

Note that the tune() function will not find the initial state and state covariance for you. It will find the measurement noise and process noises. But you will have to give the filter a good starting initial state (the State property) and state covariance (StateCovariance property) before calling the tune() function.

[Pere Garau Burguera]

Thanks for your answer.

The issue was the initial state covariance. I had the initial state correctly, but had not come around to doing anything with the covariance. Right now I am just trying out different values (scaled eye()) and the results are definitely improving, but for more complicated models this might not be enough. I am not sure how to proceed with the state covariance initialization or if there is a way to obtain a more educated guess (at least for some values). For example, if we assume the initial state is rather good (which I am able to find), then the covariance matrix will be close to 0. But the actual values I don't know how to estimate them. Is there any good way to initialize the state covariance?