姿态融合的一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序.doc

(完整版)姿态融合的一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序/一阶互补/ 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; x2 = y1 + (newAngle x_angle2C) 2 k + newRate； x_angle2C = dtc2 x2 + x_angle2C； return x_angle2C；/Here too we just have to set the k and magically we get the angle. 卡尔曼滤波/ KasBot V1 - Kalman filter modulefloat Q_angle = 0.01; /0.001float Q_gyro = 0。0003； /0.003float R_angle = 0。01; /0。03float 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 to set 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 * 1 dimensional tilt sensor using a dual axis accelerometer* and single axis angular rate gyro。 The two 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 in blocks before the functions。 Kalman filtering is an already complex* subject, made even more so by extensive hand optimizations to the C code* that implements the filter。 Ive tried to make an effort of explaining* the optimizations， but feel free to send mail to the mailing list, autopilot-devellists.sf。net, with questions about this code。* (c) 2003 Trammell Hudson * This file is part of the autopilot onboard code 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 021111307 USA*/#include /* * Our update rate。 This is how often our state is updated with gyro rate measurements。 For now, we do it every time an * 8 bit 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 updated at every time step to determine how well the sensors are tracking the actual state。 */static float P22 = 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；/* R represents the measurement covariance noise. In this case， it is a 1x1 matrix that says that we expect 0。3 rad jitter from the accelerometer。 */static const float R_angle = 0.3；/* * Q is a 2x2 matrix that represents the process covariance noise。 * In this 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 * 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。 * Our state vector is: X = angle, gyro_bias * It runs 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 = AP + PA + Q * * A is 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， weve hand optimized the C code to only compute the terms that are * explicitly nonzero， as well as expanded out the matrix math * to be done in as few steps as possible. This does make it harder to read, 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 */ const float q = q_m q_bias; / Compute the derivative of the covariance matrix * * Pdot = A*P + P*A + Q * Weve 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 are Q_angle and Q_gyro. */ const float Pdot2 2 = Q_angle P01 - P10, /* 0,0 / - P11， / 0,1 / P11， /* 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 */ P00 += Pdot0 dt; P01 += Pdot1 * dt; P10 += Pdot2 * dt； P11 += Pdot3 * 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 twoaxis 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. * As commented 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 to read 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 is the Jacobian matrix of the measurement value with respect to the 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（ const float ax_m， /* X acceleration / const float az_m / Z acceleration /） / Compute our measured angle and the error in our estimate */ const float angle_m = atan2( -az_m， ax_m )； const float angle_err = angle_m angle； / C_0 shows how the state measurement directly relates to * the state estimate。 * The C_1 shows that the state measurement does not relate to the gyro bias estimate. We dont actually use this, so we comment it out。 */ const float C_0 = 1； / const float C_1 = 0; */ / PCt = P2，2 C, which we use twice. This makes * it worthwhile to precompute and store the two values。 * Note that C0，1 = C_1 is zero, so we do not compute that * term。 / const float PCt_0 = C_0 * P00； / + C_1 P01 = 0 */ const float PCt_1 = C_0 * P10; /* + C_1 P11 = 0 */ / * Compute the error estimate. From the Kalman filter paper: * * E = C P C + R Dimensionally， * * E1，1 = C P2，2 C2,1 + R * Again， note that C_1 is zero, so we do not compute the term。 */ const float 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： * K2，1 = P * Luckilly， E is 1,1, so the inverse of E is just 1/E。 */ const float K_0 = PCt_0 / E; const float K_1 = PCt_1 / E; / Update covariance matrix。 Again， from the Kalman filter paper： * P = P K C P * Dimensionally： P = K C1，2 P2，2 * We first compute t1，2 = C P。 Note that: t0，0 = C0,0 * P0,0 + C0，1 * P1,0 * But, since C_1 is zero, we have: t0,0 = C0,0 * P0,0 = PCt0,0 * This saves us a floating point multiply。 / const float t_0 = PCt_0； /* C_0 P00 + C_1 P10 / const float t_1 = C_0 P01; /* + C_1 P11 = 0 */ P00 = K_0 * t_0; P01 -= K_0 * t_1； P10 = K_1 t_0; P11 = K_1 t_1; /* * Update our state estimate。 Again， from the Kalman paper: * X += K * err * * And， dimensionally， * * X = X2 + K err1，1 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 two accelerometer measured angle and our estimated angle. */ angle += K_0 * angle_err; q_bias += K_1 angle_err;