a_n[0]=ag[0]-scale[0]*Vgt[0];
a_n[1]=ag[1]-scale[0]*Vgt[1];
a_n[2]=ag[2]-scale[0]*Vgt[2];
g_n[0]=gg[0]-scale[1]*Vgt[0];
g_n[1]=gg[1]-scale[1]*Vgt[1];
g_n[2]=gg[2]-scale[1]*Vgt[2];
l_b[0]=a_n[0]-g_n[0];
l_b[1]=a_n[1]-g_n[1];
l_b[2]=a_n[2]-g_n[2]; // Pseudo_lift
P_b[0]=g_n[1]*Vgt[2]-g_n[2]*Vgt[1];
P_b[1]=g_n[2]*Vgt[0]-g_n[0]*Vgt[2];
P_b[2]=g_n[0]*Vgt[1]-g_n[1]*Vgt[0]; // The local horizontal reference
Major.GPRoll=asin((l_b[0]*P_b[0]+l_b[1]*P_b[1]+l_b[2]*P_b[2])/((sqrt(l_b[0]*l_b[0]+l_b[1]*l_b[1]+l_b[2]*l_b[2]))*(sqrt(P_b[0]*P_b[0]+P_b[1]*P_b[1]+P_b[2]*P_b[2]))));//Pseudo_Roll
Major.GPPitch=atan2(-Vd,sqrt((Vn*Vn)+(Ve*Ve))); //Pseudo_Pitch
Major.GPHeading=atan2(Ve,Vn); //Pseudo_Heading
if (Major.GPHeading<0.0) Major.GPHeading=Major.GPHeading+6.282723;
}//End void
انجام محاسبات فوق پس از کالمن فیلتر سرعت و برآورد شتاب امکان پذیر است . در پایان برنامه چون استخراج زاویه سمت از تابع تانژانت معکوس است و خروجی آن بین مقادیر مثبت و منفی π متغیر می باشد ، لذا به مقادیر منفی آن ، π۲ اضافه می شود تا عدد نهایی بین  قرار گیرد .
فصل چهارم : شبیه ســازی
مقدمه
طول حیات مدلهای شبیه سازی بستگی به استراتژی طراحی آنها دارد و بعد ازمدتی که یک سیستم تهیه شد به تدریج ارزش آن کاهش می یابد و مجبور است خود را با استراتژی های جدید تطبیق دهد . MATLAB Simulink همه جوانب افزایش طول عمر چرخه حیات یک مدل را در نظر می گیرد و روز به روز طراحی و ساختار آن کنترل می شود .
استفاده مؤثر از مدلهای شبیه سازی بحث مطرح امروز است و Simulink همه ابزار این کار را در دست دارد . نیز به وسیله پروتکل های ارتباطی قادر است نرم افزارها و سیستم های دیگر را کنترل کند . البته خود نیز می تواند توسط منابع خارجی به خوبی کنترل شود . به وسیله بانکهای اطلاعاتی , اطلاعات شبیه سازی به طور پویا ذخیره و بازیابی می شوند .
با وجود چنین ابزار قدرتمندی ، بر آن شدیم تا در ادامه روند طراحی خلبان خودکار با بهره گرفتن از سیستم موقعیت یاب ، برای پیاده سازی الگوریتم ها و تحلیل یافته هایمان به این نرم افزار متوسل شویم ، چیزی که امروزه در صنایعی با تکنولوژی سطح بالا^[۸۳] از آن به عنوان نرم افزار در حلقه شبیه ساز (^[۸۴] SILS ) یاد می شود .

SILS یکی از ابزارهای بسیار مهم و بدیع در صنایع کنترلیِ پرهزینه نظیر خودرو ، هوافضا و … است که از سال ۱۹۸۰ به طور جدی مورد استفاده واقع شده است و ضمن کاهش هزینه و زمان، ایمنی کار را نیز به شدت افزایش داده است. به طور خلاصه سه دلیل عمده زیر برای کاربرد آن ذکر می شود ، لذا اهمیت چنین فن آوری در هر صنعتی که به نوعی با این سه مفهوم درگیر است نمود پیدا می کند :

1. 1. هزینه (Cost)

1. 1. زمان (Duration)

1. 1. ایمنی (Safety)

ویژگیهای اصلی آزمایش نرم افزار در حلقه ی شبیه ساز عبارتند از :

1. 1. تست کلیه ی نرم افزارهای موجود در تست پرواز (ایستگاه زمینی، خلبان خودکار و …) در داخل اتاق، بدون تحمل ریسکهای پرواز

1. 1. تنظیم ضرایب کنترلی یا کالمن فیلتر هر پرنده متناسب با خودش در داخل اتاق

1. 1. آموزش کاربر جهت کار با مجموعه تجهیزات پهپاد

1. 1. تحویل به صورت یک بسته تجاری ( COTS )

هر چند SILS نمی تواند به طور صد در صد جایگزین تست های پرواز گردد لیکن با کشف ایرادات و نقص های موجود در نرم افزارها و تنظیم ضرایب کنترلی ، به طور قابل ملاحظه ای احتمال وقوع تخریب را کاهش می دهد.
ضمن اینکه هم اکنون ابزارهای SILS بیش از پیش در حال گسترش هستند لیکن هنوز هم شرکت های معدودی در سطح دنیا یافت می شوند که این ابزار را به صورت یک بسته تجاری (COTS) تحویل مشتری دهند.
در ادامه این فصل کوشیده می شود تا با شبیه سازی موارد ذیل ، صحت یافته ها آزمایش شود .

1. 1. هواپیما

1. 1. سفت افزار^[۸۵]خلبان خودکار

شبیه سازی هواپیما در نرم افزار Aerosim
نرم افزار متلب ، به دلیل استفاده ی فراگیر در کلیه ی صنایع ، دارای نرم افزار های واسطی است که در فرایند شبیه سازی ، گاه به کمک مهندسین می آیند تا از صرف زمان زیاد برای طراحی تمامی بلوک های شبیه ساز اجتناب شود . در این تحقیق ما علاوه بر شبیه سازی معادلاتی که در فصل پیشین در ارتباط با آن ها سخن به میان آمد ، نیاز به شبیه سازی هواپیمایی داریم تا بتوان از مشخصات خروجی آن در حل معادلات حلقه باز استفاده کرد .
برای مشخص شدن رفتار پرنده سایه ( پرنده انتخابی برای شبیه سازی ) در حالت حلقه باز از نرم افزارهای شبیه سازی Aerosim و FlightGear استفاده شده است. به این صورت که مدل ریاضی هواپیما در نرم افزار Aerosim حل شده و نتایج به صورت کمی و نموداری به دست می‌‌آید. با بهره گرفتن از نرم افزار FlightGear نیز می‌‌توان رفتار تصویری و کیفی پرنده را نمایش داد.
در نرم‌‌افزار AeroSim مجموعه‌‌ی کاملی از بلوک‌‌ها وجود دارد که به وسیله‌‌ی آن‌‌ها می‌‌توان رفتار هواپیما را به صورت ۶ درجه آزادی غیر خطی شبیه‌‌سازی کرد. در این بخش به معرفی و تشریع این بلوک‌‌ها خواهیم پرداخت و نحوه‌‌ی استفاده از آن‌‌ها را بیان می‌‌نماییم.
لازم به ذکر است که بلوک‌‌ها دارای ۲ نوع ورودی می‌‌باشند. نوع اول به صورت مقادیر ثابت می‌‌باشد که به نام پارامتر شناخته می‌‌شود. نوع دوم با زمان تغییر می‌‌کند و از سمت سایر بخش ها وارد بلوک می‌‌شود.
شکل ‏۴‌.۱ نمای کلی از بلوک های نرم افزار Aerosim
در هر یک از این مجموعه‌‌ها ، بلوک های متعددی وجود دارد که به وسیله هر کدام از آنها می‌‌توان قسمتی از رفتار هواپیما و شرایط پیرامون آن را شبیه سازی کرد. به عنوان مثال در بخش آیرودینامیک می‌‌توان به شبیه سازی رفتار پرنده از لحاظ چگونگی حرکت در هوا و تاثیرات نیروهای هوا بر پرنده پرداخت و…
بنابراین نرم افزار Aerosim در حقیقت یک مرجع کامل شبیه سازی پرنده در متلب است ، و بدیهی است که به تمامی بلوک های آن برای یک شبیه سازی ِ ابتدایی نیازی نیست .
آنچه برای شبیه سازی خلبان خودکار مورد نیاز است ، تنها یک بلوک هواپیما است ، که شرایط ابتدایی یک فرایند پروازی را شامل شود . بدان معنی که دارای ورودی سطوح و شرایط محیط باشد و خروجی آن تمامی متغیرهای لحظه ای پرنده از جمله زوایای اویلر ، شتاب های خطی ، ارتفاع از سطح زمین و دریا و … را شامل شود .