Диссертация (Система автоматического управления посадочным маневром беспилотного летательного аппарата при действии бокового ветра), страница 13

Сформирован алгоритм переключения режимов полета и вычислениясоответствующих уставочных значений в регуляторы в реальном времени взависимости от скорости полета и бокового ветра.3. С помощью динамического программирования сформулирована ирешена задача оптимального координированного управления боковым ипродольным движением при посадке БЛА, для чего предложено увеличиватьпередаточные числа по положению и скорости в одном регуляторе, одновременноуменьшая их в другом.4. Предложенединыйпараметрическийкритерийоценкикачестваприземления в зависимости от отклонений по курсу, путевому углу и боковомулинейному смещению относительно середины ВПП, учитывающий неудачноесочетание перечисленных параметров, а также найдена область допустимыхотклонений в точке начала выравнивания.

Это позволяет прогнозировать качествоприземления БЛА и в случае опасного выхода из этой области сформироватьвынужденную команду ухода на повторный круг.5. Моделирование на ЭВМ показало, что при скорости бокового ветра до 8м/с и турбулентных составляющих ветра амплитудой до 3м/с предложенныйспособ обеспечивает безопасную посадку при отклонениях от середины ВПП неболее 3м, по углам крена, пути и курса – не более 1 градуса.103СПИСОК ИСПОЛЬЗОВАННЫХ ИСТОЧНИКОВ1. Шевелько П.С., Акиндеев А.Е., Брага В.Г.

и др. «Справочникавиационного техника». – М.: Военное издательство Министерства обороныСССР, 1974. – 592 с.2. Русол В.А., Киселев В.Ф., Крылов Г.О. и др. «Справочник пилота иштурмана гражданской авиации». – М.: Транспорт, 1988. – 319 с.3. Пономаренко В.А., Лапа В.В., Лемещенко Н.А. «Человеческий фактор ибезопасность посадки». – М.: Воениздат, 1993. – 112 с.4. Черный М.А., Кораблин В.И. «Воздушная навигация». – М.: Транспорт,1991.

– 432 с.5. Хафизов А.В. «Радиомаячные системы посадки и системы VOR:Учебное пособие». – Кировоград: ГЛАУ, 2009. – 83 с.6. Михалев И.А., ОкоемовБ.Н., ПавлинаИ.Г. идр. «Системыавтоматического и директорного управления самолетом». – М.: Машиностроение,1974. – 230 с.7. Сушков Ю.А. «Метод и модель безопасного захода воздушного судна напосадку на основе построения оптимальной посадочной траектории», диссертацияна соискание ученой степени кандидата технических наук, Тамбов, 2009. – 99с.8.

Международная организация гражданской авиации, «Руководство повсепогодным полетам», 19919. Афанасьев П.П., Веркин Ю.В., Голубев И.С. и др. «Основы устройства,проектирования, конструирования и производства летательных аппаратов(дистанционно-пилотируемые летательные аппараты)» .

– М.: МАИ, 2006. – 528 с.10. Галушкин А.И. «Нейрокомпьютеры в авиации (самолеты)». Кн. 14. – М.:Радиотехника, 2003. – 496 с.11. Фрадков А.Л. «Адаптивное управление в сложных системах». – М.:Наука, 1990. – 296 с.12. Narendra K.S., Annaswamy A.M. Stable Adaptive Systems, Prentice HallPub., Englewood Cliffs, NJ, 198910413. Sastry S.S., Bodson M. Adaptive Control : Stability, Convergence andRobustness, Prentice Hall Pub., Englewood Cliffs, NJ, 198914. Zhang Y., Ioannou P.A. Stability and Performance of Nonlinear RobustAdaptive Control, Proceedings of the 34th Conference on Decision and Control, 1955. –pp.3941-3946.15.

Golpaswamy S., Hedrick J.K. Robust Adaptive Nonlinear Control of HighPerformance Aircraft, Proceedings of the American Control Conference, 1990. – pp.1279-1283.16. Elliot J.R. NASA’s Advanced Control Law Program for the F-8 Digital Flyby-Wire Aircraft, IEEE Transactions on Automatic Control. Vol.AC-22. – No.5. Oct.1977. – pp. 735 – 757.17. Боткин Н.Д., Кейн В.М., Красов А.И., Пацко В.С. «Управление боковымдвижением самолета на посадке в условиях ветрового возмущения», отчет о НИР.– Ленинград – Свердловск, 1983.

– 78 с.18. Медников В.Н. «Динамика полета и пилотирование самолетов». –Монино, 1976. – 386 с.19. АлексеевВ.М.,ТихомировВ.М.,ФоминС.В.«Оптимальноеуправление». – М.: Наука, 1979. – 432 с.20. Гасс С. «Линейное программирование». – М.: Физматгиз, 1961. – 304 с.21. Гришанин Ю.С., Лебедев Г.Н., Липатов А.В., Степаньянц Г.А. «Теорияоптимальных систем». – М.: МАИ, 1999. – 317 с.22. Понтрягин Л.С. «Принцип максимума в оптимальном управлении». –М.: Наука, 1989.

– 60 с.23. Понтрягин Л.С. и др. «Математическая теория оптимальных процессов».– М.: Наука, 1983. – 393 с.24. Летов А.М. «Математическая теория процессов управления». – М.:Наука, 1981. – 256 с.25. Александров А. А., Кабанов С. А. «Оптимизация посадки беспилотноголетательного аппарата с учетом ограничений на управление», Мехатроника,оптимизация, управление, 2008. №2 (83), стр.

50-5410526. Беллман Р. «Динамическое программирование». – М.: ИИЛ, 1961. – 400с.27. Летов А.М. «Динамика полета и управления». – М.: Наука, 1969. – 360 с.28. Ивашова Н.Д. «Способ повышения безопасности посадки самолета присильном боковом ветре», Труды XXIII Международного научно-техническогосеминара «Современные технологии в задачах управления, автоматики иобработки информации» 14-20 сентября, 2014 г. Алушта. Сборник тезисовдокладов, стр. 14.29.

Лебедев Г.Н., Елисеев В.Д., Ивашова Н.Д. «Постановка задачиавтоматического управления посадочным маневром беспилотного летательногоаппарата при сильном боковом ветре и подходы к ее решению», Труды МАИ,2013. № 70.30. КрасовскийА.А.,ВавиловЮ.А.,СучковА.И.«Системыавтоматического управления летательных аппаратов». – ВВИА им. Жуковского,1985. – 479 с.31.

Мосолов В.Е., Харитонов В.Н. «Системы автоматического управленияугловым движением летательных аппаратов. Учебное пособие». – М.: МАИ, 1995.– 85 с.32. Белогородский С.Л. «Автоматизация управления посадкой самолета». –М.: Транспорт, 1972. – 352 с.33. КотикМ.Г.«Динамикавзлетаипосадкисамолетов».–М.:Машиностроение, 1984.

– 256с.34. Ивашова Н.Д. «Система автоматического управления многорежимнымпосадочнымманевромприсильномбоковомветре»,Авиакосмическоеприборостроение, 2014. № 10, стр.15-2135. ЛебедевГ.Н.,ИвашоваН.Д.«Координированноеуправлениепосадочным маневром при приземлении беспилотного летательного аппарата сучетом действия ветровых возмущений», Авиакосмическое приборостроение,2014. № 4, стр.3-936. http://www.transas.ru/about/press/news/1772610637. Хортон А. «Visual C++ 2010. Полный курс». – М.: Диалектика, 2011.

–1216 с.38. Международная организация гражданской авиации, «Руководство посдвигу ветра на малых высотах», 200539. Лебедев Г. Н., Ефимов А. В., Михайлин Д. А. «Оценка векторасостояния беспилотного летательного аппарата при наличии в объектеуправления нелинейныхэлементов», Вестник Московскогоавиационногоинститута, М.: МАИ, 2012, №1, С. 12-1640.

Калман Р., Бьюси Р. «Новые результаты линейной фильтрации и теориипредсказаний». Техническая механика, 1961, №1, серия D, стр. 123-141.107ПРИЛОЖЕНИЕ108Текст программного модуля ControlSystem.cpp// SDU.cpp: implementation of the SDU class.////////////////////////////////////////////////////////////////////////#include "stdafx.h"#include <iostream>#include "ControlSystem.h"#include "System.h"#include "math.h"using namespace std;#ifdef _DEBUG#define new DEBUG_NEW#endifvoid MatrixMultiply (double* first, double* second, int m, int n, int q, double*result){int i,j,k;for (i = 0; i < m; i++) {for (j = 0; j < q; j++) {*(result + i*q + j) = 0;for (k = 0; k < n; k++) {*(result + i*q + j) += *(first + i*n + k)*(*(second + k*q +j));}}}}int Inverse (double* matrix, int n, double* matrix_inv){int i,j,k;for (i = 0; i < n; i++) {for (j = 0; j < n; j++) *(matrix_inv + i*n + j) = 0;*(matrix_inv + i*n + i) = 1;}//прямой ходdouble a, b;for (i = 0; i < n; i++) {a = *(matrix + i*n + i);for (j = i+1; j < n; j++) {b = *(matrix + j*n + i);for (k = 0; k < n; k++) {*(matrix + j*n + k) = *(matrix + i*n + k)*b - (*(matrix +j*n + k))*a;*(matrix_inv + j*n + k) = *(matrix_inv + i*n + k)*b (*(matrix_inv + j*n + k))*a;;}}}//обратный ходdouble sum;for (i = 0; i < n; i++) {for (j = n-1; j >= 0; j--) {sum = 0;for (k = n-1; k > j; k--)sum += *(matrix + j*n + k)*(*(matrix_inv + k*n + i));if (*(matrix + j*n + j) == 0) {109return 0;}*(matrix_inv + j*n + i) = (*(matrix_inv + j*n + i) sum)/(*(matrix + j*n + j));}}return 1;}void ControlSystem::Life(){///////////////////////////////////////////////////////////////////**/Algorithm_Start();/**/////////////////////////////////////////////////////////////////////Масштабирование сигналов РУС и педалейdouble trim_Xb,trim_Xe,trim_Xn;if(in.trim_Xb_up) trim_Xb = 1;else if(in.trim_Xb_dn) trim_Xb = -1;else trim_Xb = 0;if(in.trim_Xe_l) trim_Xe = 1;else if(in.trim_Xe_r) trim_Xe = -1;else trim_Xe = 0;if(in.trim_Xn_l) trim_Xn = 1;else if(in.trim_Xn_r) trim_Xn = -1;else trim_Xn = 0;doubledoubledoubletr_Xb = LimIntegral("Модель триммера Xb",-0.5,0.5, 0.1)[trim_Xb];tr_Xe = LimIntegral("Модель триммера Xe",-0.5,0.5, 0.1)[trim_Xe];tr_Xn = LimIntegral("Модель триммера Xn",-0.5,0.5, 0.1)[trim_Xn];double Xb = Clip(1)[in.Xbdouble Xe = Clip(1)[in.Xedouble Xn = Clip(1)[in.Xn// const vector3D&W+++=tr_Xb];tr_Xe];tr_Xn];in.W; // добавлено 27.12.12Const<> H1("Высота переключения 1",50);Const<> H2("Высота переключения 2",30);Const<> H3("Высота переключения 3",10);double H22 = in.Vwind.z*40/20 + H3;// Формирование выходовSetActiveContext("Канал рулей высоты");{Const<> Ktang("Ktang в", 5);Const<> Kwz("Kwz в", 0.129);Const<> Ky("Ky в", 0);Const<> Ho("Ho", 30);Const<> Kvy("Kvy в", 0);Const<> Vzad("Vzad", -2);Const<> Kal("Kal в", 0);Const<> Ky2("Ky2 в", -2);Const<> Kvy2("Kvy2 в", 0);Const<> Ktang2("Ktang2 в", 5);Const<> Kwz2("Kwz2 в", 0.129);double U = Ky * (in.Point.y - (H2 - in.Point.x * 1/20)) + Kvy * (in.Vy- Vzad)+ Kal * (in.alphaR - Const<>("alpha zad", 0)) + Ktang *(in.tang - Const<>("tang zad", 0))+ in.W.z * Kwz + Const<>("dV zad", 0);110if (in.Point.y < H3) U = Ky2 * (in.Point.y - (H3 - in.Point.x * 1/30))+ Kvy2 * (in.Vy + 1.33)+ Ktang2 * in.tang + in.W.z * Kwz2;out.Hzad = Ho - in.Point.x * 1/20;out.Fi_r = MP1("Привод ГО",0,30,20,-20,20)[U];out.Fi_l = out.Fi_r;}ReleaseContext();double KursZ, KrenZ, RN, E, Zzad;double KgE, KwxE, KpsiE, KwyE, KzE, KvzE, KyE, KvyE;double KpsiN, KwyN, KputN, KbetaN, KgN, KwxN, KzN, KvzN, KyN, KvyN, PutZ,Ktheta;SetActiveContext("Система управления 0");{SetActiveContext("Канал элеронов 0");{Const<> KgE0("Кgam э",15);Const<> KrenZ0("kren zad", 0);Const<> KwxE0("Кwx э",-10);Const<> KpsiE0("Кpsi э",0);Const<> KwyE0("Кwy э",0);Const<> KzE0("Кz э",0.21);Const<> KvzE0("Кvz э",0.92);Const<> Zzad0("Z zad", 20);Const<> KyE0("Ky э", 0);Const<> KvyE0("Kvy э", 0);Const<> E0("dE zad", 0);Zzad = Zzad0;Zzad = (9*in.Vwind.z+.68*in.Vwind.z*in.Vwind.z)/0.1/40 - 28.8;if ((in.Point.y < H1)) {KgE = KgE0; KwxE = KwxE0;KpsiE = KpsiE0; KwyE = KwyE0;KzE = KzE0; KvzE = KvzE0;KrenZ = KrenZ0; E = E0;KyE = KyE0; KvyE = KvyE0;}}ReleaseContext();SetActiveContext("Канал руля направления 0");{Const<> KpsiN0("Кpsi н",0);Const<> KursZ0("kurs zad", 0);Const<> KwyN0("Кwy н",100);Const<> KputN0("Кput н",0);Const<> PutZ0("put zad", 0);Const<> KbetaN0("Кbeta н",-100);Const<> KgN0("Кgam н",0);Const<> KwxN0("Кwx н",0);Const<> KzN0("Кz н",0);Const<> KvzN0("Кvz н",0);Const<>Const<>Const<>Const<>RN0("RN zad", 0);KyN0("Ky н", 0);KvyN0("Kvy н", 0);Ktheta0("Ktheta н", 0);if ((in.Point.y < H1)) {111KpsiN = KpsiN0; KwyN = KwyN0;KputN = KputN0; KbetaN = KbetaN0;KgN = KgN0; KwxN = KwxN0;KzN = KzN0; KvzN = KvzN0;KursZ = KursZ0; RN = RN0;KyN = KyN0; KvyN = KvyN0;Ktheta = Ktheta0;}}ReleaseContext();}ReleaseContext();SetActiveContext("Система управления с креном");{SetActiveContext("Канал элеронов 1");{Const<> KgE1("Кgam э",15);Const<> KrenZ1("kren zad", 0);Const<> KwxE1("Кwx э",-10);Const<> KpsiE1("Кpsi э",0);Const<> KwyE1("Кwy э",0);Const<> KzE1("Кz э",0.21);Const<> KvzE1("Кvz э",0.92);Const<> KyE1("Ky э", 0);Const<> KvyE1("Kvy э", 0);Const<> E1("dE zad", 0);if ((in.Point.y < H22)) {KgE = KgE1; KwxE = KwxE1;KpsiE = KpsiE1; KwyE = KwyE1;KzE = KzE1; KvzE = KvzE1;KrenZ = KrenZ1; E = E1;KyE = KyE1; KvyE = KvyE1; Zzad = 0;}}ReleaseContext();SetActiveContext("Канал руля направления 1");{Const<> KpsiN1("Кpsi н",0);Const<> KursZ1("kurs zad", 0);Const<> KwyN1("Кwy н",100);Const<> KputN1("Кput н",0);Const<> PutZ1("put zad", 0);Const<> KbetaN1("Кbeta н",-100);Const<> KgN1("Кgam н",0);Const<> KwxN1("Кwx н",0);Const<> KzN1("Кz н",0);Const<> KvzN1("Кvz н",0);Const<>Const<>Const<>Const<>RN1("RN zad", 0);KyN1("Ky н", 0);KvyN1("Kvy н", 0);Ktheta1("Ktheta н", 0);if ((in.Point.y < H22)) {KpsiN = KpsiN1; KwyN = KwyN1;KputN = KputN1; KbetaN = KbetaN1;KgN = KgN1; KwxN = KwxN1;KzN = KzN1; KvzN = KvzN1;KursZ = KursZ1; RN = RN1;112KyN = KyN1; KvyN = KvyN1;Ktheta = Ktheta1;}}ReleaseContext();}ReleaseContext();SetActiveContext("Система управления без крена");{SetActiveContext("Канал элеронов 2");{Const<> KgE2("Кgam э",200);Const<> KwxE2("Кwx э",100);Const<> KpsiE2("Кpsi э",0);Const<> KwyE2("Кwy э",0);Const<> KzE2("Кz э",0);Const<> KvzE2("Кvz э",0);Const<> KyE2("Ky э", 0);Const<> KvyE2("Kvy э", 0);Const<> E2("dE zad", 0);if ((in.Point.y < H3)) {KgE = KgE2; KwxE = KwxE2;KpsiE = KpsiE2; KwyE = KwyE2;KzE = KzE2; KvzE = KvzE2;KyE = KyE2; KvyE = KvyE2;E = E2; Zzad = 0;}}ReleaseContext();SetActiveContext("Канал руля направления 2");{Const<> KpsiN2("Кpsi н",10);Const<> KursZ2("kurs zad", 0);Const<> KwyN2("Кwy н",10);Const<> KputN2("Кput н",0);Const<> PutZ2("put zad", 0);Const<> KbetaN2("Кbeta н",0);Const<> KgN2("Кgam н",0);Const<> KwxN2("Кwx н",0);Const<> KzN2("Кz н",0);Const<> KvzN2("Кvz н",0);Const<>Const<>Const<>Const<>RN2("RN zad", 0);KyN2("Ky н", 0);KvyN2("Kvy н", 0);Ktheta2("Ktheta н", 0);if (in.Point.y < H3) {KpsiN = KpsiN2; KwyN = KwyN2; KputN = KputN2;KbetaN = KbetaN2; KgN = KgN2;KwxN = KwxN2; KzN = KzN2; KvzN = KvzN2;KrenZ = 0; KursZ = KursZ2; RN = RN2;KyN = KyN2; KvyN = KvyN2; PutZ = PutZ2;Ktheta = Ktheta2;}}ReleaseContext();}ReleaseContext();113doubleif (dZif (dZdouble* in.W.ydZ = KzE * (in.Point.z - Zzad) + KvzE * in.Vg.z;> 20) dZ = 20;< -20) dZ = -20;Ue = KgE * (in.gamR - KrenZ) + KwxE * in.W.x + KpsiE * in.psiR + KwyE+ dZ + in.Point.y * KyE + in.Vg.y * KvyE + E;out.El_r = MP1("Привод Элеронов",0,30,20,-20,20)[Ue];out.El_l = -out.El_r;double Un = KpsiN * (in.psiR - KursZ) + KwyN * in.W.y + KputN * (in.PsiR_V PutZ)+ KbetaN * in.betaR+ KgN * in.gamR + KwxN * in.W.x + KzN *in.Point.z+ KvzN * in.Vg.z + RN + in.Point.y * KyN + in.Vg.y * KvyN;out.Vo_r = MP1("Привод ВО",0,30,20,-20,20)[Un];out.Vo_l = -out.Vo_r;/*Kalman*/int i, j;double T0 = 0.2;double g = 9.8;double V0 = 40;double MSE_dn = 10;double alpha0 = 0;double tang0 = 0;double Zb_b = -0.122;double Zdn_dn = -.061;double Mx_b = 553.4;//???double My_b = -0.415;double Mx_wx = -387.4;double My_wy = -12.78;double Mx_wy = -16.6;double My_wx = -3.2;double Mx_de = -2.77;double Mx_dn = -0.5;double My_de = 0;double My_dn = -0.26;//% X = (dZ dbeta dwx dwy dgamma dpsi dbetaw ddelta_betaw)^T;double x0[8] = {0, 1.37, 0, 0, 0, 0, -8, 6};double A[8][8] = {{0, -M_PI*V0/180, 0, 0, 0, M_PI*V0/180, 0, 0},{0, Zb_b, sin(alpha0), cos(alpha0),g*cos(tang0)/V0, 0, Zb_b, 0},{0, Mx_b, Mx_wx, Mx_wy, 0, 0, Mx_b, 0},{0, My_b, My_wx, My_wy, 0, 0, My_b, 0},{0, 0, 1, -tan(tang0), 0, 0, 0, 0},{0, 0, 0, 1/cos(tang0), 0, 0, 0, 0},{0, 0, 0, 0, 0, 0, -1/T0, 180/(M_PI*V0*T0)},{0, 0, 0, 0, 0, 0, 0, 0}};double B[8][2] = {{0, 0}, {0, Zdn_dn}, {Mx_de, Mx_dn}, {My_de, My_dn}, {0, 0}, {0, 0},{0, 0}, {0, 0} };double G[8][1] = {{0}, {0}, {0}, {0}, {0}, {0}, {180/(M_PI*V0*T0)}, {0}};double P0[8][8] = {{0.02, 0, 0, 0, 0, 0, 0, 0},{0, 0.05, 0, 0, 0, 0, 0, 0},{0, 0, 0.05, 0, 0, 0, 0, 0},{0, 0, 0, 0.03, 0, 0, 0, 0},114{0,{0,{0,{0,0,0,0,0,0,0,0,0,0,0,0,0,0.02, 0, 0, 0},0, 0.02, 0, 0},0, 0, 0.02, 0},0, 0, 0, 0.02}};doubledoubledoubledoubledoubledoubledoubledoubleQ0 = MSE_dn*MSE_dn;MSE_dZ = 1;MSE_db = 1;MSE_dwx = 0.1;MSE_dwy = 0.1;MSE_dgam = 0.1;MSE_dpsi = 0.1;R[6][6] = {{MSE_dZ*MSE_dZ, 0, 0, 0, 0, 0},{0, MSE_db*MSE_db, 0, 0, 0, 0},{0, 0, MSE_dwx*MSE_dwx, 0, 0, 0},{0, 0, 0, MSE_dwy*MSE_dwy, 0, 0},{0, 0, 0, 0, MSE_dgam*MSE_dgam, 0},{0, 0, 0, 0, 0, MSE_dpsi*MSE_dpsi}};double H[6][8] = {{1, 0, 0, 0, 0, 0, 0, 0},{0, 1, 0, 0, 0, 0, 0, 0},{0, 0, 1, 0, 0, 0, 0, 0},{0, 0, 0, 1, 0, 0, 0, 0},{0, 0, 0, 0, 1, 0, 0, 0},{0, 0, 0, 0, 0, 1, 0, 0}};double x[8][1];double AT[8][8], GT[1][8], HT[8][6];double P[8][8];FILE *f;if (Time < .1) {memcpy(x, x0, sizeof(x0));memcpy(P, P0, sizeof(P0));}else {f = fopen("Kalman.txt","r");fread(*x, sizeof(double), 8, f);fread(*P, sizeof(double), 64, f);fclose(f);}for (i = 0; i < 8; i++) {for (j = 0; j < 8; j++) {AT[i][j] = A[j][i];GT[0][j] = G[j][0];if (j < 6) HT[i][j] = H[j][i];}}double m81[8][1];double m881[8][8];double m882[8][8];//x = A*x + B*u;MatrixMultiply(&A[0][0], &x[0][0], 8, 8, 1, &m81[0][0]);for (i = 0; i < 8; i++)x[i][0] = m81[i][0];//P = A*P*A' + G*Q*G';//A*P*A'MatrixMultiply(&A[0][0], &P[0][0], 8, 8, 8, &m881[0][0]);MatrixMultiply(&m881[0][0], &AT[0][0], 8, 8, 8, &m882[0][0]);memcpy(P,m882,sizeof(P));115//G*Q*G'MatrixMultiply(&G[0][0], &Q0, 8, 1, 1, &m81[0][0]);MatrixMultiply(&m81[0][0], >[0][0], 8, 1, 8, &m881[0][0]);//P = A*P*A' + G*Q*G';for (i = 0; i < 8; i++)for (j = 0; j < 8; j++)P[i][j] += m881[i][j];//K = P*H'/(H*P*H'+R);double K[8][6];double m68[6][8];double m661[6][6];double m662[6][6];MatrixMultiply(&H[0][0], &P[0][0], 6, 8, 8, &m68[0][0]);MatrixMultiply(&m68[0][0], &HT[0][0], 6, 8, 6, &m661[0][0]);for (i = 0; i < 6; i++)for (j = 0; j < 6; j++)m661[i][j] += R[i][j];Inverse(&m661[0][0], 6, &m662[0][0]);double m861[8][6];double m862[8][6];MatrixMultiply(&P[0][0], &HT[0][0], 8, 8, 6, &m861[0][0]);MatrixMultiply(&m861[0][0], &m662[0][0], 8, 6, 6, &m862[0][0]);memcpy(K, m862, sizeof(K));// X =doublez[0] =z[1] =z[2] =z[3] =z[4] =z[5] =(dZ dbeta dwx dwy dgamma dpsi dbetaw ddelta_betaw)^T;z[6];in.Point.z;in.betaR;in.W.x;in.W.y;in.gamR;in.psiR;//x = x+K*(z - H*x);double m61[6][1];MatrixMultiply(&H[0][0], &x[0][0], 6, 8, 1, &m61[0][0]);for (i = 0; i < 6; i++)z[i] -= m61[i][0];MatrixMultiply(&K[0][0], &z[0], 8, 6, 1, &m81[0][0]);for (i = 0; i < 8; i++)x[i][0] += m81[i][0];//P = (eye(8) - K*H)*P;double e[8][8];for (i = 0; i<8; i++)for (j = 0; j<8; j++)if (i==j) e[i][j] = 1;else e[i][j] = 0;MatrixMultiply(&K[0][0], &H[0][0], 8, 6, 8, &m881[0][0]);for (i = 0; i<8; i++)for (j = 0; j<8; j++)m882[i][j] = e[i][j] - m881[i][j];MatrixMultiply(&m882[0][0], &P[0][0], 8, 8, 8, &m881[0][0]);memcpy(P, m881, sizeof(P));out.W = x[7][0];f = fopen("Kalman.txt","w");116fwrite(*x, sizeof(double), 8, f);fwrite(*P, sizeof(double), 64, f);fclose(f);////////////////////////////////////////////////////////////////////**/Algorithm_Stop();/**///////////////////////////////////////////////////////////////////}.