  DPRG List

 [DPRG] Single Axis IMU Message index sorted by: [ date ] [ thread ] [ subject ] [ author ] Previous message: [DPRG] Using a mouse for logistics Next message: [DPRG] Robot math problem 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 home-brew inertial measurement sensor for the two-wheel 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 short-term angles (hi-freq) but the bias drift and the drift inherent in the integration means that the long-term stability is not reliable. The commercial unit I have (same two sensors) uses a strong low-pass filter on the accelerometer to remove transients, and a strong hi-pass 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/~dpa-www/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 3-axis 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 ------------------------------------------------------------------------ /* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8: * \$Id: imu-1d.c,v 1.5 2002/06/24 20:44:05 tramm Exp \$ * * (c) Aaron Kahn * (c) Trammell Hudson * * 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 02111-1307 USA * */ #include #include #include #include #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; } ``` Previous message: [DPRG] Using a mouse for logistics Next message: [DPRG] Robot math problem Message index sorted by: [ date ] [ thread ] [ subject ] [ author ] More information about the DPRG mailing list