[DPRG] Single Axis IMU
Subject: [DPRG] Single Axis IMU
From: David P. Anderson
dpa at io.isem.smu.edu
Date: Mon Jun 24 17:06:38 CDT 2002
Howdy Ed,
I've been working on a homebrew inertial measurement sensor
for the twowheel balancing robot, to replace the (expensive)
commercial one I am currently using.
The sensor consists of two devices: a single axis rate gyro, and
a two axis accelerometer (ADXL202). The accelerometer provides
accurate tilt angles when it is static, but the tilt signal is
swamped by the accelerations of the platform when it is in motion.
The rate gyro is integrated to provide tilt angles, and it provides
accurate shortterm angles (hifreq) but the bias drift and the
drift inherent in the integration means that the longterm stability
is not reliable.
The commercial unit I have (same two sensors) uses a strong lowpass
filter on the accelerometer to remove transients, and a strong hipass
filter on the integrated gyro position, to remove the drifting DC bias
offset. The two are combined with a crossover fq of about 1 hz. The
tech I spoke with said they are using a "Weiner" filter.
There is a paper Larry Barello (SRS) found that describes this technique,
more or less, at:
http://www.geology.smu.edu/~dpawww/robo/balance/inertial.pdf
In the meantime, I've been able to interest the folks at the sourceforge
autopilot project (autonomous helicopter) in this problem. They have a
similar if more difficult problem, with 3axis where we only need one.
They are solving the problem with a Kalman filter, and have provided a
simulation in MatLab and C, attached below. I'm not quite sure what
to do with this, but seems like they have pointed in the right direction.
what do you think?
regards,
dpa

/* * indenttabsmode:T; cbasicoffset:8; tabwidth:8; * vi: set ts=8:
* $Id: imu1d.c,v 1.5 2002/06/24 20:44:05 tramm Exp $
*
* (c) Aaron Kahn <Aaron.Kahn at itt.com>
* (c) Trammell Hudson <hudson at rotomotion.com>
*
* One dimensional IMU to compute pitch angle based on the readings
* from an ADXL202 and a pitch gyro.
*
* plot [200:1000] \
* "kalman.log" using 1 title "Actual state" with lines, \
* "" using 2 title "Estimated state" with lines, \
* "" using 3 title "Error" with lines
*
*************
*
* This file is part of the autopilot simulation package.
*
* Autopilot is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* Autopilot is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Autopilot; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 021111307 USA
*
*/
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#define pi 3.14159
const double dt = 1.0 / 30.0; /* 30 Hz sensor update */
const double omega = 0.5; /* Max roll rate */
const double Q = 0.0001; /* Noise weighting matrix */
const double max_angle = 30*pi/180.0; /* Maximum pitch angle */
static double theta = 4*pi/180; /* Our initial state estimate */
static double R = 1; /* Measurement error weight */
static double P = 1; /* Covariance matrix */
double
kalman(
double t, /* Time */
double q, /* Pitching gyro */
double ax, /* X acceleration */
double ay /* Y acceleration */
)
{
double Pdot; /* Derivative of P */
double E; /* ? */
double K; /* ? */
double theta_m; /* Our state measurement */
/* A = 0 */
Pdot = Q; /* Pdot = A*P + P*A' + Q */
P += Pdot * dt;
/* Update our state estimate from the rate gyro */
theta += q * dt;
/* We only run the Kalman filter at a slower 10 Hz */
if( (int)( t * 100 ) % 10 != 0 )
return theta;
/* Compute our measured state from the accelerometers */
theta_m = atan2( ay, ax );
E = P + R; /* E = CPC' + R */
K = P / E; /* K = PC'inv(E) */
/* Update the state */
theta += K * ( theta_m  theta );
/* Covariance update */
P *= ( 1  K );
return theta;
}
static inline double
noise( void )
{
return 2.0 * drand48()  1.0;
}
int main( void )
{
double t;
srand48( time(0) );
for( t=0 ; t<60.0 ; t+=dt )
{
/*
* Compute our actual state as a function of time.
*/
double real_q = max_angle * omega * cos(omega*t);
double real_theta = max_angle * sin(omega*t);
/*
* Fake our sensor readings by adding a little bit of noise
* to the system.
*/
double ax = cos( real_theta ) + noise() * 0.9;
double ay = sin( real_theta ) + noise() * 0.9;
double q = real_q + noise() * 8 * pi / 180.0;
/*
* Compute our new estimated state with the Kalman filter
*/
double theta = kalman( t, q, ax, ay );
printf( "%f %f %f\n",
real_theta,
theta,
real_theta  theta
);
}
return 0;
}
More information about the DPRG mailing list
