Инерциальные и интегрированные навигационные системы

МЕТОДИЧЕСКИЕ УКАЗАНИЯ

к выполнению лабораторных работ по дисциплине

Инерциальные и интегрированные навигационные системы

 

Раздел 2. Исследование алгоритмов и модели погрешностей в пакете Matlab (Simulink) интегрированных систем ориентации и навигации на основе БИИМ на ВОГ, стандартной и мультиантенной ПА GPS и лага по данным их стендовых и мореходных испытаний

 

профессор кафедры информационно-навигационных систем СПбГУ ИТМО   д.т.н., проф.Г.И. Емельянцев ____________________В.Г. Пешехонов "_____"__________________2004 г. д.т.н., проф.____________________В.Г. Пешехонов "_____"__________________2004 г. д.т.н., проф.____________________В.Г. Пешехонов "_____"__________________2004 г.

Автор:

	Зав. кафедрой ЛИНС СПб ГЭТУ   д.т.н., проф.Ю.В. Филатов   Зав. кафедрой СПб ГУАП   д.т.н., проф.Л.А. Северов   Зам. зав. кафедрой ИНС СПбГУ ИТМО   д.т.н., проф.В.В.Серегин ____________________В.Г. Пешехонов "_____"__________________2004 г. д.т.н., проф.____________________В.Г. Пешехонов "_____"__________________2004 г. д.т.н., проф.____________________В.Г. Пешехонов "_____"__________________2004 г.
		Начальник центра профессионального образования ЦНИИ «Электроприбор»   д.т.н.О.А. Степанов ____________________В.Г. Пешехонов "_____"__________________2004 г. д.т.н., проф.____________________В.Г. Пешехонов "_____"__________________2004 г. д.т.н., проф.____________________В.Г. Пешехонов "_____"__________________2004 г.

 

 

Санкт-Петербург

СОДЕРЖАНИЕ

Перечень основных обозначений и сокращений......................................................................... 3

1. Цель цикла работ........................................................................................................................ 4

2. Основные вопросы теории........................................................................................................ -

3. Основные сведения.................................................................................................................... -

3.1.Измерения............................................................................................................................ -

3.2.Расчетная модель погрешностей ИСОН........................................................................... 5

3.2.1. Модель погрешностей ВОГ.................................................................................... -

3.2.2. Модель погрешностей линейных акселерометров.............................................. 6

3.2.3. Линеаризованная относительно алгоритма идеальной работы модель погрешностей БИИМ в выработке параметров ориентации и навигационных параметров... 7

3.2.4. Расчетная модель погрешностей системы............................................................ 8

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

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

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

4.1. Назначение, технические и точностные характеристики ИСОН на основе БИИМ на ВОГ............................................................................................................................................ -

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

4.1.2. БИИМ на ВОГ............................................................................................................ -

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

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

5.1. Лабораторная работа № 1. Режим начальной выставки и калибровки ИСОН на основе БИИМ на ВОГ и ПА GPS в условиях стенда.............................................................................. -

5.2. Лабораторная работа № 2. Обсервационный и автономный режимы работы ИСОН на основе БИИМ на ВОГ и ПА GPS при стендовых испытаниях................................................. 20

5.3. Лабораторная работа № 3.Обсервационный и автономный режимы работы ИСОН на основе БИИМ на ВОГ, ПА GPS и лага при мореходных испытаниях..................................... 22

Содержание итогового отчета....................................................................................................... 27

Литература…………………………………………………………………………………..……-

Приложения………………………………………………………………………………….....28

П1. Программыдля обеспечения лабораторных работ

kurs№n1.m, loader_Ncn.m, IS24re_VG_StRdr_bqN_dat.m ……………………………-

ПЕРЕЧЕНЬ ОСНОВНЫХ ОБОЗНАЧЕНИЙ И СОКРАЩЕНИЙ

БИИМ - бескарданный инерциальный измерительный модуль;

ВОГ - волоконно-оптический гироскоп;

ДП - динамические параметры;

ДУС – датчик угловой скорости;

ИБ - измерительный блок;

ИМ – измерительный модуль;

ИИМ - инерциальный измерительный модуль;

ИСК - инерциальная система координат;

ИСОН - интегрированная система ориентации и навиагции;

ЛА - линейный акселерометр;

МГХ - массогабаритные характеристики;

МПО - морской подвижный объект;

НП - навигационные параметры;

ПА - приемная аппаратура;

СНС - спутниковая навигационная система;

ФК - фильтр Калмана

ЧЭ – чувствительные элементы;

 

GPS – глобальная СНС (США);

PC - персональный компьютер.

 

 

1. Цель цикла работ

 

Цель работ заключается в изучении структуры построения, режимов работы, дискретных алгоритмов интеграции данных БИИМ на ДУС, ПА GPS и лага с использованием фильтра Калмана, моделей погрешностей ИСОН и их анализ по результатам стендовых и мореходных испытаний.

 

2. Основные вопросы теории

 

Изучение особенностей дискретных алгоритмов фильтровой задачи ИСОН для режимов начальной выставки и калибровки, обсервационного и автономного, а также обсервационного и автономного режимов работы в условиях подвижного объекта [1,2]:

· формирование измерений;

· описание вектора состояния, матрицы динамики системы и матрицы измерений;

· ковариационный канал ФК;

· оценочный канал;

· формирование обратных связей.

 

Основные сведения

 

Фильтровая задача математического обеспечения ИСОН по интеграции данных БИИМ на ВОГ, стандартной и мультиантенной ПА GPS и лага с использованием ФК

Измерения

· Скоростные измерения по GPS:

(1)

где - приращения декартовых координат МПО в проекциях на географические оси, вырабатываемые по данным БИИМ о составляющих вектора линейной скорости на интервале измерений приращений декартовых координат в доплеровском канале ПА СНС, пересчитанных от точки размещения приемной антенны СНС к месту установки ИБ БИИМ

,

где - приращение матрицы ориентации МПО за время , - отстояние приемной антенны СНС от ИБ БИИМ).

Соотношения (1) могут быть приведены к виду:

(2)

где - погрешности доплеровского канала ПА GPS/ГЛОНАСС - дискретные белые шумы с дисперсиями на частоте 1Гц, ; - реальные шумы скоростных измерений.

Позиционные измерения

(3)

· Курсовое измерение

, (4)

· Скоростные измерения по лагу:

(5)

где - восточная и северная составляющие морских течений, как основная методическая погрешность относительных лагов, могут быть представлены в виде [1]

(6)

где

(7)

, уз, ч^-1, ч^-1, - линейная скорость объекта в узлах, - “белый” шум единичной интенсивности; - шумы измерений, включающие неизмеряемую лагом поперечную составляющую вектора скорости корабля и инструментальные погрешности лага и аппроксимированные белыми шумами с дисперсией на частоте 1 гц.

Для построения расчетной модели погрешностей ИСОН восточную и северную составляющие морских течений аппроксимируем марковскими процессами первого порядка с интервалом корреляции порядка 5400с и .

 

Модель погрешностей ВОГ

Модель дрейфов ВОГ может быть аппроксимирована в виде суммы нескольких составляющих:

· погрешности калибровки начального смещения “нуля” и его нестабильности в пуске, т.е. погрешности практически постоянной на достаточно длительном интервале времени, которую вследствие отсутствия данных о спектре ее изменчивости целесообразно описывать винеровским процессом при соответствующих начальных условиях;

· погрешности масштабного коэффициента, которая определяет составляющую, пропорциональную измеряемой величине;

· погрешности знания румбовых дрейфов ВОГ, которые обусловлены влиянием внешнего магнитного поля и могут быть представлены в виде первой гармоники от угла поворота ИБ;

· составляющей, обусловленной неортогональностями осей измерительного блока ВОГ;

· “шумовой” составляющей, характеризующей флуктуационные погрешности гироскопов

(8)

где – квазисистематическая составляющая с начальным уровнем , характеризуемым погрешностью калибровки смещения “нуля” ВОГ от пуска к пуску, и интенсивностью , обусловленной нестабильностью смещения “нуля” в пуске из-за температурных деформаций гироскопа; – погрешность масштабного коэффициента гироскопа, а - измеряемая им угловая скорость; - составляющие, обусловленные неортогональностями (аппроксимированными соответствующими винеровскими процессами) осей измерительного блока ВОГ; - ”белошумная” составляющая c интенсивностью ; - “белый” шум единичной интенсивности;

- румбовые дрейфы ВОГ, обусловленные в основном влиянием на гироскопы внешнего аномального магнитного поля.

 

3.2.2. Модель погрешностей линейных акселерометров

Модель погрешностей линейных акселерометров, как правило, имеет следующие составляющие:

· погрешность калибровки начального смещения “нуля” и его нестабильность в пуске, практически постоянную на достаточно длительном интервале, которая может описываться либо случайной постоянной величиной либо интегралом от белого шума;

· погрешность масштабного коэффициента, которая определяет составляющую, пропорциональную измеряемой величине;

· составляющую, обусловленную неортогональностями осей измерительного блока акселерометров;

· шумовую составляющую, характеризующую флуктуационные погрешности датчиков.

С учетом этого инструментальные погрешности линейных акселерометров могут быть описаны следующим образом:

(9)

где - погрешность калибровки начального смещения “нуля” и его нестабильность в пуске; - интенсивность изменения квазисистематической составляющей; - погрешность масштабного коэффициента акселерометра; - составляющие, обусловленные неортогональностями осей измерительного блока акселерометров; - белошумная составляющая погрешности, характеризуемая среднеквадратическим отклонением на частоте обработки данных; - измеряемое акселерометрами кажущееся ускорение в осях измерительного блока ; - “белый” шум единичной интенсивности.

При формировании расчетной модели погрешностей ИСОН использовались следующие аппроксимации:

· смещения нулей гироскопов и акселерометров , изменения систематических составляющих погрешностей масштабных коэффициентов ВОГ от запуска к запуску и их изменчивость в пуске - были аппроксимированы (из-за отсутствия достоверных данных об их спектральном составе) соответствующими винеровскими процессами;

· погрешности знания румбовых дрейфов ВОГ были представлены в виде первой гармоники от угла поворота ИБ

, (10)

где для условий стенда (для корабельных условий: ); - искомые коэффициенты разложения, аппроксимированные соответствующими винеровскими процессами (i = x, y); здесь - курс, - угол поворота ИБ относительно корпуса БИИМ.

3.2.3. Линеаризованная относительно алгоритма идеальной работы модель погрешностей БИИМ в выработке параметров ориентации и навигационных параметров

Линеаризованная относительно алгоритма идеальной работы модель погрешностей БИИМ в выработке параметров ориентации и навигационных параметров, включающая модели погрешностей в решении задач ориентации, преобразования сигналов акселерометров на навигационные оси и их интегрирования (вычисления составляющих векторов линейной скорости в проекциях на навигационные оси и географических координат места), может быть представлена в следующем виде [1, 2]:

(11)

где

- погрешности БИИМ в аналитическом моделировании горизонтной системы координат с географической ориентацией осей (географического сопровождающего трехгранника) ;

- погрешности в выработке составляющих вектора линейной скорости;

- погрешности в выработке географических широты, долготы и высоты места;

- проекции нескомпенсированных дрейфов ДУС и так называемых "вычислительных" дрейфов на оси горизонтной системы координат; - проекции инструментальных погрешностей акселерометров на оси горизонтной системы координат; - погрешность компенсации вертикальной составляющей вектора нормальной силы тяжести, обусловленная погрешностями знания координат места; , - составляющие уклонения отвесной линии (УОЛ) и аномалия силы тяжести; - ускорение силы тяжести нормальной Земли; - средний радиус Земли; - угловая скорость суточного вращения Земли; - погрешности компенсации ”вредных” ускорений по соответствующим осям, выражения для которых имеют вид:

(12)

- составляющие угловой скорости вращения горизонтного трехгранника с географической ориентацией осей, которые определяются как

, , , (13)

- проекции кажущегося ускорения на оси горизонтной системы координат, которые определяются выражениями:

(14)

 

3.2.4. Расчетная модель погрешностей системы

В этом случае расчетная модель погрешностей ИСОН будет иметь вид

(15)

где x - вектор состояния системы

здесь - погрешности в выработке соответственно восточной, северной и вертикальной составляющих вектора линейной скорости; - погрешности выработки географических координат места; - переходная на шаге матрица системы (15) для момента времени ,

, (17)

где - единичная матрица размерности ; - матрица динамики системы, ненулевые элементы которой определяются соотношениями:

,

;

; ;

; ;

;

;

; , (18)

здесь и - текущие значения составляющих вектора угловой скорости вращения трехгранника и вектора кажущегося ускорения в месте установки ИБ

БИИМ, вычисляемые по данным ИСОН [1]; - значения соответственно угловой скорости вращения Земли, широты места и восточной составляющей линейной скорости объекта относительно Земли; - элементы матрицы направляющих косинусов, определяющих взаимную ориентацию связанного с ИБ трехгранника (b) и горизонтного географического трехгранника ENH (h); - матрица, определяющая влияние вектора входных шумов с ковариациями , где - диагональная матрица интенсивностей входных шумов , соответствующая модели погрешностей системы (15) с учетом моделей погрешностей гироскопов (8, 10) и акселерометров (9).

БИИМ на ВОГ

БИИМ на ВОГ (прибор ВИИМ – рис. 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. Сигналы ПА СНС, с соответствующими моделями погрешностей, были получены с использованием имитатора.

 

Литература

 

1. Анучин О.Н., Емельянцев Г.И. (под общей ред. акад. РАН В.Г.Пешехонова). Интегрированные системы ориентации и навигации для морских подвижных объектов. - СПб., 2004.
2. Лекции проф. Емельянцева Г.И. (Электронная версия в сети ЦНИИ «Электроприбор», раздел 3. Корабельные интегрированные системы ориентации и навигации (ИСОН).

 

Приложения

П1. Программы для обеспечения лабораторных работ

kurs№n1.m, loader_Ncn.m, IS24re_VG_StRdr_bqN_dat.m

 

 

Программа kurs№n1.m

загрузкив рабочую область Workspace Matlab массивов kurs№.mat результатов стендовых испытаний ИБ БИИМ на ВОГ

 

clear all;

load kurs0.mat;

n=541000;

x=x';

[row, col]=size(x)

dlin=1:n;

f=50;

dt=0.02;

time=(dlin)*dt;

time=time';

TIME=n*dt;

Ax(:,1)=x(dlin,2)-0.0*x(dlin,3)+0.0018*x(dlin,4);

Ay(:,1)=x(dlin,3)-0.0*x(dlin,2)+0.0000136*x(dlin,4);

Az(:,1)=x(dlin,4)+0.00375*x(dlin,2)+0.0008*x(dlin,3);

Gx(:,1)=x(dlin,5)+0.00083*x(dlin,7)-0.0005*x(dlin,6);

Gy(:,1)=x(dlin,6)-0.00025*x(dlin,7)-0.0016*x(dlin,5);

Gz(:,1)=x(dlin,7)+0.0023*x(dlin,5)+0.0017*x(dlin,6);

Gamma(:,1)=x(dlin,8);

clear x;

Программа loader_Ncn.m

загрузкив рабочую область Workspace Matlab массива load 2_Mr.mat результатов мореходных испытаний ИБ БИИМ на ВОГ, стандартной и мультиантенной GPS и компаса

clear all;

load 2_Mr.mat;

dt=0.01;

n=2260000;%n=1900000;

x=x';

[row, col]=size(x)

row=n;

c=100000;

time(:,1)=x(c:row,1)-x(c,1);

Ax(:,1)=x(c:row,2)-0*x(c:row,3)+0.0035*x(c:row,4);

Ay(:,1)=x(c:row,3)-0*x(c:row,2)+0.0000136*x(c:row,4);

Az(:,1)=x(c:row,4)+0.00375*x(c:row,2)+0.0008*x(c:row,3);

Gx(:,1)=x(c:row,5)-0.0005*x(c:row,6)+0.00083*x(c:row,7);

Gy(:,1)=x(c:row,6)-0.0016*x(c:row,5)-0.00025*x(c:row,7);

Gz(:,1)=x(c:row,7)+0.0023*x(c:row,5)+0.0017*x(c:row,6);

Gamma(:,1)=x(c:row,12);

La_s(:,1)=x(c:row,8);

Fi_s(:,1)=x(c:row,9);

V_s(:,1)=x(c:row,10);

K_gku(:,1)=x(c:row,11);

Temper(:,1)=x(c:row,13);

K_mrk(:,1)=x(c:row,14);

clear x;

 

 

Программа IS24re_VG_StRdr_bqN_dat.m

загрузкив рабочую область Workspace Matlab массива необходимых констант (дискретность поступления данных испытаний ИБ на ВОГ, GPS, компаса и лага, масштабные коэффициенты гироскопов и акселерометров и априорные значения коэффициентов моделей их погрешностей, начальные значения параметров ориентации из режима «грубой» выставки и параметров движения объекта, параметры фигуры и гравитационного поля Земли, значения коэффициентов имитационных моделей погрешностей GPS и лага, значения настройки параметров фильтра Калмана и т.д.)

format long;

%параметры дискретизации данных

dt=0.02;

dT=0.02;

dT1=0.1;

Tz=1;

n_z=Tz/dT1;

Tq=dT1;

%Характеристики нормальной Земли и нормального поля (межд.асамб.Люцерна, 1967)

U=7.2921151467e-5;

R=6371000;

Om=U;

go=9.780318;

bet=0.0053024;

bet_1=0.0000059;

ge_o=go*(1+bet*(sin(fio*pi/180))^2-bet_1*(sin(2*fio*pi/180))^2);

%параметры общеземного эллипсоида

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

al=1/298.26;% сжатие

b=a*(1-al);

e2=(a^2-b^2)/a^2; % квадрат эксцентриситета

%Координаты точки старта

fio=59.9583;

lambdao=30;

ho=0;

Vo=0;

%kurs=0;

Roo=359.999*pi/180;

Ko=-0.5*pi/180;

Psio=0;

Tetao=0;

%kurs=-60;

%Roo=107.9434*pi/180;

%Ko=(-0.5+300)*pi/180;

%kurs=-120;

%Roo=358.4676*pi/180;

%Ko=(-0.5+240)*pi/180;

%kurs=-180;

%Roo=234.285*pi/180;

%Ko=(-0.5+180)*pi/180;

%kurs=-240;

%Roo=140.6186*pi/180;

%Ko=(-0.5+120)*pi/180;

%kurs=-300;

%Roo=242.5121*pi/180;

%Ko=(-0.5+60)*pi/180;

OmEo=-Vo*cos(Ko)/R;

OmNo=U*cos(fio*pi/180)+(Vo*sin(Ko))/R;

OmHo=U*sin(fio*pi/180)+(Vo*sin(Ko)/R)*tan(fio*pi/180);

Omho=[OmEo;OmNo;OmHo];

% Model GPS po cursu

Sigma1DKs=60*5e-6;

MUDKs=1/1800;

Sigma2DKs=180*5e-6;

% Model Laga

SigmaVTE=0.2;

MUVTE=1/5400;

SigmaVTN=0.3;

MUVTN=1/5400;

% Model shumov Z