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