Мо задачи формирования приращений векторов кажущихся линейной скорости и линейного перемещения

Константы:

dt =0.001c;

dT =0.01c;

k_aks -масштабный коэффициент ЛА;

DAo=[DAxo;DAyo;DAzo] – смещение нулей линейных акселерометров (ЛА);

DMao=[DMaxo 0 0

DMayo 0

0 0 DMazo];

- погрешности масштабных коэффициентов ЛА;

Cab – матрицаориентации осей чувствительности ЛА (их приведения к осям измерительного модуля (ГСП) – приборным осям xb,yb,zb);

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

n(i+1)=[nxb;nyb;nzb] – текущие на высокой частоте (с дискретностью dt) значениявектора кажущегося ускорения в проекциях на связанные (приборные) оси ГСП, измеряемые ЛА;

FKDn(j)=[FKDnx;FKDny;FKDnz] – значенияоценок смещений нуля ЛА из предыдущего цикла dT (на предыдущем шаге), формируемые в задаче выработки корректур с дискретностью Tz (в частности, это могут быть поправки, учитывающие влияние изменений температуры).

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

b1(j+1)=[b1xb;b1yb;b1zb] - приращения составляющих вектора кажущейся линейной скорости в связанных (приборных) осях (осях ГСП), формируемые с дискретностью dT;

b2(j+1)=[b2xb;b2yb;b2zb] - приращения составляющих вектора кажущегося линейного перемещения в связанных осях (осях ГСП), формируемые с дискретностью dT.

 

 

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

Алгоритм задачи, решаемой с дискретностью dt (оператор i) внутри рабочего цикла dT:

b1a_(i+1)=b1a_(i)+k_aks*n(i+1)*dt;

b2a_(i+1)=b2a_(i)+(b1a_(i+1)+b1a_(i))*dt/2;

при i=0

b1a_(i)=0;b2a_(i)=0;

В конце цикла dt

( значенияпервых и вторых интегралов в осях чувствительности ЛА на шаге dT)

b1a_(j+1)=b1a_(i+1);

b2a_(j+1)=b2a_(i+1); -

% формирование первых и вторых интегралов в осях ГСП с учетом погрешностей масштабных коэффициентов и смещений нуля (м/c²) ЛА

В цикле dT ( оператор j)

b1b_(j+1)=Cab*[(E - DMao)*b1a_(j+1) - DAo*dT];

b2b_(j+1)=Cab*[(E - DMao)*b2a_(j+1) - DAo*(dT^2)/2];

b1(j+1)=b1b_(j+1) - FKDn(j)*dT;

b2(j+1)=b2b_(j+1) - FKDn(j)*(dT^2)/2;

 

МО задачи ориентации

Константы:

dT=0.01с;

dro; % рад/с; - угловая скорость дополнительного вращения ГСП;

qo; % рад, - начальное значение угла поворота ГСП относительно осей объекта;

Ko; % рад, - начальное значение курса;

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

Omh_(j) – значения вектора угловой скорости вращения географического трехгранника - (из задачи преобразования сигналов ЛА на навигационные оси и их интегрирования);

FKOr=[ ; ; ] – значения оценок погрешностей построения географического трехгранника - (из задачи фильтрации);

FKDr(j)=[FKDrx(j);FKDry(j);FKDrz(j)] - значения оценки дрейфов гироскопов (из задачи фильтрации);

FKDMg(j)=[FKDMgx(j);

FKDMgy(j);

FKDMgz(j)] - значения оценок погрешностей масштабных коэффициентов гироскопов (из задачи фильтрации);

Алгоритм:

roo=qo-Ko; % рад; - начальное значение угла поворота ГСП относительно ;

ro(j+1)=ro(j)+dro*dT;

;

- матрица ориентации, характеризующая положение расчетных осей измерительного модуля (ГСП) относительно навигационных осей ;

Omh(j+1)=Omh_(j)-FKOr*1/Tz;

Omb_(j+1)=Cbh(j+1)’*Omh(j+1)+dro;

Алгоритм:

Omh(j+1)=Omh_(j) – FKOr(j)*1/Tz;

Omb(j+1)=(E - FKDMg)*Omb_(j+1) - FKDr(j); -сигналы поступают на ДМгироскопов в каналы управления вращением ГСП;

МО задачи преобразования сигналов ЛА на навигационные оси и интегрирования

Константы:

dT=0.01с;

go=9.780318;% м/с2 - ускорение силы тяжести на экваторе;

bet=0.0053024;

bet_1=0.0000059;

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

% параметры земного эллипсоида (Красовского)

a =6378245 м; % большая полуось

e2 =0.0066934; % квадрат эксцентриситета

dro; % рад/с;

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

b1(j+1)=[b1xb;b1yb;b1zb] - приращения составляющих вектора кажущейся линейной скорости в связанных (приборных) осях (осях ГСП), (поступают с дискретностью dT из задачи формирования приращений векторов кажущихся линейной скорости и линейного перемещения);

b2(j+1)=[b2xb;b2yb;b2zb] - приращения составляющих вектора кажущегося линейного перемещения в связанных (приборных) осях (осях ГСП), (поступают с дискретностью dT из задачи формирования приращений векторов кажущихся линейной скорости и линейного перемещения);

Cbh – значенияматрицы ориентации для перехода от связанных (приборных) осей (осей ГСП) к горизонтной системе координат с географической ориентацией осей (формируются в задаче ориентации);

roo – начальное значение угла ro (формируются в задаче ориентации);

Vho=[VEo;VNo;VHo] – значения составляющих вектора линейной скорости в начальный момент времени;

FLho=[Fio;Lao;ho] – значения координат места ИНС в начальный момент времени;

FKDV(j)=[FKDVE;FKDVN;FKDVH] - значенияоценок погрешностей скорости из предыдущего цикла dT (на предыдущем шаге), формируемые в задаче выработки корректур с дискретностью dT;

FKDFLh(j)=[FKDFi;FKDLa;FKDh] - значенияоценок погрешностей координат из предыдущего цикла dT (на предыдущем шаге), формируемые в задаче выработки корректур с дискретностью Tz;

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

Vh(j+1) -значения вектора линейной скорости в проекциях на оси ENH;

DSh(j+1) - значения приращений на конечном интервале временивектора линейных перемещений в проекциях на оси ENH;

Fi(j+1) – широта места;

La(j+1) – долгота места;

h(j+1) – высота.

Omh_(j+1)=[OmE_;OmN_;OmH_] -значения вектора угловой скорости вращения ENH;

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

% «Вредные» ускорения

BAe(j+1)=VH(j)*(2*U+dLa(j))*cos(Fi(j)) - VN(j)*(2*U+dLa(j))*sin(Fi(j));

BAn(j+1)=VE(j)*(2*U+dLa(j))*sin(Fi(j))+VH(j)*dFi(j);

ge=go*(1+bet*(sin(Fi))^2-bet_1*(sin(2*Fi))^2);

BAh(j+1)=ge - VE(j)*(2*U+dLa(j))*cos(Fi(j)) - VN(j)*dFi(j);

BA=[BAe;BAn;BAh];

% Вычисление составляющих вектора линейной скорости и приращений линейных перемещений

dC(j+1)=(Cbh(j+1) - Cbh(j))/dT;

dVkh(j+1)=Cbh(j+1)*b1(j+1) - dC(j+1)*b2(j+1);

dVh(j+1)=dVkh(j+1) – BA*dT;

Vh(j+1)=Vh(j)+dVh(j+1) – FKDV(j)*dT/Tz;

[VE;VN;VH]=Vh(j+1);

При j=0

Vh(j)=Vho;

dSkh(j+1)=Cbh(j+1)*b2(j+1);

dSh(j+1)=Vh(j)*dT+dSkh(j+1) – (1/2)*BA*dT^2;

% Вычисление радиусов кривизны для эллипсоида (общеземного или Красовского)

RE=(a/sqrt(1-e2*(sin(Fi))^2))+h;

RN=(a*(1-e2)/sqrt((1-e2*(sin(Fi))^2)^3))+h;

% Вычисление приращений декартовых координат и географических координат

DSh(j+1)=DSh(j)+dSh(j+1);

[DSE;DSN;DSH]=DSh;

При j=0

DSh(j)=0;

La(j+1)=La(j)+dSE(j+1)/(RE*cos(Fi(j+1))) – FKDLa(j)*dT/Tz;

Fi(j+1)=Fi(j)+dSN(j+1)/RN – FKDFi(j)*dT/Tz;

h(j+1)=h(j)+dSH(j+1) – FKDh(j)*dT/Tz;

При j=0

La(j)=Lao; Fi(j)=Fio; h(j)=ho;

% Вычисление угловых скоростей

dLa(j+1)=VE(j+1)/(RE*cos(Fi(j+1)));

dFi(j+1)=VN(j+1)/RN;

OmE_(j+1)= - dFi(j+1);

OmN_(j+1)=(U+dLa(j+1))*cos(Fi(j+1));

OmH_(j+1)=(U+dLa(j+1))*sin(Fi(j+1));

Omh_(j+1)=[OmE_;OmN_;OmH_];