Autopilot-CVS: sim/src/imu-filter ahrs.cpp,2.0,2.1
Status: Alpha
Brought to you by:
tramm
Menu
▾
▴
Autopilot-CVS: sim/src/imu-filter ahrs.cpp,2.0,2.1
|
From: Trammell H. <tr...@us...> - 2003-02-28 06:39:29
|
Update of /cvsroot/autopilot/sim/src/imu-filter
In directory sc8-pr-cvs1:/tmp/cvs-serv21783
Modified Files:
ahrs.cpp
Log Message:
Simple IMU and AHRS code for testing 2.4 board
Index: ahrs.cpp
===================================================================
RCS file: /cvsroot/autopilot/sim/src/imu-filter/ahrs.cpp,v
retrieving revision 2.0
retrieving revision 2.1
diff -u -d -r2.0 -r2.1
--- ahrs.cpp 22 Sep 2002 02:07:31 -0000 2.0
+++ ahrs.cpp 28 Feb 2003 06:39:25 -0000 2.1
@@ -2,11 +2,8 @@
* $Id$
*
* (c) Trammell Hudson
- * (c) Aaron Kahn
*
- * AHRS simulator based on Kalman filtering of the gyro and
- * accelerometer data. Converted from Aaron's matlab code
- * to use the C++ math library.
+ * Simple program to test the IMU and AHRS codes
*
**************
*
@@ -27,473 +24,112 @@
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
-#include <mat/Vector.h>
-#include <mat/Matrix.h>
-#include <mat/Matrix_Invert.h>
-#include <mat/Quat.h>
-#include <mat/Nav.h>
-#include <mat/Conversions.h>
-#include <mat/Kalman.h>
-
-#include <state/state.h>
-#include <state/Server.h>
+#include <imu-filter/AHRS.h>
+#include <imu-filter/IMU.h>
#include <iostream>
-#include <cmath>
-#include <cstdio>
+#include <fstream>
#include <vector>
#include <string>
-
+#include <mat/Conversions.h>
#include "macros.h"
#include "read_line.h"
#include "timer.h"
-using namespace util;
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+
using namespace libmat;
-using namespace libstate;
+using namespace imufilter;
using namespace std;
+using namespace util;
+const double dt = 1024 * 256 / 8000000.0;
+int verbose = 1;
-class imu_t
-{
-public:
- Vector<8> samples;
- Vector<3> accel;
- Vector<3> pqr;
-};
-
-
-istream &
-operator >> (
- istream & in,
- imu_t & imu
+int
+main(
+ int argc,
+ const char ** argv
)
{
- char line[256];
- const double accel_scale = 9.78 / 6835.0;
- const double gyro_scale = 3.14159 / 6000.0;
-
- while( in.getline( line, sizeof(line), '\n' ) )
- {
-
- // Skip any line that doesn't begin with GPADC
- if( strncmp( line, "$GPADC", 6 ) != 0 )
- continue;
-
- vector<string> values( split( line, ',' ) );
-
- // Convert from hex to raw samples
- FOR_ALL_CONST( vector<string>, val, values,
- if( val_index == 0 )
- continue;
- if( val_index > 8 )
- break;
+ const char * serial_dev = "/dev/ttyS1";
+ int samples = 0;
- imu.samples[val_index-1] = strtol(
- val->c_str(),
- 0,
- 16
- );
- );
+ if( argc > 1 )
+ serial_dev = argv[1];
- // Scale the raw samples to inertial measurements
- imu.accel[0] = (imu.samples[3] - 16461.0) * accel_scale;
- imu.accel[1] = (imu.samples[4] - 17537.0) * accel_scale;
- imu.pqr[0] = (imu.samples[7] - 20345.0) * gyro_scale;
- imu.pqr[1] = (imu.samples[6] - 10782.0) * gyro_scale;
- imu.pqr[2] = (imu.samples[5] - 0.0) * gyro_scale;
+ ifstream serial_port( serial_dev, ios::in );
- // Return the stream
- return in;
+ if( !serial_port )
+ {
+ cerr << "Unable to open " << serial_dev << endl;
+ return -1;
}
- return in;
-}
-
-
-
-class kalman_state_t
-{
-public:
- // Quaternion state estimate
- Vector<4> q;
-
- // Gyro bias estimation
- Vector<3> bias;
-};
-
-
-class ahrs_t
-{
-public:
- Vector<3> theta;
- Vector<3> pqr;
- Vector<3> bias;
-};
-
-
-static int
-do_nothing(
- const string & UNUSED( line )
-)
-{
- return 0;
-}
-
-
-static void
-write_to_clients(
- Server & server,
- const imu_t & imu,
- const ahrs_t & ahrs
-)
-{
- state_t state;
-
- gettimeofday( &state.time, 0 );
-
- state.ax = imu.accel[0];
- state.ay = imu.accel[1];
- state.az = 9.81;
-
- state.p = ahrs.pqr[0];
- state.q = ahrs.pqr[1];
- state.r = ahrs.pqr[2];
-
- state.x = 0;
- state.y = 0;
- state.z = -5;
-
- state.phi = ahrs.theta[0];
- state.theta = ahrs.theta[1];
- state.psi = 0; // ahrs.theta[2];
-
- state.vx = 0;
- state.vy = 0;
- state.vz = 0;
-
- state.mx = 0;
- state.my = 0;
-
- server.send_state( &state );
-
-}
-
-
-
-
-void
-make_a_matrix(
- const imu_t & imu,
- const kalman_state_t & x,
- Matrix<6,6> & A
-)
-{
- const double P = (imu.pqr[0] - x.bias[0]) / 2.0;
- const double Q = (imu.pqr[1] - x.bias[1]) / 2.0;
-
- A[0][0] = 0;
- A[0][1] = -P;
- A[0][2] = -Q;
- A[0][3] = 0;
- A[0][4] = x.q[1] / 2.0;
- A[0][5] = x.q[2] / 2.0;
-
- A[1][0] = P;
- A[1][1] = 0;
- A[1][2] = 0;
- A[1][3] = -Q;
- A[1][4] = -x.q[0] / 2.0;
- A[1][5] = x.q[3] / 2.0;
-
- A[2][0] = Q;
- A[2][1] = 0;
- A[2][2] = 0;
- A[2][3] = P;
- A[2][4] = -x.q[3] / 2.0;
- A[2][5] = -x.q[0] / 2.0;
-
- A[3][0] = 0;
- A[3][1] = Q;
- A[3][2] = -P;
- A[3][3] = 0;
- A[3][4] = x.q[2] / 2.0;
- A[3][5] = -x.q[1] / 2.0;
-
- A[4][0] = 0;
- A[4][1] = 0;
- A[4][2] = 0;
- A[4][3] = 0;
- A[4][4] = 0;
- A[4][5] = 0;
-
- A[5][0] = 0;
- A[5][1] = 0;
- A[5][2] = 0;
- A[5][3] = 0;
- A[5][4] = 0;
- A[5][5] = 0;
-}
-
-
-void
-PropagateState(
- const imu_t & imu,
- kalman_state_t & x,
- double dt
-)
-{
- const Vector<3> pqr( (imu.pqr - x.bias) / 2.0 );
-
- const Vector<4> Xdot(
- -pqr[0] * x.q[1] - pqr[1] * x.q[2],
- pqr[0] * x.q[0] - pqr[1] * x.q[3],
- pqr[0] * x.q[3] + pqr[1] * x.q[0],
- -pqr[0] * x.q[2] - pqr[1] * x.q[1]
- );
-
- x.q += Xdot * dt;
- x.q.norm_self();
-}
-
-
-void
-PropagateCovariance(
- const Matrix<6,6> & A,
- Matrix<6,6> & P,
- const Matrix<6,6> & Q,
- double dt
-)
-{
- const Matrix<6,6> Pdot(
- A * P + P * A.transpose() + Q
- );
-
- P += Pdot * dt;
-
-#if 0
- Matrix<6,6> Pdot( P * A );
- Pdot += P * A.transpose();
- Pdot += Q;
-
- Pdot *= dt;
- P += Pdot;
-#endif
-}
-
-
-/**
- * Convert acceleration measurements to estimated angles.
- * Z acceleration is unused and no heading is assumed.
- */
-const Vector<3>
-accel2euler(
- const Vector<3> & accel
-)
-{
- const double g = 9.78;
-
- return Vector<3>(
- -asin( limit( -accel[0] / g, -1, 1 ) ), // Roll
- -asin( limit( -accel[1] / g, -1, 1 ) ), //theta
- 0.0 // psi
- );
-}
-
-
-void
-AttitudeUpdate(
- imu_t & imu,
- kalman_state_t & x,
- Matrix<6,6> & P,
- const Matrix<2,2> & R
-)
-{
- double err;
-
- // compute the euler angles from the accelerometers
- const Vector<3> THETAm( accel2euler( imu.accel ) );
-
-
- // compute the euler angles from the state estimate
- const Vector<3> THETAe( quat2euler( x.q ) );
-
- // compute the DCM matrix
- const Matrix<3,3> DCM( quatDC( x.q ) );
-
- // make the C matrix
- Matrix<2,6> C( 0.0 );
-
- // PHI section
- err = 2.0 / ( sqr(DCM[2][2]) + sqr(DCM[1][2]) );
-
- C[0][0] = ( x.q[1]*DCM[2][2] ) * err;
- C[0][1] = ( x.q[0]*DCM[2][2] + 2.0*x.q[1]*DCM[1][2] ) * err;
- C[0][2] = ( x.q[3]*DCM[2][2] + 2.0*x.q[2]*DCM[1][2] ) * err;
- C[0][3] = ( x.q[2]*DCM[2][2] ) * err;
-
- // THETA section
- err = -1.0 / sqrt(1.0 - sqr(DCM[0][2]) );
-
- C[1][0] = -2.0 * x.q[2] * err;
- C[1][1] = 2.0 * x.q[3] * err;
- C[1][2] = -2.0 * x.q[0] * err;
- C[1][3] = 2.0 * x.q[1] * err;
-
-
- // compute the error; this should be ( THETAm - THETAe ),
- // but we can only use the pitch and roll angles here
- Vector<2> eTHETA;
- eTHETA[0] = THETAm[0] - THETAe[0];
- eTHETA[1] = THETAm[1] - THETAe[1];
-
- // We throw away the K result
- Matrix<6,2> K;
-
- // Kalman() wants a vector, not an object. Serialize the
- // state data into this vector, then extract it out again
- // once we're done with the loop.
- Vector<6> X_vect;
-
- X_vect[0] = x.q[0];
- X_vect[1] = x.q[1];
- X_vect[2] = x.q[2];
- X_vect[3] = x.q[3];
-
- X_vect[4] = x.bias[0];
- X_vect[5] = x.bias[1];
-
- Kalman(
- P,
- X_vect,
- C,
- R,
- eTHETA,
- K
- );
-
- x.q[0] = X_vect[0];
- x.q[1] = X_vect[1];
- x.q[2] = X_vect[2];
- x.q[3] = X_vect[3];
-
- x.bias[0] = X_vect[4];
- x.bias[1] = X_vect[5];
-
- x.q.norm_self();
-}
-
-
-
-void
-ahrs_run(
- Server & server,
- imu_t & imu,
- ahrs_t & ahrs
-)
-{
- double dt = 30.0 * 1024.0 / 1000000.0;
- Matrix<6,6> P( eye<6,double>() );
- Matrix<6,6> Q( 0.0 );
- Matrix<2,2> R( 0.0 );
- Matrix<6,6> A;
-
- kalman_state_t x;
- Vector<3> angles(
- asin( limit( -imu.accel[1] / 9.78, -1, 1 ) ),
- -asin( limit( -imu.accel[0] / 9.78, -1, 1 ) ),
- 0.0
- );
-
- Q[0][0] = 0.0001;
- Q[1][1] = 0.0001;
- Q[2][2] = 0.0001;
- Q[3][3] = 0.0001;
- Q[4][4] = 0.03;
- Q[5][5] = 0.03;
-
- R[0][0] = 0.3;
- R[1][1] = 0.3;
-
-
- x.q = euler2quat( angles );
-
- /*
- * Take our initial reading, assume that the vehicle is still
- */
- cin >> imu;
- x.bias = imu.pqr;
-
-
- // Read samples and run the filter
- double runtime = 0;
- int samples = 0;
+ IMU imu;
+ AHRS ahrs( dt );
- while( cin >> imu )
+ while( 1 )
{
- stopwatch_t timer;
-
- start( &timer );
-
- make_a_matrix( imu, x, A );
-
- /* Propagate the state estimate */
- PropagateState( imu, x, dt );
-
- /* Propagate the coverience matrix */
- PropagateCovariance( A, P, Q, dt );
-
- /* Kalman Update */
- AttitudeUpdate( imu, x, P, R );
-
- /* compute angles from quaternions */
- angles = quat2euler( x.q );
-
- ahrs.theta = angles;
- ahrs.bias = x.bias;
- ahrs.pqr = imu.pqr - x.bias;
-
- runtime += stop( &timer );
- samples++;
-
- cout
- << "theta=" << ahrs.theta
- << " bias=" << ahrs.bias
- << " pqr=" << ahrs.pqr
- //<< " accel=" << imu.accel
- << endl;
+ const string s = read_line( serial_port );
+ const char *line = s.c_str();
- if( isnan( ahrs.theta[0] ) )
+ if( strncmp( line, "$GPADC", 6 ) != 0 )
{
- cerr << "NaN attitude estimate!" << endl;
- break;
+ cerr << "Unknown line: " << line << endl;
+ continue;
}
- write_to_clients( server, imu, ahrs );
-
- server.read_lines(
- do_nothing,
- 1
- );
- }
+ imu.update( line );
+ if( verbose )
+ {
+ printf( "\nAccel: % 3.4f % 3.4f % 3.4f\n",
+ imu.accel[0],
+ imu.accel[1],
+ imu.accel[2]
+ );
+ printf( "PQR: % 3.4f % 3.4f % 3.4f\n",
+ imu.pqr[0] * C_RAD2DEG,
+ imu.pqr[1] * C_RAD2DEG,
+ imu.pqr[2] * C_RAD2DEG
+ );
+ }
- cerr << "Processed "
- << samples << " samples in "
- << runtime / 1000000.0 << " seconds: "
- << samples * 1000000.0 / runtime << " samples/sec"
- << endl;
-}
+ if( samples++ == 0 )
+ {
+ ahrs.initialize(
+ imu.accel,
+ imu.pqr,
+ 0
+ );
+ } else {
+ ahrs.imu_update(
+ imu.accel,
+ imu.pqr
+ );
+ }
-int main()
-{
- imu_t imu;
- ahrs_t ahrs;
- Server server( 2002 );
+ if( verbose )
+ {
+ printf( "PQR2: % 3.4f % 3.4f % 3.4f\n",
+ ahrs.pqr[0] * C_RAD2DEG,
+ ahrs.pqr[1] * C_RAD2DEG,
+ ahrs.pqr[2] * C_RAD2DEG
+ );
- set_state_dt( 32768.0 / 1000000.0 );
+ printf( "Angls: % 3.4f % 3.4f % 3.4f\n",
+ ahrs.theta[0] * C_RAD2DEG,
+ ahrs.theta[1] * C_RAD2DEG,
+ ahrs.theta[2] * C_RAD2DEG
+ );
+ }
+ }
- ahrs_run( server, imu, ahrs );
+ return 0;
}
|
