imu_ahrs_航姿参考系统_AHRS_

/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8: * $Id: ahrs.c,v 2.0 2002/09/22 02:10:16 tramm Exp $ * * (c) 2002 Trammell Hudson <hudson@swcp.com> ************* * * 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 02111-1307 USA * */ #include "ahrs.h" #include "vector.h" #define Rad2Deg 57.2957795130823208767981548141052 v_t ac_Xe, ac_Xm; /** * Kalman weighting matrices */ static m_t Q = { { 0.00015, 0.00000, 0.00000, 0.00000 }, { 0.00000, 0.00015, 0.00000, 0.00000 }, { 0.00000, 0.00000, 0.00015, 0.00000 }, { 0.00000, 0.00000, 0.00000, 0.00015 }, }; static m_t R = { { 0.086, 0.000, 0.000 }, { 0.000, 0.086, 0.000 }, { 0.000, 0.000, 0.015 }, }; /* * Covariance matrix */ static m_t P; /* * Position vector * Initial position: Level, facing north * * Estimated position is derived from this. Unless ahrs_init() is called * the filter is likely to spike. */ static v_t X = { 1.0, 0.0, 0.0, 0.0 }; /** * Vector helpers related to the AHRS computation */ static f_t sq( f_t d ) { return d * d; } /* * This will normalize a quaternion vector q * q/norm(q) * q(4,1) */ static void v_normq( v_t q_out, const v_t q ) { v_scale( q_out, q, 1.0 / sqrt( sq(q[0]) + sq(q[1]) + sq(q[2]) + sq(q[3]) ), 4 ); } /* * This will convert from quaternions to euler angles * q(4,1) -> euler[phi;theta;psi] (rad) * * Requires 13670 instructions (3420 microseconds) */ static void quat2euler( v_t euler, const v_t q ) { f_t q0 = q[0]; f_t q1 = q[1]; f_t q2 = q[2]; f_t q3 = q[3]; f_t q02 = q0*q0; f_t q12 = q1*q1; f_t q22 = q2*q2; f_t q32 = q3*q3; /* phi */ euler[0] = atan2( 2.0 * (q2*q3 + q0*q1), 1.0 - 2.0 * (q12 + q22) ); /* theta */ euler[1] = -asin( 2.0 * (q1*q3 - q0*q2) ); /* psi */ euler[2] = atan2( 2.0 * (q1*q2 + q0*q3), 1.0 - 2.0 * (q22 + q32) ); } /* * This will convert from euler angles to quaternion vector * phi, theta, psi -> q(4,1) * euler angles in radians */ static void euler2quat( v_t q, f_t phi, f_t theta, f_t psi ) { f_t sphi = sin( 0.5 * phi ); f_t stheta = sin( 0.5 * theta ); f_t spsi = sin( 0.5 * psi ); f_t cphi = cos( 0.5 * phi ); f_t ctheta = cos( 0.5 * theta ); f_t cpsi = cos( 0.5 * psi ); q[0] = cphi*ctheta*cpsi + sphi*stheta*spsi; q[1] = -cphi*stheta*spsi + sphi*ctheta*cpsi; q[2] = cphi*stheta*cpsi + sphi*ctheta*spsi; q[3] = cphi*ctheta*spsi - sphi*stheta*cpsi; } /* * This will convert from accelerometer and compass readings into * [phi, theta, psi] (in radians). The accelerometer readings are * assumed to be in m/s^2, but the actual units don't matter. * * Requires 13250 instructions (3310 microseconds) */ static void accel2euler( v_t euler, f_t ax, f_t ay, f_t az, f_t bx, f_t by, f_t bz ) { f_t g = sqrt( ax*ax + ay*ay + az*az ); euler[0] = atan2( ay, -az ); /* Roll */ euler[1] = asin( ax / -g ); /* Pitch */ //euler[2] = compass; /* Yaw */ euler[2] = -atan2(bz*sin(euler[0])-by*cos(euler[0]), bx*cos(euler[0])+by*sin(euler[1])*sin(euler[0])+bz*sin(euler[1])*cos(euler[0])); /* Yaw */ } /* * This will construct a direction cosine matrix from * quaternions in the standard rotation sequence * [phi][theta][psi] * * body = tBL(3,3)*NED * q(4,1) */ static void quat2dcm( m_t dcm, const v_t q ) { f_t q0 = q[0]; f_t q1 = q[1]; f_t q2 = q[2]; f_t q3 = q[3]; f_t q02 = q0 * q0; f_t q12 = q1 * q1; f_t q22 = q2 * q2; f_t q32 = q3 * q3; dcm[0][0] = 1.0 - 2.0 * ( q22 + q32 ); dcm[0][1] = 2.0 * ( q1*q2 + q0*q3 ); dcm[0][2] = 2.0 * ( q1*q3 - q0*q2 ); dcm[1][0] = 2.0 * ( q1*q2 - q0*q3 ); dcm[1][1] = 1.0 - 2.0 * ( q12 + q32 ); dcm[1][2] = 2.0 * ( q2*q3 + q0*q1 ); dcm[2][0] = 2.0 * ( q1*q3 + q0*q2 ); dcm[2][1] = 2.0 * ( q2*q3 - q0*q1 ); dcm[2][2] = 1.0 - 2.0 * ( q12 + q22 ); } /* * This will construct the quaternion omega matrix * W(4,4) * p, q, r (rad/sec) */ static void rotate2omega( m_t W, f_t p, f_t q, f_t r ) { W[0][0] = 0.0; W[0][1] = -p/2.0; W[0][2] = -q/2.0; W[0][3] = -r/2.0; W[1][0] = p/2.0; W[1][1] = 0.0; W[1][2] = r/2.0; W[1][3] = -q/2.0; W[2][0] = q/2.0; W[2][1] = -r/2.0; W[2][2] = 0.0; W[2][3] = p/2.0; W[3][0] = r/2.0; W[3][1] = q/2.0; W[3][2] = -p/2.0; W[3][3] = 0.0; } /* * F[3] is a temp vector of d( arcsin(X) ) to reduce the complexity * of the C matrix calculations. */ static void compute_f( v_t F, m_t DCM ) { F[0] = 2.0 / ( sq(DCM[2][2]) + sq(DCM[1][2]) ); F[1] = -1.0 / sqrt( 1.0 - sq(DCM[0][2]) ); F[2] = 2.0 / ( sq(DCM[0][0]) + sq(DCM[0][1]) ); } /* * C is the measurement matrix that has the measured state * and the estimated state. * * C = [ * [ dphi/dq0 dhpi/dq1 ... ] * [ dtheta/qd0 dtheta/dq1 ... ] * [ dpsi/dq0 dpsi/dq1 ... ] * ] */ static void compute_c( m_t C, v_t X ) { m_t DCM; v_t F; /* Compute the new DCM matrix from the quaternion position */ quat2dcm( DCM, X ); /* Build our temporary matrix */ compute_f( F, DCM ); /* Compute the estimated state */ C[0][0] = F[0] * ( X[1] * DCM[2][2] ); C[0][1] = F[0] * ( X[0] * DCM[2][2] + 2.0 * X[1] * DCM[1][2] ); C[0][2] = F[0] * ( X[3] * DCM[2][2] + 2.0 * X[2] * DCM[1][2] ); C[0][3] = F[0] * ( X[2] * DCM[2][2] ); C[1][0] = F[1] * -2.0 * X[2]; C[1][1] = F[1] * 2.0 * X[3]; C[1][2] = F[1] * -2.0 * X[0]; C[1][3] = F[1] * 2.0 * X[1]; C[2][0] = F[2] * ( X[3] * DCM[0][0] ); C[2][1] = F[2] * ( X[2] * DCM[0][0] ); C[2][2] = F[2] * ( X[1] * DCM[0][0] + 2.0 * X[2] * DCM[0][1] ); C[2][3] = F[2] * ( X[0] * DCM[0][0] + 2.0 * X[3] * DCM[0][1] ); } /* * Kalman filter update * * P is [4,4] and holds the covariance matrix 协方差阵 * R is [3,3] and holds the measurement weights 测量权重 * C is [4,3] and holds the euler to quat tranform * X is [4,1] and holds the estimated state as a quaterion * Xe is [3,1] and holds the euler angles of the estimated state * Xm is [3,1] and holds the measured euler angles */ static void kalman_update( m_t P, m_t R, m_t C, v_t X, const v_t Xe, const v_t Xm ) { m_t T1; m_t T2; m_t E; m_t K; v_t err; v_t temp; ac_Xe[0] =Xe[0]; ac_Xe[1] =Xe[1]; ac_Xe[2] =Xe[2]; ac_Xm[0] =Xm[0]; ac_Xm[1] =Xm[1]; ac_Xm[2] =Xm[2]; /* E[3,3] = C[3,4] P[4,4] C'[4,3] + R[3,3] */ m_mult( T1, C, P, 3, 4, 4 ); m_transpose( T2, C, 3, 4 ); m_mult( E, T1, T2, 3, 4, 3 ); m_add( E, R, E, 3, 3 ); /* K[4,3] = P[4,4] C'[4,3] inv(E)[3,3] */ m_mult( T1, P, T2, 4, 4, 3 ); m33_inv( E, T2 ); m_mult( K, T1, T2, 4, 3, 3 ); /* X[4] = X[4] + K[4,3] ( Xm[3] - Xe[3] ) */ v_sub( err, Xm, Xe, 3 ); mv_mult( temp, K, err, 4, 3 ); v_add( X, temp, X, 4 ); /* P[4,4] = P[4,4] - K[4,3] C[3,4] P[4,4] */ m_mult( T1, K, C, 4, 3, 4 ); m_mult( T2, T1, P, 4, 4, 4 ); m_sub( P, T2, P, 4, 4 ); } /** * ahrs_init() takes the initial values from the IMU and converts * them into the quaterion state vector. */ void ahrs_init( f_t ax, f_t ay, f_t