(۳-۲۱)
پارامترهای V و W کمیت های اسکالر هستند . با ماتریس P داده شده :
(۳-۲۲)
معادله (۳-۱۹) به شکل (۳-۲۳) در می آید :
(۳-۲۳)
و با حل معادله (۳-۲۳) بدست می آید :
(۳-۲۴)

پیشتر در معادله (۳-۲۰) گفته شد که در این تحقیق هدف از بکارگیری فیلتر کالمن ، استخراج شتاب و همچنین مشتق شتاب از روی اطلاعات سرعت است ، لذا تمام آنچه جز به جزء آورده شد را می توان در فلوچارت بعد خلاصه کرد :
شکل۳‌.‌‌۳ الگوریتم نهایی فیلتر کالمن[۴]
لازم به ذکر است تمامی معادلات بالا برای هر بعد به صورت جداگانه باید محاسبه شود . در ادامه از اطلاعات استخراج شده برای استفاده در روابط اینرسی استفاده می شود .

( اینجا فقط تکه ای از متن فایل پایان نامه درج شده است. برای خرید متن کامل پایان نامه با فرمت ورد می توانید به سایت feko.ir مراجعه نمایید و کلمه کلیدی مورد نظرتان را جستجو نمایید. )

محاسبه زوایای شبه وضعیت
زوایای شبه وضعیت جانشین مناسبی برای زوایای سنتی پرنده ( غلت ، اوج ، سمت ) هستند ، با این مزیت که عملیات محاسبه آن ها تنها از روی بردار سرعت v_g و شتاب a_g قابل پردازش است .
زوایای شبه وضعیت بدون نیاز به داشتن اطلاعات دیگری از وسیله پرنده نظیر زاویه حمله^[۷۹] یا میزان زاویه باد و تنها با بهره گرفتن از اطلاعات GPS اندازه گیری می شوند .همانگونه که پیشتر اشاره شد ، زوایای شبه وضعیت از زاویه مسیر پرواز در محور طولی و از زاویه شبه غلت در محور عرضی تشکیل شده است .
زاویه مسیر پرواز (γ) ، زاویه بین بردار سرعت و خط افق است و با تقریب قابل قبولی به جای زاویه اوج به کار برده می شود .همچنین زاویه شبه غلت  هم به عنوان زاویه چرخش^[۸۰] موثر در محورعرضی به کار برده می شود و جایگزین مناسبی برای زاویه غلت متداول است .
شکل۳‌.‌‌۴ نمایش زوایای شبه وضعیت[۱۲]
شکل ۳‌.۴ بردارهای سرعت بدنی v_g و نیز محورهای بدنی ( y_b , z_b , x_b) را نشان می دهد ، همانگونه که اشاره شد ، زاویه اوج ، زاویه بین v_g و افق است و از طریق رابطه زیر محاسبه می شود .
(۳-۲۵)
زاویه شبه غلت از محاسبه زاویه ی شتاب پرنده و شتاب جاذبه به دست می آید ( شکل ۳‌.۵)
شکل۳‌.‌‌۵ طریقه محاسبه زاویه غلت
در شکل ۳‌.۵ ابتدا به توضیح بردار شبه شتاب بالارونده^[۸۱] (  ) پرداخته می شود ، در طول پرواز چهار نیرو بر هواپیما وارد می شود که این چهار نیرو عبارتند از :

1. 1. نیروی وزن

1. 1. نیروی لیفت (بَرا یا بالابرنده)

1. 1. نیروی پیشران یا جلوبرنده

1. 1. نیروی دِرَگ (پسا یا مقاومت هوا)

نیروی شبه لیفت از تفاضل نیروی شتاب پرنده و محور شتاب زمین حاصل می شود و همواره بردار شتاب پرنده و شتاب زمین بر بردار سرعت عمود است .
(۳-۲۶)
(۳-۲۷)
(۳-۲۸)
(۳-۲۹)
(۳-۳۰)
در معادلات فوق t^[82] یا همان بردار مماس و n بیانگر بردار نرمال است . پس از محاسبه بردار شبه لیفت ، نوبت به محاسبه بردار شبه افق است .
(۳-۳۱)
و در نهایت پس از انجام محاسبات فوق زاویه شبه غلت از رابطه زیر محاسبه می شود :
(۳-۳۲)
پس از محاسبه زوایای شبه اوج و غلت نوبت به زاویه سمت است ، همانگونه که واضح است این زاویه به صورت مستقیم و بدون نیاز به انجام عملیات ریاضی در خروجی اطلاعات جی پی اس ، تحت عنوان زاویه مسیر وجود دارد و قابل استفاده است ، اما در صورتی که محاسبه آن مستقیما از روی اطلاعات سرعت مطلوب باشد ، به دست می آید:
(۳-۳۳)
پیاده سازی با زبان برنامه نویسی C
به جهت تکمیل فرایند طراحی ، کد زبان C استفاده شده در این تحقیق ا که به محاسبه زوایای مورد نیاز می پردازد ، آورده می شود . برای پیاده سازی سعی شده تا متغیرها ، اسامی و حتی ترتیب انجام آن ها طبق توضیحات پیشین باشد :
void C_GPSAngle( double *Vgt,double *ag,double *gg)
{
double Vn,Ve,Vd,V_gt,V_g2;
double a_n[3],g_n[3],scale[2],P_b[3],l_b[3];
Vn=Vgt[0]; // V_North
Ve=Vgt[1]; // V_East
Vd=Vgt[2]; // V_Down
V_gt=sqrt(Vgt[0]*Vgt[0]+Vgt[1]*Vgt[1]+Vgt[2]*Vgt[2]);
V_g2=V_gt*V_gt;
scale[0]=ag[0]*(Vgt[0]/V_g2)+ag[1]*(Vgt[1]/V_g2)+ag[2]*(Vgt[2]/V_g2);
scale[1]=gg[0]*(Vgt[0]/V_g2)+gg[1]*(Vgt[1]/V_g2)+gg[2]*(Vgt[2]/V_g2);