/*?-*-?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個文件信息