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

Ковариационный канал ФК (вычисляется с дискретностью 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

 

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

 

Оценочный канал ФК (вычисляется с дискретностью 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.1. Назначение, технические и точностные характеристики ИСОН

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

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

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

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

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

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

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

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

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

- курса географического...................................................................... 0,4°secj;

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

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

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

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

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

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

Условия эксплуатации:

- линейная скорость............................................................................ до 60 узлов,

- углы качки......................................................................................... до 45°,

- угловые скорости качки.................................................................. до 30°/с,

- линейные ускорения........................................................................ до 2g,

- диапазон рабочих температур......................................................... 0…+55°C,

- диапазон предельных температур (для хранения изделия)........... -20…+60°C,

- допустимый уровень одиночных ударов....................................... до 20g.

БИИМ на ВОГ

БИИМ на ВОГ (прибор ВИИМ – рис. 1) является основным прибором изделия «Мининавигация-К» и состоит из измерительного модуля (ИМ) с блоком электроники и основания с элементами системы автокомпенсационного вращения.

Состав ИМ:

· три ВОГ (ВГ 951);

· три линейных акселерометра АК10/4 (с кварцевым чувствительным элементом);

· плата вторичного источника питания (ВИП) чувствительных элементов;

· плата системы термостатирования (СТС) с нагревательными транзисторами и терморезистором;

· плата датчика температуры (ДТ);

· модуль аналогового ввода-вывода (АЦП) DM6430HR-1;

· модуль процессорный СМС16686GX300HR-32;

· расширитель интерфейса РСМ-3610-А;

· источник питания IPWR104-L60W;

· плата индикации.

Состав основания:

· микроконтроллер МК-БСС управления моментным двигателем и датчиком угла;

· моментный двигатель БМДР-12-2;

· преобразователь угла ИПУ-ДУЦ;

· токоподвод неограниченного угла вращения;

· арретир.

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

Рис. 1.Прибор ВИИМ (внешний вид со снятой крышкой)

1 - измерительный модуль; 2 - гироскопы; 3 – акселерометры;

4 - бортовой вычислитель; 5 – основание

 

4.2. Описание программного обеспечения

 

На рис. 2 приведена блок-схема, характеризующая работу основных задач программного обеспечения ИСОН на основе БИИМ на на ВОГ в пакете Matlab (Simulink), которая состоит из следующих основных блоков:

Блок 1. Чтение реальных данных записи показаний ЧЭ ИБ и внешних источников информации о параметрах движения объекта из файла массива данных стендовых или корабельных испытаний ИСОН:

§ показания ИИМ: 1) ВОГ: dfxyz = , 2) акселерометров: b1 = ;

§ угол Ro поворота ИБ ИИМ относительно корпуса ИИМ и соответствующая ему матрица Cob_pr перехода от осей объекта к осям ИБ ИИМ;

§ показания ПА СНС: по координатам - FiLa_gps = [φ; λ] и составляющим вектора скорости V_gps=[ ]);

§ истинная (модельная) скорость - V_m, используемая для имитации данных лага и GPS;

§ курс по показаниям гирокомпаса: K_gk.

Блок 2. Выработка параметров ориентации ИБ ИИМ:

§ Определение матрицы Cbh_pr;

При этом используется результат работы ФК (оценки вектора состояния x (16): FKCDr =[ ; ; ]; FKDMg =[ ; ; ]; FKRDr_b =[ ; ; ; ]).

Блок 3. Выработка параметров поступательного движения:

§ для преобразования кажущихся ускорений используется матрица Cbh_pr перехода от связанных с ИБ ИИМ осей к географическим осям (см. Блок 2);

§ выработка составляющих вектора линейной скорости (V=[ ]) и вектора переносной угловой скорости (OmENh_=[ ]);

§ выработка текущих координат (FiLa =[ ], h = h).

При этом используется результат работы ФК (оценки вектора состояния x (16): FKDn =[ ; ; ]; FKDV =[ ; ; ]; FKDFDL =[ ; ]; FKDh = ).

Блок 4. Формирование при имитационном моделировании сигналов внешних источников информации о движении объекта

(координат FiLa_gps = [ ] (3) и скорости V_gps по показания ПА СНС; скорости по показаниям относительного лага - VL; курса по показаниям мультиантенной ПА СНС - Ks; датчика изменения высоты - h_et, методическая погрешность которого для условий надводного корабля – вертикальная качка - моделируется марковским процессом второго порядка).

При этом используется результат работы ФК по оценке течений (оценки вектора состояния x (16): FKVT =[ ; ].

 

Блок 5. Определение и запись погрешностей работы ИСОН в файлы (для последующего построения результирующих графиков):

§ «файл №1»: Or_VG_**.mat (погрешности ИСОН в выработке параметров ориентации);

§ «файл №2»: DV_VG_**.mat (погрешности ИСОН в выработке линейной скорости движения);

§ «файл №3»: DS_VG_**.mat (погрешности ИСОН в выработке координат места);

§ «файл №4»: Dr_VG_**.mat (результат оценки погрешностей ВОГ);

§ «файл №5»: Dn_VG_**.mat (результат оценки погрешностей акселерометров);

§ «файл №6»: D Mg_VG_**.mat (результат оценки погрешностей масштабный коэффициентов ВОГ);

§ «файл №7»: Krdr_VG_**.mat (результат оценки коэффициентов модели румбовых дрейфов ВОГ (см. выражение (10)).

ВНИМАНИЕ: ** - формируются оператором (в зависимости от исследуемого режима).

Блок 6. Вывод значений диагональных элементов ковариационной матрицы, выработанной ФК (ковариационный канал ФК)

Блок 7. Фильтр Калмана (см. рис. 3).

7.1: Формирование вектора измерений Z (зивисит от исследуемого режима и устанавливается соответствующими положениями ключей № 1…3);

7.2: Формирование матриц состояния F и измерений H. Вид последней зивисит от исследуемого режима и устанавливается соответствующими положениями ключей № 4…8;

7.3: Реализация дискретного алгоритма ФК, включающего ковариационный и оценочный каналы и блок формирования оценок румбовых дрейфов на основе оценок коэффициэнтов их расчетной модели. На выходе оценки элементов вектора состояния x (16), которые используются при работе ИСОН (см. рис. 2). Предусмотрена также возможность подключения (ключ №9) процедуры отбраковки измерений при превышении их допустимых значений (3…5 ), расчитываемых по данным ковариационного канала.

 

Рис. 2. Блок-схема основных задач программного обеспечения ИСОН на основе БИИМ на ВОГ в пакете Matlab (Simulink)

 

 

Рис. 3. Фильтр Калмана (Блок 7 рис.2

При выполнении лабораторной работы используются следующие файлы, расположенные в рабочей папке d:\student\Emlib\Prog_Lab_EP \:

· kurs№n1.m -программа загрузки 3-х часового массива данных ИБ на ВОГ из соответствующего ему файла (kurs№.mat), “ № ”соответствует номеру варианта (по указанию преподавателя (см. ниже из таблиц)) - в лаб.раб. №1;

· kurs№n2.m -программа загрузки 6-ти часового массива данных ИБ на ВОГ из соответствующего ему файла (kurs№.mat), “ № ”соответствует номеру варианта (по указанию преподавателя (см. ниже из таблиц)) - в лаб.раб. №2;

· loader_Ncn.m -программа загрузки 4-х часового массива данных ИБ на ВОГ (получен при мореходных испытаниях) из соответствующего ему файла (2_Mr4.mat)- в лаб.раб. №3;

· IS24re_VG_StRdr_bqN_dat.m ( в лаб.раб. №1,2 )

и IS24re_VG_MrRdr_dat03_04p1.m ( в лаб.раб. №3 ) -программы загрузки начальных условий (дискретность поступления данных ИБ на ВОГ и дискретность формирования измерений, начальные значения параметров ориентации из режима «грубой» выставки и параметров движения объекта, параметры фигуры и гравитационного поля Земли, априорные значения оценок смещений нулей акселерометров и гироскопов, погрешностей масштабных коэффициентов и коэффициентов румбовых дрейфов гироскопов, значения матриц Po, Q, H и R, характеризующих настройкуалгоритма фильтра Калмана и т.д.);

· M_IS24re_VG_StRdr_bqN_Lit.mdl ( в лаб.раб. №1,2 )

и M_IS24re_VG_MrRdr_Lit_f.mdl ( в лаб.раб. №3 ) -программы, описывающие дискретные алгоритмы различных режимов работы ИСОН, визуализацию и запись в file.mat значений погрешностей ИСОН и используемые для обработки данных соответственно стендовых и корабельных испытаний ИБ БИИМ.

Порядок выполнения работ

 

Массивы данных стендовых испытаний ИБ на ВОГ с реверсными поворотами: 360⁰ (1оборот - 5мин) с постоянной скоростью и ступенчатым изменением ее знака, на различных румбах: 0⁰, -60⁰, -120⁰, -180⁰, -240⁰ и -300⁰, сформированные на частоте 50 гц длительностью каждый около 8 ч из результатов предварительно проведенных испытаний, которые занесены в память PC. Сигналы ПА СНС, с соответствующими моделями погрешностей, были получены с использованием имитатора.