一阶互补
// 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;
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
