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
void Init() { CalibrationMeasurements.AccV[0] = ReadADC(AccVxChannel); CalibrationMeasurements.AccV[1] = ReadADC(AccVyChannel); CalibrationMeasurements.AccV[2] = ReadADC(AccVzChannel);
CalibrationMeasurements.GyrV[0] = ReadADC(GyrVxChannel); CalibrationMeasurements.GyrV[1] = ReadADC(GyrVyChannel); CalibrationMeasurements.GyrV[2] = ReadADC(GyrVzChannel);
CalibrationMeasurements.MagV[0] = ReadADC(MagVxChannel); CalibrationMeasurements.MagV[1] = ReadADC(MagVyChannel); CalibrationMeasurements.MagV[2] = ReadADC(MagVzChannel);
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 }
void UpdateState() { T = T + dT; CurrentMeasurements.AccV[0] = ReadADC(AccVxChannel); CurrentMeasurements.AccV[1] = ReadADC(AccVyChannel); CurrentMeasurements.AccV[2] = ReadADC(AccVzChannel);
CurrentMeasurements.GyrV[0] = ReadADC(GyrVxChannel); CurrentMeasurements.GyrV[1] = ReadADC(GyrVyChannel); CurrentMeasurements.GyrV[2] = ReadADC(GyrVzChannel);
CurrentMeasurements.MagV[0] = ReadADC(MagVxChannel); CurrentMeasurements.MagV[1] = ReadADC(MagVyChannel); CurrentMeasurements.MagV[2] = ReadADC(MagVzChannel);
// 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
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload

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
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload

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

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 /
snipped-for-privacy@kcwc.com http://NewsReader.Com /
  Click to see the full signature.
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload
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
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload
Miem wrote:

This web site has some very good papers on this:
http://www-personal.umich.edu/~johannb /
Good Luck, Bob
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload

Polytechforum.com is a website by engineers for engineers. It is not affiliated with any of manufacturers or vendors discussed here. All logos and trade names are the property of their respective owners.