No usable results when using insEKF() instead of imufilter()

Part of what I want to do is fuse gyroscope and accelerometer data to get an orientation estimate. When I fuse the data using the imufilter() I get results looking very similar to what I am expecting, without majorly tweaking the noise settings and so on.

I then proceeded to try to fuse it using the insEKF() because I eventually also need to fuse GPS measurements. For the first part I only added the gyroscope and the accelerometer to see the orientation estimate. I coded it very similar to the example in the documentation but for some reason the estimated orientation just start spinning and jumping around "uncontrollably".

Heres part of how Ive implemented it:

I also tried tuning it using the data I got from the other filter but that not seems like the issue. Also fusing the data sequentially using fuse() didnt change anything.

...

gyrofix=[gyro(:,2) gyro(:,3) gyro(:,1)];    %data already in workspace
acclfix=[-accl(:,2) -accl(:,3) -accl(:,1)];


option = insOptions(ReferenceFrame="ENU");
insAccel=insAccelerometer;
insGyro=insGyroscope;

tt_meas=timetable(acclfix,gyrofix,'RowTimes', seconds(0:0.01:(size(acclfix,1)/100)-0.01))
tt_meas.Properties.VariableNames={'Accelerometer' 'Gyroscope'}
tt_meas.Accelerometer(:,:)=0;

filt = insEKF(insAccel, insGyro, option);

initOrient=quaternion(1, 0, 0, 0);
stateparts(filt,"Orientation",compact(initOrient));
statecovparts(filt,"Orientation",0.01);

mnoise=tunernoise(filt);
mnoise.AccelerometerNoise=0.0002;
mnoise.GyroscopeNoise=9e-5;
untunedEst = estimateStates(filt,tt_meas,mnoise);

[Umar]

Hi Christian,

Based on the analysis, here are some suggested modifications to the provided code snippet:

% Adjusted Noise Parameters
mnoise.AccelerometerNoise = 0.0002;
mnoise.GyroscopeNoise = 9e-5;
% Updated State Estimation
tunedEst = estimateStates(filt, tt_meas, mnoise);
% Additional Fusion Steps (if needed)
% fusedEst = fuse(filt, additionalData, additionalNoise);

Following suggestions in the above code snippet, the stability and accuracy of the orientation estimation using insEKF() can be significantly improved. Also, try continuously iterating on these adjustments while monitoring the behavior of the estimated orientation to achieve the desired performance.

If further assistance is needed or if specific issues persist, don't hesitate to provide additional details for a more targeted troubleshooting approach.

[Christian]

Hello Umar,

thanks for your quick response. Unfortunately the issue still persists no matter how I set the noise values. I added the files I am using.

imufilter_orientation.m is using the imufilter() and insEKF_orientation_tune.m the insEKF() (ignore the tuning parts that I commented out).

[Umar]

Hi Christian,

I would like Walter Robertson’s opinion on these files because he is actively participating in all my postings. So, let us wait to hear back about his solutions to the attached files. He should be able to provide response within a day.

[Umar]

Hi Christian,

In the meantime, could you please let me know which one of these files is causing the issue regarding the noise values, any details that you want to share about errors observed when executing these files, because more details you share will help resolve problem in better way.

[Umar]

Hi Christian,

I need your help to share your code snippets in order to resolve your issue. The issue that I am running into is regarding to insOptions requires one of the following:Sensor Fusion and Tracking Toolbox and Navigation Toolbox while executing “insEKF_orientation_tune.m” as shown in the attached screenshot.

However, I did notice the following while executing the file

Warning: Variable 'qmean' originally saved as a quaternion cannot be instantiated as an object and will be read in as a uint32. > In insEKF_orientation_tune (line 1)

This warning lies in “load("orientation_truth.mat");”

To fix this warning, you can utilize function like quaternion to convert the variable appropriately. Here's an example of how you can convert a uint32 variable to a quaternion in Matlab:

% Assuming qmean_uint32 is the uint32 variable

qmean_quaternion = quaternion(qmean_uint32);

By converting 'qmean' to a quaternion object before passing it to the function, you can resolve the instantiation issue and ensure proper handling of the variable within the insEKF_orientation_tune function. Also, it can affect the spinning and jumping of the estimated orientation, so make sure you properly initialize and process sensor data. Another thing, I can think of handling of sensor measurements in your code needs to be revised. For more information on quaternion function, please refer to:

https://www.mathworks.com/help/robotics/ref/quaternion.html

Also,I did get chance to review “insEKF_orientation_tune.m” ignoring comments, your code is loading sensor data, processes it using an Extended Kalman Filter (EKF) for Inertial Navigation System (INS) estimation, and then visualizing the estimated orientation. However, there are potential issues with the code that need to be addressed. It seems to lack proper initialization and tuning of the EKF parameters, which can lead to erratic behavior in the estimated orientation. Additionally, the loop for visualization may not be optimal for displaying the orientation accurately.

[Umar]

Hi Christian,

One more recommendation that I forgot to mention,

Added these lines of code in yours “insEKF_orientation_tune.m” to adjust the process noise and measurement noise for better parameter tuning.

mnoise.ProcessNoise = 0.01; % Adjust the process noise

mnoise.MeasurementNoise = 0.01; % Adjust the measurement noise

By adjusting the process and measurement noise values, the EKF parameters are tuned to provide more stable and accurate estimates of orientation, preventing erratic behavior in the estimated orientation output.

Sorry, I do not have access to Sensor Fusion and Tracking Toolbox and Navigation Toolbox but I am trying my best to help you out to achieve your goal. Hopefully, following my suggestions and feedback, you will be able to accomplish your goal now. Good luck!

Answer 1

[Brian Fanous]

Christian, do you have a ground truth recorded? Meaning, do you have logged data of what you want the filter output to look like? If so, you can try the tune() method on the insEKF to optimize the noise parameters. (You'll still have to handle initializing the filter states correctly).

If you are happy with the results from the imufilter, you could use that as your ground truth orientation. Be aware though that you'll be tuning the insEKF to also match the noise in the output of the imufilter in that case.

[Paul]

@Brian Fanous

Do you know why the properties of imufilter indicates that the units are rad, rad/sec, and m/sec^2, but the Algorithms section states the units are deg, deg/sec, and g?