基于IMU的AHRS

基于IMU(三轴角速率、三轴加速计和三轴磁场)的方位姿态解算系统(ahrs)，基于C语言开发，VC 6.0和VC2010均可运行编译器 。

/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
 * $Id: ahrs.cv 2.0 2002/09/22 02:10:16 tramm Exp $
 *
 * （c） 2002 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  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（41）
 */
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（41） -> 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（41）
 * 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  2014-09-04 21:07  8imu_ahrs\
     目录           0  2014-09-04 21:07  8imu_ahrs\Debug\
     文件       15442  2014-06-27 15:00  8imu_ahrs\Debug\CL.read.1.tlog
     文件        1416  2014-06-27 15:00  8imu_ahrs\Debug\CL.write.1.tlog
     文件       27466  2014-06-27 15:00  8imu_ahrs\Debug\Sim.obj
     文件       23739  2014-06-27 15:00  8imu_ahrs\Debug\ahrs.obj
     文件        1554  2014-06-27 15:00  8imu_ahrs\Debug\cl.command.1.tlog
     文件      503296  2014-06-27 15:00  8imu_ahrs\Debug\imu_ahrs.exe
     文件         406  2014-06-26 11:28  8imu_ahrs\Debug\imu_ahrs.exe.embed.manifest
     文件         472  2014-06-26 11:28  8imu_ahrs\Debug\imu_ahrs.exe.embed.manifest.res
     文件         381  2014-06-27 15:00  8imu_ahrs\Debug\imu_ahrs.exe.intermediate.manifest
     文件     1092456  2014-06-27 15:00  8imu_ahrs\Debug\imu_ahrs.ilk
     文件         109  2014-06-27 15:00  8imu_ahrs\Debug\imu_ahrs.lastbuildstate
     文件        2979  2014-06-27 15:00  8imu_ahrs\Debug\imu_ahrs.log
     文件     2386944  2014-06-27 15:00  8imu_ahrs\Debug\imu_ahrs.pdb
     文件         212  2014-06-26 11:28  8imu_ahrs\Debug\imu_ahrs_manifest.rc
     文件           2  2014-06-27 15:00  8imu_ahrs\Debug\link-cvtres.read.1.tlog
     文件           2  2014-06-27 15:00  8imu_ahrs\Debug\link-cvtres.write.1.tlog
     文件           2  2014-06-27 15:00  8imu_ahrs\Debug\link.1836-cvtres.read.1.tlog
     文件           2  2014-06-27 15:00  8imu_ahrs\Debug\link.1836-cvtres.write.1.tlog
     文件           2  2014-06-27 15:00  8imu_ahrs\Debug\link.1836.read.1.tlog
     文件           2  2014-06-27 15:00  8imu_ahrs\Debug\link.1836.write.1.tlog
     文件        2340  2014-06-27 15:00  8imu_ahrs\Debug\link.command.1.tlog
     文件        5304  2014-06-27 15:00  8imu_ahrs\Debug\link.read.1.tlog
     文件        1268  2014-06-27 15:00  8imu_ahrs\Debug\link.write.1.tlog
     文件         448  2014-06-27 15:00  8imu_ahrs\Debug\mt.command.1.tlog
     文件         490  2014-06-27 15:00  8imu_ahrs\Debug\mt.read.1.tlog
     文件         422  2014-06-27 15:00  8imu_ahrs\Debug\mt.write.1.tlog
     文件         650  2014-06-26 11:28  8imu_ahrs\Debug\rc.command.1.tlog
     文件         394  2014-06-26 11:28  8imu_ahrs\Debug\rc.read.1.tlog
     文件         402  2014-06-26 11:28  8imu_ahrs\Debug\rc.write.1.tlog
............此处省略20个文件信息