# Low cost MEMs accelerometers, gyroscopes and magnetometers --> 3D Posisition and attitude

Hi,
I am an amateur robotics fanatic and I need an expert advise. I have a 3D accelerometers and three 1D gyros connected and a 3D
magnetic field sensor. These sensors attached on to a carrier printed circuit board (PCB) in such a way that I can observe (by oscillascope) that their output changes as I move/rotate the carrier board in 3D space.
Initially I leave the PCB in a predefined position on a table for calibration. After that I move the PCB in 3D space and I want to estimate the 3D position and orientation/attitude of the PCB at an arbitrary time.
Assume that we have the following declerations to store the initial, calibration values and the current values.
typedef struct { float AccV[3]; // Output voltages (AVx, AVy, AVz) of the acceleormeters float GyrV[3]; // Output voltages (GVx, GVy, GVz) of the gyros float MagV[3]; // Output voltages (MVx, MVy, MVz) of the magnetometers }MeasStruct; // Measurements
typedef struct { float Pos[3]; // Position float Vel[3]; // Velocities float Acc[3]; // Acceleration float Att[3]; // Attitude (Roll, Pitch, Yaw angles) }PVAAStruct; // Pos,Vel, Acc, Att
MeasStruct CalibrationMeasurements; PVAAStruct CalibrationPVAA;
MeasStruct CurrentMeasurements; PVAAStruct CurrentPVAA;
long int T =0; // Time counter int dT = 10; // Time increment in millisecond
CalibrationPVAA.Pos[0] = 0.0; // Position X CalibrationPVAA.Pos[1] = 0.0; // Position Y CalibrationPVAA.Pos[2] = 0.0; // Position Z
CalibrationPVAA.Vel[0] = 0.0; // Velocities X CalibrationPVAA.Vel[1] = 0.0; // Velocities Y CalibrationPVAA.Vel[2] = 0.0; // Velocities Z
CalibrationPVAA.Acc[0] = 0.0; // Acceleration X CalibrationPVAA.Acc[1] = 0.0; // Acceleration Y CalibrationPVAA.Acc[2] = 0.0; // Acceleration Z
CalibrationPVAA.Att[0] = 0.0; // Attitude Rol angle CalibrationPVAA.Att[1] = 0.0; // Attitude Pitch angle CalibrationPVAA.Att[2] = 0.0; // Attitude Yaw angle }
// How to calculate current Pos, Vel, Acc, Att values?
CurrentPVAA.Pos[0] = ??; // Position X CurrentPVAA.Pos[1] = ??; // Position Y CurrentPVAA.Pos[2] = ??; // Position Z
CurrentPVAA.Vel[0] = ??; // Velocities X CurrentPVAA.Vel[1] = ??; // Velocities Y CurrentPVAA.Vel[2] = ??; // Velocities Z
CurrentPVAA.Acc[0] = ??; // Acceleration X CurrentPVAA.Acc[1] = ??; // Acceleration Y CurrentPVAA.Acc[2] = ??; // Acceleration Z
CurrentPVAA.Att[0] = ??; // Attitude Rol angle CurrentPVAA.Att[1] = ??; // Attitude Pitch angle CurrentPVAA.Att[2] = ??; // Attitude Yaw angle }
} int main() { Init(); while (1) { UpdateState(); fprintf("Position=(%f, %f, %f), Attitude=(%f, %f, %f)",
CurrentPVAA.Pos[0],CurrentPVAA.Pos[1],CurrentPVAA.Pos[2],
CurrentPVAA.Att[0],CurrentPVAA.Att[1],CurrentPVAA.Att[2]);
} }
Question; Can you show me/us how to calculate position and attitude estimates of the PCB at any time step? i.e. Can you help me how to finalise the UpdateState() function?
Regards,
Miem Chan miemchan at gmail com
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>

Sure. The new velocity is the old velocity plus the acceleration times dT. The new position is the old position plus the velocity times dT. Just be careful with your units and make sure they all match properly.
Best, - Joe
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>

Thank you Joe,
But how to improve that approach by also using gyros and magnetometers?
Regards,
Miem
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>

I've got no experience with solving the problem but there are some well known approaches and algorithms. Here's some links to get you started:
http://en.wikipedia.org/wiki/Sensor_fusion
http://en.wikipedia.org/wiki/Kalman_filter
--
Curt Welch http://CurtWelch.Com /
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Miem wrote:

The technique for combining these sensors into a single orientation sensor is called an Extended Kalman Filter.
It's a little more involved than just adapting the code you have posted here. Take a look at the sourceforge autopilot project:
<http://autopilot.sourceforge.net
which has Kalman Filter source code for a home-brew 3-D inertial measurement unit.
best dpa
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Miem wrote:

This web site has some very good papers on this:
http://www-personal.umich.edu/~johannb /
Good Luck, Bob