П.4. Алгоритмы основных задач БИНС на ДУС с вычислением в задаче ориентации в качестве промежуточного кинематического параметра вектора Эйлера

4.1. МО задачи формирования приращений на рабочей частоте (частоте решения задачи ориентации) векторов кажущихся линейной скорости, линейного перемещения и вектора квазикоординат

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

Константы:

dt=0.001 c;

dT=0.005 c;

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

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

DMao=[DMaxo 0 0

DMayo 0

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

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

weight_Dn=[weight_Dnx 0 0

0 weight_Dny 0

0 0 weight_Dnz];

– веса текущих оценок смещений нулей ЛА.

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

n(i+1)=[nxb;nyb;nzb] – текущие на высокой частоте (с дискретностью dt); значениявектора кажущегося ускорения в проекциях на оси ИБ БИИМ (yb - продольная ось, zb – ортогонально палубе объекта, xb – правый борт), измеряемые ЛА

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

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

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

% формирование первых и вторых интегралов в осях чувствительности акселерометров на шаге 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) - weight_Dn*FKDn(j)*dT;

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

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

Константы:

dt=0.001 c;

dT=0.005 c;

k_gyr -масштабный коэффициент ДУС;

DGo=[DGxo;DGyo;DGzo] -смещение нулей ДУС;

DMgo=[DMgxo 0 0

DMgyo 0

0 0 DMgzo]; -погрешности масштабных коэффициентов ДУС;

Cgb -матрицаориентации осей чувствительности ДУС (их приведения к осям измерительного блока (ИБ) – приборным осям xb,yb,zb);

weight_Dr=[weight_Drx 0 0

0 weight_Dry 0

0 0 weight_Drz];

– веса текущих оценок дрейфов VG;

weight_DMg=[weight_DMgx 0 0

0 weight_DMgy 0

0 0 weight_DMgz];

- веса текущих оценок погрешностей масштабных коэффициентов ДУС

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

Gb(i+1)=[Gx;Gy;Gz] – текущие значениявектора абсолютной угловой скорости ИБ БИИМ в проекциях на связанные оси, измеряемые гироскопами (ДУС) на высокой частоте (с дискретностью dt);

FKDr(j)=[FKDrx;FKDry;FKDrz] – значенияоценок дрейфов ДУС из предыдущего цикла dT (на предыдущем шаге), формируемые в задаче выработки корректур с дискретностью Tz (в частности, это могут быть поправки, учитывающие влияние изменений температуры);

FKDMg(j)=[FKDMgx;FKDMgy;FKDMgz] – значенияоценок погрешностей масштабных коэффициентов ДУС из предыдущего цикла dT (на предыдущем шаге), формируемые в задаче выработки корректур с дискретностью Tz (в частности, это могут быть поправки, учитывающие влияние изменений температуры);

MFKDMg=[FKDMgx 0 0

FKDMgy 0

0 0 FKDMgz];

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

dfb1(j+1)=[dfxb1;dfyb1;dfzb1] – значения вектора угла поворота (вектора квазикоординат – первого интеграла от вектора угловой скорости на шаге dT) в проекциях на связанные оси (оси ИБ БИИМ), формируемые с дискретностью dT;

dfb2(j+1)=[dfxb2;dfyb2;dfzb2] – значения вектора угла поворота (вектора квазикоординат - второго интеграла от вектора угловой скорости на шаге dT) в проекциях на связанные оси (оси ИБ БИИМ), формируемые с дискретностью dT;

 

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

dfb1_(i+1)=dfb1_(i)+k_gyr*(Gb(i+1)+Gb(i))*dt/2;

dfb2_(i+1)=dfb2_(i)+(dfb1_(i+1)+dfb1_(i))*dt/2;

при i=0

dfb1_(i)=0;dfb2_(i)=0;

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

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

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

% формирование первых и вторых интегралов в осях ИБ с учетом погрешностей масштабных коэффициентов и смещений нуля (рад/c) ДУС

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

df1b_(j+1)=Cgb*[(E - DMgo)*df1_(j+1) - DGo*dT];

df2b_(j+1)=Cgb*[(E - DMgo)*df2_(j+1) - DGo*(dT^2)/2];

 

dfb1(j+1)=(E – weight_DMg*MFKDMg(j))*df1b_(j+1) – weight_Dr*FKDr(j)*dT;

dfb2(j+1)=(E – weight_DMg*MFKDMg(j))*df2b_(j+1) – weight_Dr*FKDr(j)*(dT^2)/2;