Go语言视频零基础入门到精通

admin

一阶互补

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

复制代码

1. // newAngle = angle measured with atan2 using the accelerometer

复制代码

1. //加速度传感器输出值

复制代码

1. // newRate = angle measured using the gyro

复制代码

1. // looptime = loop time in millis()

复制代码

复制代码

1. [b]float[/b] tau=0.075;

复制代码

1. [b]float[/b] a=0.0;

复制代码

1. [b]float[/b] Complementary([b]float[/b] newAngle, [b]float[/b] newRate,[b]int[/b] looptime)

复制代码

1. {

复制代码

1. [b]float[/b] dtC = [b]float[/b](looptime)/1000.0;

复制代码

1. a=tau/(tau+dtC);

复制代码

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

复制代码

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

复制代码

1. [b]return[/b] x_angleC;

复制代码

}
   
二阶互补

1. // newAngle = angle measured with atan2 using the accelerometer

复制代码

1. // newRate = angle measured using the gyro

复制代码

1. // looptime = loop time in millis()

复制代码

1. [b]float[/b] Complementary2([b]float[/b] newAngle, [b]float[/b] newRate,[b]int[/b] looptime)

复制代码

1. {

复制代码

1. [b]float[/b] k=10;

复制代码

1. [b]float[/b] dtc2=[b]float[/b](looptime)/1000.0;

复制代码

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

复制代码

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

复制代码

1. y1 = dtc2*x1 + y1;

复制代码

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

复制代码

1. x_angle2C = dtc2*z1 + x_angle2C;

复制代码

1. [b]return[/b] x_angle2C;

复制代码

} Here too we just have to setthe k and magically we get the angle.
卡尔曼滤波

1. // KasBot V1 - Kalman filter module

复制代码

复制代码

1. [b]float[/b] Q_angle  =  0.01; //0.001

复制代码

1. [b]float[/b] Q_gyro   =  0.0003;  //0.003

复制代码

1. [b]float[/b] R_angle  =  0.01;  //0.03

复制代码

复制代码

1. [b]float[/b] x_bias = 0;

复制代码

1. [b]float[/b] P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;

复制代码

1. [b]float[/b]  y, S;

复制代码

1. [b]float[/b] K_0, K_1;

复制代码

复制代码

1. // newAngle = angle measured with atan2 using the accelerometer

复制代码

1. // newRate = angle measured using the gyro

复制代码

1. // looptime = loop time in millis()

复制代码

复制代码

1. [b]float[/b] kalmanCalculate([b]float[/b] newAngle, [b]float[/b] newRate,[b]int[/b] looptime)

复制代码

1. {

复制代码

1. [b]float[/b] dt = [b]float[/b](looptime)/1000;

复制代码

1. x_angle += dt * (newRate - x_bias);

复制代码

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

复制代码

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

复制代码

1. P_01 +=  - dt * P_11;

复制代码

1. P_10 +=  - dt * P_11;

复制代码

1. P_11 +=  + Q_gyro * dt;

复制代码

复制代码

1. y = newAngle - x_angle;

复制代码

1. S = P_00 + R_angle;

复制代码

1. K_0 = P_00 / S;

复制代码

1. K_1 = P_10 / S;

复制代码

复制代码

1. x_angle +=  K_0 * y;

复制代码

1. x_bias  +=  K_1 * y;

复制代码

1. P_00 -= K_0 * P_00;

复制代码

1. P_01 -= K_0 * P_01;

复制代码

1. P_10 -= K_1 * P_00;

复制代码

1. P_11 -= K_1 * P_01;

复制代码

复制代码

1. [b]return[/b] 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 */

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

复制代码

        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;
}