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'});
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'});
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.
