在线亚洲欧美综合网站,久久精品国产999久久久,日韩欧美亚洲综合久久影院

/*
1主函數(shù)在最后面
2作者：黑市，黑視，智涅
3四元數(shù)更新用的是一階算法，還有二階三階甚至全階，階數(shù)越高精度越好，不過沒多大必要。
一階二階這些簡化算法就是用簡單的值取代了一些三角函數(shù)而已
4詳細(xì)書籍可以看《捷聯(lián)式慣性導(dǎo)航原理》，袁信著。我以前看了這本書PDF版好久了，估計(jì)一半還沒能吃透。
5關(guān)于利用加速度計(jì)來修正姿態(tài)，大家貌似都是做飛機(jī)的，飛機(jī)上的加速度計(jì)的情況跟我做的東西差別太大，
應(yīng)該不能直接引用我那一套，大家還是引用網(wǎng)上例如權(quán)重法來進(jìn)行修正吧！用羅盤修正姿態(tài)就更不用說了~
*/
#define EulerAngle_Type float //定義類型
#define Quaternion_Type float
#define Acc_Type int
#define Gyro_Type int
#define Euler_Martix_Type float
struct EulerAngle //歐拉角結(jié)構(gòu)體
{
EulerAngle_Type Roll, Pitch, Yaw;
}
struct Quaternion //四元數(shù)結(jié)構(gòu)體
{
Quaternion_Type q0, q1, q2, q3;
}
struct Acc //加速度值結(jié)構(gòu)體
{
Acc_Type x, y, z;
}
struct Gyro //陀螺儀值結(jié)構(gòu)體
{
Gyro_Type x, y, z;
}
struct Euler_Martix //歐拉（姿態(tài)）矩陣結(jié)構(gòu)體
{
Euler_Martix_Type T11,T12,T13, T21,T22,T23, T31,T32,T33;
}
Quaternion Normalize(Quaternion e) //四元數(shù)歸一化
{
Quaternion_Type s = (Quaternion_Type)Math.Sqrt(e.q0 * e.q0 + e.q1 * e.q1 + e.q2 * e.q2 + e.q3 * e.q3);
e.q0 /= s;
e.q1 /= s;
e.q2 /= s;
e.q3 /= s;
return e;
}
Quaternion Multiply_L1(Acc lacc) //一階算法
{
Quaternion Q_result;
Q_result.q0 = BQ.q0 - BQ.q1 * lacc.x / 2 - BQ.q2 * lacc.y / 2 - BQ.q3 * lacc.z / 2;
Q_result.q1 = BQ.q1 + BQ.q0 * lacc.x / 2 + BQ.q2 * lacc.z / 2 - BQ.q3 * lacc.y / 2;
Q_result.q2 = BQ.q2 + BQ.q0 * lacc.y / 2 - BQ.q1 * lacc.z / 2 + BQ.q3 * lacc.x / 2;
Q_result.q3 = BQ.q3 + BQ.q0 * lacc.z / 2 + BQ.q1 * lacc.y / 2 - BQ.q2 * lacc.x / 2;
Q_result = Quaternion_Normalize(Q_result);
return Q_result;
}
Euler_Martix Q_to_EM(Quaternion e) //把四元數(shù)變換成歐拉角（姿態(tài)）矩陣T
{
Euler_Martix result;
Euler_Martix_Type q00,q01,q02,q03,q11,q12,q13,q22,q23,q33;
q00=e.q0*e.q0;
q01=e.q0*e.q1;
q02=e.q0*e.q2;
q03=e.q0*e.q3;
q11=e.q1*e.q1;
q12=e.q1*e.q2;
q13=e.q1*e.q3;
q22=e.q2*e.q2;
q23=e.q2*e.q3;
q33=e.q3*e.q3;
result.T11=q00+q11-q22-q33;
result.T12=2*(q12+q03);
result.T13=2*(q13-q02);
result.T21=2*(q12-q03);
result.T22=q22-q33+q00-q11;
result.T23=2*(q23+q01);
result.T31=2*(q13+q02);
result.T32=2*(q23-q01);
result.T33=q33-q22-q11+q00;
return result;
}
Quaternion Ea_to_Qu(EulerAngle ea) //把歐拉角變換成四元數(shù) 后來不用這個(gè)方法了，用矩陣那個(gè)了
{
Quaternion result;
Quaternion_Type CosY = Math.Cos(ea.Yaw * .5);
Quaternion_Type SinY = Math.Sin(ea.Yaw * .5);
Quaternion_Type CosP = Math.Cos(ea.Pitch * .5f);
Quaternion_Type SinP = Math.Sin(ea.Pitch * .5);
Quaternion_Type CosR = Math.Cos(ea.Roll * .5f);
Quaternion_Type SinR = Math.Sin(ea.Roll * .5f);
result.q0 = CosY * CosP * CosR + SinY * SinP * SinR;
result.q1 = CosY * CosP * SinR - SinY * SinP * CosR;
result.q2 = CosY * SinP * CosR + SinY * CosP * SinR;
result.q3 = SinY * CosP * CosR - CosY * SinP * SinR;
return result;
}
Acc coordinate_body_to_inertia(Euler_Martix EM,Acc lacc) //將體坐標(biāo)加速度變換到慣性坐標(biāo)
{
//做飛機(jī)不需要，省略
}
EulerAngle EM_to_EU(Euler_Martix lem) //從姿態(tài)矩陣中提取姿態(tài)角
{
EulerAngle result;
result.Yaw = Math.Atan2(lem.T12, lem.T11);
result.Pitch = -Math.Asin(lem.T13);
result.Roll = Math.Atan2(lem.T23, lem.T33);
return result;
}
void main()
{
Quaternion BQ;//定義姿態(tài)四元素
Euler_Martix BEM;//定義歐拉矩陣
EulerAngle BEA;//定義歐拉角
BQ.q0=1;//初始化四元數(shù)
BQ.q1=BQ.q2=BQ.q3=0;//初始化四元數(shù)
char gx,gy,gz;//定義陀螺儀三個(gè)軸，用來裝值
while（1）//要不斷更新，所以弄個(gè)循環(huán)，你要知道你的更新速率，才能轉(zhuǎn)化下面的一些參數(shù)。
//根據(jù)轉(zhuǎn)動(dòng)的不可交互性，更新速率越快越好，看書！
{
gx=25;
gy=14;
gz=4;
//以上為讀取陀螺儀的三個(gè)值，我隨便賦值作為例子哈
EulerAngle lea;//定義歐拉小轉(zhuǎn)角，每次更新的小轉(zhuǎn)角，下面就用到
lea.Yaw = ((float)gx / 62200 * Math.PI);
lea.Pitch = ((float)gy / 62200 * Math.PI);
lea.Roll = ((float)gz/ 62200 * Math.PI);
//把陀螺儀的三個(gè)值轉(zhuǎn)化為角度值（根據(jù)自己的采樣率、精度等參數(shù)轉(zhuǎn)化），不懂請(qǐng)請(qǐng)自行查資料搞懂
BQ = Multiply_L1(BQ, lea);//更新姿態(tài)四元素，由舊四元數(shù)和小轉(zhuǎn)角更新得到新四元數(shù)
BEM = Q_to_EM(BQ);//將更新完的姿態(tài)四元素轉(zhuǎn)成歐拉矩陣
BEA = EM_to_EU(BEM);//從歐拉矩陣中提取歐拉角
}
}
……………………
…………限于本文篇幅余下代碼請(qǐng)從51黑下載附件…………

復(fù)制代碼

欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136