一阶互补滤波，二阶互补滤波，卡尔曼滤波

一阶互补

// a=tau / (tau + loop time)

// newAngle = angle measured with atan2 using the accelerometer

//加速度传感器输出值

// newRate = angle measured using the gyro

// looptime = loop time in millis()

float tau=0.075;

float a=0.0;

float Complementary(float newAngle, float newRate,int looptime) 

{

float dtC = float(looptime)/1000.0;

a=tau/(tau+dtC);

//以下代码更改成白色，下载后恢复成其他颜色即可看到

x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);

return x_angleC;

}

 

二阶互补

// newAngle = angle measured with atan2 using the accelerometer

// newRate = angle measured using the gyro

// looptime = loop time in millis()

float Complementary2(float newAngle, float newRate,int looptime) 

{

float k=10;

float dtc2=float(looptime)/1000.0;

//以下代码更改成白色，下载后恢复成其他颜色即可看到

x1 = (newAngle -   x_angle2C)*k*k;

y1 = dtc2*x1 + y1;

z1= y1 + (newAngle -   x_angle2C)*2*k + newRate;

x_angle2C = dtc2*z1 + x_angle2C;

return x_angle2C;

} Here too we just have to setthe k and magically we get the angle.

卡尔曼滤波

// KasBot V1 - Kalman filter module

float Q_angle  =  0.01; //0.001

float Q_gyro   =  0.0003;  //0.003

float R_angle  =  0.01;  //0.03

float x_bias = 0;

float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;

float  y, S;

float K_0, K_1;

// newAngle = angle measured with atan2 using the accelerometer

// newRate = angle measured using the gyro

// looptime = loop time in millis()

float kalmanCalculate(float newAngle, float newRate,int looptime)

{

float dt = float(looptime)/1000;

x_angle += dt * (newRate - x_bias);

//以下代码更改成白色，下载后恢复成其他颜色即可看到

P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;

P_01 +=  - dt * P_11;

P_10 +=  - dt * P_11;

P_11 +=  + Q_gyro * dt;

y = newAngle - x_angle;

S = P_00 + R_angle;

K_0 = P_00 / S;

K_1 = P_10 / S;

x_angle +=  K_0 * y;

x_bias  +=  K_1 * y;

P_00 -= K_0 * P_00;

P_01 -= K_0 * P_01;

P_10 -= K_1 * P_00;

P_11 -= K_1 * P_01;

return x_angle;

} To get the answer, you have toset 3 parameters: Q_angle, R_angle,R_gyro.

 

 

详细卡尔曼滤波

/* -*- indent-tabs-mode:T;c-basic-offset:8; tab-width:8; -*- vi: set ts=8:

 *$Id: tilt.c,v 1.1 2003/07/09 18:23:29 john Exp $

 *

 * 1dimensional tilt sensor using a dual axis accelerometer

 *and single axis angular rate gyro.  Thetwo sensors are fused

 *via a two state Kalman filter, with one state being the angle

 *and the other state being the gyro bias.

 *

 *Gyro bias is automatically tracked by the filter.  This seems

 *like magic.

 *

 *Please note that there are lots of comments in the functions and

 * inblocks before the functions.  Kalmanfiltering is an already complex

 *subject, made even more so by extensive hand optimizations to the C code

 *that implements the filter.  I've triedto make an effort of explaining

 *the optimizations, but feel free to send mail to the mailing list,

 *autopilot-devel@lists.sf.net, with questions about this code.

 *

 *

 *(c) 2003 Trammell Hudson <hudson@rotomotion.com>

 *

 *************

 *

 *  Thisfile is part of the autopilot onboard code package.

 * 

 * Autopilot is free software; you can redistribute it and/or modify

 *  itunder the terms of the GNU General Public License as published by

 *  theFree Software Foundation; either version 2 of the License, or

 *  (atyour option) any later version.

 * 

 * Autopilot is distributed in the hope that it will be useful,

 *  butWITHOUT ANY WARRANTY; without even the implied warranty of

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

 *  GNUGeneral Public License for more details.

 * 

 *  Youshould have received a copy of the GNU General Public License

 *  alongwith Autopilot; if not, write to the Free Software

 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA

 *

 */

#include <math.h>

 

 

/*

 *Our update rate.  This is how often ourstate is updated with

 *gyro rate measurements.  For now, we doit every time an

 * 8bit counter running at CLK/1024 expires. You will have to

 *change this value if you update at a different rate.

 */

static const float     dt    = ( 1024.0 * 256.0 )/ 8000000.0;

 

 

/*

 *Our covariance matrix.  This is updatedat 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;

 

 

/*

 * Rrepresents the measurement covariance noise. In this case,

 * itis a 1x1 matrix that says that we expect 0.3 rad jitter

 *from the accelerometer.

 */

static const float     R_angle   = 0.3;

 

 

/*

 * Qis a 2x2 matrix that represents the process covariance noise.

 * Inthis case, it indicates how much we trust the acceleromter

 *relative to the gyros.

 */

static const float     Q_angle  = 0.001;

static const float     Q_gyro   = 0.003;

 

 

/*

 *state_update is called every dt with a biased gyro measurement

 * bythe user of the module.  It updates thecurrent angle and

 *rate estimate.

 *

 *The pitch gyro measurement should be scaled into real units, but

 *does not need any bias removal.  Thefilter will track the bias.

 *

 *Our state vector is:

 *

 *    X = [ angle, gyro_bias ]

 *

 * Itruns the state estimation forward via the state functions:

 *

 *    Xdot = [ angle_dot, gyro_bias_dot ]

 *

 *    angle_dot       =gyro - gyro_bias

 *    gyro_bias_dot = 0

 *

 *And updates the covariance matrix via the function:

 *

 *    Pdot = A*P + P*A' + Q

 *

 * Ais the Jacobian of Xdot with respect to the states:

 *

 *    A = [ d(angle_dot)/d(angle)     d(angle_dot)/d(gyro_bias) ]

 *        [d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ]

 *

 *      = [0 -1 ]

 *        [0  0 ]

 *

 *Due to the small CPU available on the microcontroller, we've

 *hand optimized the C code to only compute the terms that are

 *explicitly non-zero, as well as expanded out the matrix math

 * tobe done in as few steps as possible. This does make it harder

 * toread, debug and extend, but also allows us to do this with

 *very little CPU time.

 */

void

state_update(   const float             q_m /* Pitch gyro measurement */)

{

       /*Unbias our gyro */

       constfloat             q = q_m - q_bias;

 

       /*

        * Compute the derivative of the covariancematrix

        *

        *    Pdot= A*P + P*A' + Q

        *

        * We've hand computed the expansion of A = [ 0-1, 0 0 ] multiplied

        * by P and P multiplied by A' = [ 0 0, -1, 0].  This is then added

        * to the diagonal elements of Q, which areQ_angle and Q_gyro.

        */

       constfloat             Pdot[2 * 2] = {

              Q_angle- P[0][1] - P[1][0],  /* 0,0 */

                      - P[1][1],              /* 0,1 */

                      -P[1][1],              /* 1,0 */

              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

 *accelerometer measurement is available. ax_m and az_m do not

 *need to be scaled into actual units, but must be zeroed and have

 *the same scale.

 *

 *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.

 *

 *For a two-axis accelerometer mounted perpendicular to the rotation

 *axis, we can compute the angle for the full 360 degree rotation

 *with no linearization errors by using the arctangent of the two

 *readings.

 *

 * Ascommented in state_update, the math here is simplified to

 *make it possible to execute on a small microcontroller with no

 *floating point unit.  It will be hard toread the actual code and

 *see what is happening, which is why there is this extensive

 *comment block.

 *

 *The C matrix is a 1x2 (measurements x states) matrix that

 * isthe Jacobian matrix of the measurement value with respect

 * tothe states.  In this case, C is:

 *

 *    C = [ d(angle_m)/d(angle)  d(angle_m)/d(gyro_bias) ]

 *      = [1 0 ]

 *

 *because the angle measurement directly corresponds to the angle

 *estimate and the angle measurement has no relation to the gyro

 *bias.

 */

void

kalman_update(

       constfloat             ax_m,     /* X acceleration */

       constfloat             az_m       /* Z acceleration */

)

{

       /*Compute our measured angle and the error in our estimate */

//以下代码更改成白色，下载后恢复成其他颜色即可看到

       constfloat             angle_m = atan2( -az_m,ax_m );

       constfloat             angle_err = angle_m -angle;

 

       /*

        * C_0 shows how the state measurement directlyrelates to

        * the state estimate.

       *

        * The C_1 shows that the state measurement doesnot relate

        * to the gyro bias estimate.  We don't actually use this, so

        * we comment it out.

        */

       constfloat             C_0 = 1;

       /*const float          C_1 = 0; */

 

       /*

        * PCt<2,1> = P<2,2> *C'<2,1>, which we use twice.  Thismakes

        * it worthwhile to precompute and store thetwo values.

        * Note that C[0,1] = C_1 is zero, so we do notcompute that

        * term.

        */

       constfloat             PCt_0 = C_0 * P[0][0];/* + C_1 * P[0][1] = 0 */

       constfloat             PCt_1 = C_0 * P[1][0];/* + C_1 * P[1][1] = 0 */

             

       /*

        * Compute the error estimate.  From the Kalman filter paper:

        *

        *    E =C P C' + R

        *

        * Dimensionally,

        *

        *    E<1,1>= C<1,2> P<2,2> C'<2,1> + R<1,1>

        *

        * Again, note that C_1 is zero, so we do notcompute the term.

        */

       constfloat             E =

              R_angle

              +C_0 * PCt_0

       /*    + C_1 * PCt_1 = 0 */

       ;

 

       /*

        * Compute the Kalman filter gains.  From the Kalman paper:

        *

        *    K =P C' inv(E)

        *

        * Dimensionally:

        *

        *    K<2,1>= P<2,2> C'<2,1> inv(E)<1,1>

        *

        * Luckilly, E is <1,1>, so the inverseof E is just 1/E.

        */

       constfloat             K_0 = PCt_0 / E;

       constfloat             K_1 = PCt_1 / E;

             

       /*

        * Update covariance matrix.  Again, from the Kalman filter paper:

        *

        *    P =P - K C P

        *

        * Dimensionally:

        *

        *    P<2,2>-= K<2,1> C<1,2> P<2,2>

        *

        * We first compute t<1,2> = C P.  Note that:

        *

        *    t[0,0]= C[0,0] * P[0,0] + C[0,1] * P[1,0]

        *

        * But, since C_1 is zero, we have:

        *

        *    t[0,0]= C[0,0] * P[0,0] = PCt[0,0]

        *

        * This saves us a floating point multiply.

        */

       constfloat             t_0 = PCt_0; /* C_0 *P[0][0] + C_1 * P[1][0] */

       constfloat             t_1 = C_0 * P[0][1]; /*+ C_1 * P[1][1]  = 0 */

 

       P[0][0]-= K_0 * t_0;

       P[0][1]-= K_0 * t_1;

       P[1][0]-= K_1 * t_0;

       P[1][1]-= K_1 * t_1;

      

       /*

        * Update our state estimate.  Again, from the Kalman paper:

        *

        *    X +=K * err

        *

        * And, dimensionally,

        *

        *    X<2>= X<2> + K<2,1> * err<1,1>

        *

        * err is a measurement of the difference inthe measured state

        * and the estimate state.  In our case, it is just the difference

        * between the two accelerometer measured angleand our estimated

        * angle.

        */

       angle       += K_0 * angle_err;

       q_bias     += K_1 * angle_err;

}