/***************************************************************************/ /* kalman.c */ /* 1-D Kalman filter Algorithm, using an inclinometer and gyro */ /* Author: Rich Chi Ooi */ /* Version: 1.0 */ /* Date:30.05.2003 */ /* Adapted from Trammel Hudson(hudson@rotomotion.com) */ /* ------------------------------- */ /* Compilation procedure: */ /* Linux */ /* gcc68 -c XXXXXX.c (to create object file) */ /* gcc68 -o XXXXXX.hex XXXXXX.o ppwa.o */ /*Upload data : */ /* ul filename.txt */ /***************************************************************************/ /* In this version: */ /***************************************************************************/ /* This is a 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. */ /* */ /* this code 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 "eyebot.h" /* * The state is updated with gyro rate measurement every 20ms * change this value if you update at a different rate. */ static const float dt = 0.02; /* * The covariance matrix.This is updated at every time step to * determine how well the sensors are tracking the actual state. */ static float P[2][2] = {{ 1, 0 }, { 0, 1 }}; /* * Our two states, the angle and the gyro bias.As a byproduct of computing * the angle, we also have an unbiased angular rate available.These are * read-only to the user of the module. */ float angle; float q_bias; float rate; /* * The R represents the measurement covariance noise.R=E[vvT] * In this case,it is a 1x1 matrix that says that we expect * 0.1 rad jitter from the inclinometer. * for a 1x1 matrix in this case v = 0.1 */ static const float R_angle = 0.001 ; /* * Q is a 2x2 matrix that represents the process covariance noise. * In this case, it indicates how much we trust the inclinometer * relative to the gyros. */ static const float Q_angle = 0.001; static const float Q_gyro = 0.0015; /* * state_update is called every dt with a biased gyro measurement * by the user of the module. It updates the current angle and * rate estimate. * * The pitch gyro measurement should be scaled into real units, but * does not need any bias removal. The filter will track the bias. * * A = [ 0 -1 ] * [ 0 0 ] */ void stateUpdate(const float q_m){ float q; float Pdot[4]; /* Unbias our gyro */ q = q_m - q_bias; /* * Compute the derivative of the covariance matrix * (equation 22-1) * Pdot = A*P + P*A' + Q * */ Pdot[0] = Q_angle - P[0][1] - P[1][0]; /* 0,0 */ Pdot[1] = - P[1][1]; /* 0,1 */ Pdot[2] = - P[1][1]; /* 1,0 */ Pdot[3] = Q_gyro; /* 1,1 */ /* Store our unbias gyro estimate */ rate = q; /* * Update our angle estimate * angle += angle_dot * dt * += (gyro - gyro_bias) * dt * += q * dt */ angle += q * dt; /* Update the covariance matrix */ P[0][0] += Pdot[0] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; } /* * kalman_update is called by a user of the module when a new * inclinoometer measurement is available. * * This does not need to be called every time step, but can be if * the accelerometer data are available at the same rate as the * rate gyro measurement. * * H = [ 1 0 ] * * because the angle measurement directly corresponds to the angle * estimate and the angle measurement has no relation to the gyro * bias. */ void kalmanUpdate(const float incAngle) { /* Compute our measured angle and the error in our estimate */ float angle_m = incAngle; float angle_err = angle_m - angle; /* * h_0 shows how the state measurement directly relates to * the state estimate. * * H = [h_0 h_1] * * The h_1 shows that the state measurement does not relate * to the gyro bias estimate. We don't actually use this, so * we comment it out. */ float h_0 = 1; /* const float h_1 = 0; */ /* * Precompute PH' as the term is used twice * Note that H[0,1] = h_1 is zero, so that term is not not computed */ const float PHt_0 = h_0*P[0][0]; /* + h_1*P[0][1] = 0*/ const float PHt_1 = h_0*P[1][0]; /* + h_1*P[1][1] = 0*/ /* * Compute the error estimate: * (equation 21-1) * * E = H P H' + R */ float E = R_angle +(h_0 * PHt_0); /* * Compute the Kalman filter gains: * (equation 21-2) * * K = P H' inv(E) */ float K_0 = PHt_0 / E; float K_1 = PHt_1 / E; /* * Update covariance matrix: * (equation 21-3) * * P = P - K H P * Let * Y = H P */ float Y_0 = PHt_0; /*h_0 * P[0][0]*/ float Y_1 = h_0 * P[0][1]; P[0][0] -= K_0 * Y_0; P[0][1] -= K_0 * Y_1; P[1][0] -= K_1 * Y_0; P[1][1] -= K_1 * Y_1; /* * Update our state estimate: * * Xnew = X + K * error * * err is a measurement of the difference in the measured state * and the estimate state. In our case, it is just the difference * between the inclinometer measured angle and the estimated angle. */ angle += K_0 * angle_err; q_bias += K_1 * angle_err; }