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.
2 个评论
BOUCHILAOUN Yacine
2022-5-5
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
2022-5-12
采纳的回答
Bhargavi Maganuru
2020-4-1
0 个投票
Hi,
You can go through following links.
更多回答（1 个）
Martin Seyr
2020-4-24
0 个投票
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
7 个评论
Linda Scarzanella
2020-7-8
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
2020-7-13
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
2020-7-13
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
2020-7-16
Swapnil Sayan Saha
2020-7-16
Andrea Gusmara
2020-9-1
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
2020-9-1
类别
在 帮助中心 和 File Exchange 中查找有关 Aerospace Applications 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!
Translated by 
