Программный код начальной выставки

% ----- 1ая Часть ----- Начальная выставка

% Начальные показатели датчиков(арифм. среднее элементов каждого столбца)

f0 = mean(imu(1:60*fs,5:7))';

w0 = mean(imu(1:60*fs, 2:4))';

phi = phi0/180*pi;

lambda = lambda0/180*pi;

gamma = - atan2(f0(1,1),f0(3,1));

theta = atan2(f0(2,1),sqrt((f0(1,1))^2+(f0(3,1))^2));

Z1 = [cos(gamma) 0 sin(gamma);

0 1 0;

-sin(gamma) 0 cos(gamma)];

Z2 = [1 0 0;

0 cos(theta) -sin(theta);

0 sin(theta) cos(theta)];

Ypsi = Z2*Z1*w0;

psi = atan2(Ypsi(1,1),Ypsi(2,1));

S0 = [cos(psi)*cos(gamma)-sin(psi)*sin(theta)*sin(gamma) sin(psi)*cos(gamma)+cos(psi)*sin(theta)*sin(gamma) -cos(theta)*sin(gamma);

-sin(psi)*cos(theta) cos(psi)*cos(theta) sin(theta);

(cos(psi)*sin(gamma))+(sin(psi)*sin(theta)*cos(gamma)) sin(psi)*sin(gamma)-cos(psi)*sin(theta)*cos(gamma) cos(theta)*cos(gamma)];

B0 = [-sin(lambda) cos(lambda) 0;

-cos(lambda)*sin(phi) -sin(lambda)*sin(phi) cos(phi);

cos(lambda)*cos(phi) sin(lambda)*cos(phi) sin(phi)];

A0 = S0 * B0;

3. Модель погрешностей инерциальных датчиков.

В систему моделирования полёта воздушного судна добавляем погрешности.

Модель погрешностей представлена ниже.

Рис. 11. Модель погрешностей.

Программный код модели погрешностей

for j = 1:1

w = [0 0 0 0 0 0 0];

for i = 2:N

w(i,2:4)= w(i-1,2:4) + q(i,1:3) * dt * 0.001/3600;

w(i,5:7)= w(i-1,5:7) + q(i,4:6) * dt * 10^-5;

end

imu(1:N,2:4) = imu1(1:N,2:4) + 0.5/3600*randn(N,3) + imu1(1:N,2:4)*Gamma' + w(1:N,2:4);

imu(1:N,5:7) = imu1(1:N,5:7) + 0.001*randn(N,3) + imu1(1:N,5:7)*Thetta' + w(1:N,5:7);

% Переводим из градусов в радианы столбцы показателей гироскопов

imu(:,2:4) = imu(:,2:4)*pi/180;

3. Вывод

В ходе выполнения лабораторной работы была разработана модель траектории полёта воздушного судна Cessna Citation X из города Сингапур в город Бангкок по показаниям идеальных датчиков угловой скорости и ньютонометра.

1 Пример обозначения примечания.

2 Долготу и широту.

3 Точки взлёта и точки посадки.

4 Под данными выражениями понимаются выражения, которые в ходе вычисления компилятором Matlab влияют на дальнейшие вычисления.