4.2. Мо задачи начальной («грубой») ориентации объекта

Константы:

dT=0.005 с;

g =9.78049 м/с2; % ускорение силы тяжести;

U=7.292116e-5; % угловая скорость суточного вращения Земли;

Входные переменные:

b1o=[b1xb;b1yb;b1zb] – начальные приращения составляющих вектора кажущейся линейной скорости в связанных осях (осях ИБ БИИМ) из задачи формирования приращений векторов кажущихся линейной скорости и линейного перемещения;

dfb1o=[dfxb1;dfyb1;dfzb1] – начальные значения вектора угла поворота (вектора квазикоординат – первого интеграла от вектора угловой скорости) в проекциях на связанные оси (оси ИБ БИИМ) из задачи формирования приращений вектора угла поворота (вектора квазикоординат;

Roo - начальное значение угла поворота ИБ относительно осей корпуса БИИМ (объекта), (поворот вокруг оси Z , положительное направление - против часовой стрелки);

Fio – значение широты места в точке запуска БИИМ.

Выходные переменные:

Cbh_o – начальное значение матрицы матрицы ориентации для перехода от осей ИБ БИИМ к горизонтной системе координат;

Lhb_o - начальное значение кватерниона Lhb.

Алгоритм задачи, решаемой с дискретностью dT

nsb_o=b1o/dT;

Cbo_o=[cos(Roo) (-1)*sin(Roo) 0;

sin(Roo) cos(Roo) 0;

0 0 1];

nso_o=Cbo_o*nsb_o;

[nsxo;nsyo;nszo]=nso_o;

% формирование начальных значений sin и cos углов качки

sinPsio=nsyo/g;

cosPsio=sqrt(1 – sinPsio^2);

sinTetao= - nsxo/(g*cosPsio);

cosTetao=sqrt(1 – sinTetao^2);

% формирование матрицы Cohk_o ориентации для перехода от осей объекта к горизонтной системе координат, связанной с курсом объекта

ck11o=cosTetao;

ck12o=0;

ck13o=sinTetao;

ck21o=sinPsio*sinTetao;

ck22o=cosPsio;

ck23o= - sinPsio*cosTetao;

ck31o= - cosPsio*sinTetao;

ck32o=sinPsio;

ck33o=cosPsio*cosTetao;

Cohk_o=[ck11o ck12o ck13o;

ck21o ck22o ck23o;

ck31o ck32o ck33o];

% формирование начального значения sin и cos угла курса (при условии, что угловые скорости качки, рыскания и модуляционного вращения ИБ равны нулю)

Omb_o=dfb1o/dT;

Omhk_o=Cohk_o*Cbo_o*Omb_o;

OmEko=Omhk_o(1,1);

OmNko=Omhk_o(2,1);

sinKo= - OmEko/U*cos(Fio);

cosKo=OmNko/U*cos(Fio);

% формирование начального значения матрицы направляющих косинусов Coh_o

d11o=cosKo*cosTetao+sinKo*sinPsio*sinTetao;

d12o=sinKo*cosPsio;

d13o=cosKo*sinTetao - sinKo*sinPsio*cosTetao;

d21o= - sinKo*cosTetao+cosKo*sinPsio*sinTetao;

d22o=cosKo*cosPsio;

d23o= - (sinKo*sinTetao+cosKo*sinPsio*cosTetao);

d31o= - cosPsio*sinTetao;

d32o=sinPsio;

d33o=cosPsio*cosTetao;

Coh_o=[d11o d12o d13o

d21o d22o d23o

d31o d32o d33o];

%Формирование начальных значений углов качки и курса

Psio=atan2(d32o, sqrt(1 – d32o^2));

Tetao=atan2( - d31o, d33o);

Ko=atan2(d12o, d22o);

при

atan2<0

Ko=atan2(d12o, d22o)+2*pi;

% формирование начального значения матрицы ориентации Cbh_o;

Cbh_o=Coh_o*Cbo_o;

[c11o c12o c13o

c21o c22o c23o

c31o c32o c33o]=Cbh_o;

% формирование начального значения кватерниона Lho_o;

L0=cos(Ko/2)*cos(Psio/2)*cos(Tetao/2)+sin(Ko/2)*sin(Psio/2)*sin(Tetao/2);

L1=cos(Ko/2)*cos(Tetao/2)*sin(Psio/2)+sin(Ko/2)*sin(Tetao/2)*cos(Psio/2);

L2=cos(Ko/2)*sin(Tetao/2)*cos(Psio/2) – sin(Ko/2)*cos(Tetao/2)*sin(Psio/2);

L3=cos(Ko/2)*sin(Tetao/2)*sin(Psio/2) – sin(Ko/2)*cos(Tetao/2)*cos(Psio/2)];

Lho_o=[L0;L1;L2;L3];

MLho=[L0,-L1,-L2,-L3;

L1, L0,-L3, L2;

L2, L3, L0,-L1;

L3,-L2, L1, L0];

Lob_o=[cos(Roo/2);

0;

0;

sin(Roo/2)];

% формирование начального значения кватерниона Lhb_o;

Lhb_o=MLho*Lob_o;