Estimate position from inertial data

Can someone provide me an example of how kalman filters can be used to estimate position of an object from 6DOF/9DOF IMU data. All examples I have seen just seem to find orientation of the object using ahrs/imufilter.

[BOUCHILAOUN Yacine]

Hello

I have a question if you don't mind, I use data from an IMU sensor to determine positioning in the civil engineering field. I am using MATLAB code to do this. For the moment I don't have the right result compare by the real case.

Did you have a satisfactory result according to your studies?

[Swapnil Sayan Saha]

Yes. https://www.researchgate.net/publication/360075622_TinyOdom_Hardware-Aware_Efficient_Neural_Inertial_Navigation

Answer 1

[Bhargavi Maganuru]

Hi,

You can go through following links.

https://www.mathworks.com/help/fusion/inertial-sensor-fusion.html

https://www.mathworks.com/help/fusion/ref/imufilter-system-object.html#mw_8d99f490-b04c-46e5-b3ae-e34474c79714

https://www.mathworks.com/help/fusion/examples/estimate-orientation-through-inertial-sensor-fusion.html

[Swapnil Sayan Saha]

These either require GPS or uses IMU to give orientation only, not position. Was looking for something that gives position without GPS.

[Bhargavi Maganuru]

You can find below links helpful!

https://www.mathworks.com/discovery/kalman-filter.html

https://www.mathworks.com/help/control/getstart/estimating-states-of-time-varying-systems-using-kalman-filters.html

[Swapnil Sayan Saha]

https://www.mathworks.com/help/fusion/ref/insfiltermarg.html << This one to be more precise. Thanks.

Answer 2

[Martin Seyr]

hello,

you need to integrate the accelerometers if you want to calculate linear positions. this will be subject to quadratic error propagation over time, so it is necessary to periodically reset the integrator.

it works like this: you use the orientation calculated from the fusion algorithm (kalman filter or some other algorithm) to rotate locally measured accelerations into the world frame. then you subtract nominal gravity, then you integrate twice.

good luck,

Martin

[Linda Scarzanella]

Hi, I've tried to write a code as you explained. The porblem is that the values of displacement and velocities for the z are completely wrong. Any idea why? I'll attach here the code:

clc 
clear all
close all

% pos = [0 0 0];
delete(instrfindall);
%serialportlist("available")'
a = serial("COM7", "BaudRate", 9600);
gyroReadings = [0 0 0];
time = 0;
fopen(a);

g = 9.81
accelReadings = [0 0 -9.81];


Fs = 200;
tic;
for i=1:21
data = fscanf(a);
[ax ay az gx gy gz] = strread(data, '%f %f %f %f %f %f ','delimiter',';');
toc;
gx = gx*pi/180;
gy = gy*pi/180;
gz = gz*pi/180;
accelReadings = [accelReadings; ax ay az];
gyroReadings = [gyroReadings; gx gy gz];
time = [time; toc];

end
decim = 1
fuse = imufilter('SampleRate',Fs,'DecimationFactor',decim);
% [orientation,angularVelocity] = FUSE()
q = fuse(accelReadings,gyroReadings)
time_1 = (0:decim:size(accelReadings,1)-1)/Fs;

plot(time_1,eulerd(q,'ZYX','frame'))
title('Orientation Estimate')
legend('Z-axis', 'Y-axis', 'X-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')

%convert Acceleration to World Ref Frame
rotatedAcceleration = rotatepoint(q,accelReadings)

AccelWithoutGravity = [rotatedAcceleration(:,1) rotatedAcceleration(:,2) rotatedAcceleration(:,3)-g];


fs = 200; % Sampling Rate

fc = 0.1/30;  % Cut off Frequency

order = 6; % 6th Order Filter

%%Filter  Acceleration Signals

[b1 a1] = butter(order,fc,'high');

accf=filtfilt(b1,a1,AccelWithoutGravity );

figure (2)

plot(time,accf,'r'); hold on

plot(time,AccelWithoutGravity)

xlabel('Time (sec)')

ylabel('Acceleration (mm/sec^2)')


%First Integration (Acceleration - Veloicty)

velocity=cumtrapz(time,accf);

figure (3)

plot(time,velocity)

xlabel('Time (sec)')

ylabel('Velocity (mm/sec)')
legend('Z-axis', 'Y-axis', 'X-axis')

%%Filter  Veloicty Signals

[b2 a2] = butter(order,fc,'high');

velf = filtfilt(b2,a2,velocity);

%%Second Integration   (Velocity - Displacement)

Displacement=cumtrapz(time, velf);

figure(4)

plot(time,Displacement)

xlabel('Time (sec)')

ylabel('Displacement (mm)');

legend('Z-axis', 'Y-axis', 'X-axis')

 

[Martin Seyr]

haven't gone through all of the code, to be honest.

Few comments:

I think you shouldn't filter accelerations before you integrate them, at least not if it's not a linear filter.

Initialisation of the acceleration before the loop indicates that you have the signs wrong - in ENU notation, the acceleration reading of a sensor under gravity, that is at rest, will be +9.81m/s^2, not -9.81.

What also strikes me as a bit odd is that you use tic/toc for the time increment... the fusion filter and the integrator should be running at a fixed time. is this code for a real-time application?

[Linda Scarzanella]

I tried not to filter the acceleration, but I get noisy signal!

About the notation of acceleration: I use the orientation to get acceleration in the world reference frame.. is that wrong ? I need to remove the gravity also

Yes, I want to track the position of the tip of an endoscope for my thesis project.I need to record and display real-time (or display later) the position of the sensor/tip.

I'm pretty new to everything!!

[Swapnil Sayan Saha]

https://www.mathworks.com/help/fusion/ref/insfiltermarg.html This is what I was looking for.

[Swapnil Sayan Saha]

Although it includes GPS but GPS is not really needed, though errors are likely rise in the long run without GPS.

[Andrea Gusmara]

HI linda , have you finally found the solution of the wrong position problem ? Beacause I found myself in the same problem for my thesis.

[Swapnil Sayan Saha]

It's best to not dead-reckon in z axis. I have observed similar phenomena from real-world data. In my MS thesis I am discussing the implications of inaccurate z axis localization. Better option is to use a pressure sensor or some other sensor (e.g. GPS if available, acoustics, Radar etc.)