3.3. Ковариационный канал фильтра Калмана

Ковариационный канал ФК (вычисляется с дискретностью Tz в фоне с момента прихода измерений)

Pk=Po; % - начальное значение на первом шаге

Pp=Fkp*Pk*Fkp'+(Fkp*Tz)*Q*(Fkp'*Tz);

Pk=(Pp+Pp')/2;

KF=Pk*H'*(inv(H*Pk*H'+R));

Pk=(E-KF*H)*Pk*(E-KF*H)’+KF*R*KF’;

(здесь Fkp= )

• режим калибровки (при наличии курсовых измерений), P0=P0gps; Q=Qgps;

• обсервационный режим (GPS) (включается по признаку прихода данных GPS)

• обсервационный режим (GPS+лаг) (включается по признаку прихода данных GPS и лага), Q=QgpsL;

• автономный режим (включается по признаку отсутствия данных GPS), Q=QgpsL

3.4. Оценочный канал фильтра Калмана и формирование обратных связей

Оценочный канал ФК (вычисляется с дискретностью Tz в фоне с момента прихода измерений)

X(1:24);

X1=X(1:9,1);

X2=X(10:24,1);

Xpr=Fkp(k+1/k)*X(k);

x1pr=[0;0;0;0;0;0;0;0;0];

Xpr=[x1pr;X2pr];

X(k+1)=Xpr+KF*z(k+1);

(фиксируется запаздывание dtj времени окончания вычисления оценок с момента прихода измерений и соответствующая этому запаздыванию матрица Fkp(dtj))

X=Fkp(dtj)*X;

Выходные данные фильтровой задачи - управления, вырабатываемые в ФК, подаются в обратную связь:

alpha=X(1,1);

beta=X(2,1);

gamma=X(3,1);

FKOr=[beta;gamma;alpha];% - со знаком «-» и весом 1/Tz (с обнулением управлений через интервал времени Tz) на вход алгоритма задачи ориентации (см. Приложения к методическим указаниям …, п. 4.3. П.4)

FKDVE=X(4,1);

FKDVN=X(5,1);

FKDVH=X(6,1);

FKDFi=X(7,1);

FKDLa=X(8,1);

FKDh=X(9,1); % - со знаком «-» и весом dT/Tz (с обнулением управлений через интервал времени Tz) на входы соответствующих интеграторов алгоритма задачи преобразования сигналов акселерометров на навигационные оси и интегрирования (см. Приложения к методическим указаниям …, п. 4.4. П.4)

FKDrx= - (X(10,1)+(RAxo+X(19,1))*cos(q)+(RBxo+X(20,1))*sin(q));

FKDry= - (X(11,1)+(RAyo+X(21,1))*cos(q)+(RByo+X(22,1))*sin(q));

FKDrz= - X(12,1);

FKDnx=X(13,1); FKDny=X(14,1); FKDnz=X(15,1);

FKDMgx= - X(16,1); FKDMgy= - X(17,1); FKDMgz= - X(18,1);

FKVTE=X(23,1);FKVTN=X(24,1); % - со знаком «-» в выходные сигналы соответственно гироскопов и акселерометров и со знаком «+» в выходные сигналы лага.

4. Описание лабораторной установки

4.1. Назначение, технические и точностные характеристики исон

на основе БИИМ на ВОГ

4.1.1. Инерциальный измерительный модуль

Инерциальный измерительный модуль на базе ВОГ, акселерометров и встроенного микрокомпьютера предназначен для выработки курса и динамических параметров движения быстроходных судов, катеров, яхт и наземного транспорта.

При интегрировании с ПА GPS/ГЛОНАСС точность определения координат места, скорости и путевого угла соответствует параметрам используемого приемника с учетом инерциального сглаживания помех на интервале до 100 с.

Интерфейс канала связи — RS232 по протоколу NMEA 0183. Выдача потребителю угла курса, динамических и навигационных параметров движения по интерфейсу RS 232/422/485 на частоте до 100 Гц.

Электропитание системы – 24В постоянного тока. Потребляемая мощность – 60 Вт.

Время готовности – не более 60 мин (в зависимости от температуры окружающей среды).

Габаритные размеры – 252x342 мм, Масса – 17 кг.

Допустимые значения погрешностей выработки

- курса географического 0,4sec;

- углов качки и рыскания 0,1;

- составляющих угловой скорости по трем осям 0,1/с ;

- составляющих линейной скорости качки 0,1 м/с;

- вертикальных перемещений 0,1 м;

- вертикальных ускорений 0,1 м/с2.

Указанные погрешности выработки динамических параметров движения обеспечиваются при качке до 15.