Алгоритм адаптивного двухшагового расширенного фильтра Калмана в задаче совместной оценки навигационного вектора и смещения нулей датчика МЭМС в слабосвязанной комплексированной навигационной системе Текст научной статьи по специальности «Математика»

УДК 629.7.052.82(075.8)

А.С. Конаков, В.В. Шаврин, Д.О. Ноздреватых, А.А. Савин, В.И. Тисленко

Алгоритм адаптивного двухшагового расширенного фильтра Калмана в задаче совместной оценки навигационного вектора и смещения нулей датчика МЭМС в слабосвязанной комплексированной навигационной системе

Рассмотрены алгоритмы совместного оценивания навигационного вектора потребителя и неизвестного смещения нуля акселерометра и гироскопа в слабосвязанной комплексированной навигационной системе, представляющей собой объединение двух навигационных систем: спутниковой и бесплатформенной инерциальной, основанных на двухшаговом фильтре Калмана и адаптивном двухшаговом расширенном фильтре Калмана. Приведены результаты математического моделирования, представлены статистические характеристики оценок навигационного вектора и смещений нуля акселерометра и гироскопа, полученные при использовании данных алгоритмов.

Ключевые слова: двухшаговый расширенный фильтр Калмана, адаптивная фильтрация, интегрированные инерциальные и спутниковые навигационные системы, микроэлектромехани-ческие системы, смещение нуля акселерометра и гироскопа.

Интегрирование бесплатформенных инерциальных навигационных систем (БИНС), в которых используются микроэлектромеханическе системы (МЭМС), и спутниковых навигационных систем (СНС) позволяет повысить точность определения навигационного вектора (НВ), который включает в себя координаты, скорость, угловую ориентацию. При этом в течение определенного интервала времени возможно определение НВ с погрешностью не выше заданной. Совместная (интегрированная) обработка информации с использованием конечных показаний двух систем лежит в основе построения слабосвязанной спутниковой комплексированной навигационной системы (СКНС). Погрешности оценок навигационного вектора в таких системах не достигают минимально возможного значения, а при работе на длительном интервале времени среднеквадратическое отклонение данных оценок начинает увеличиваться, что в теории фильтрации динамических систем носит название расходимости оценок.

Основными источниками погрешностей в СКНС являются не включаемые в модель остаточные погрешности определения координат и скорости в СНС, неизвестные смещения нуля и масштабные коэффициенты гироскопов и акселерометров (модели сигналов гироскопов и акселерометров можно найти в [1-4].)

В настоящее время для устранения погрешностей определения НВ, связанных с вектором масштабных коэффициентов и вектором смещений нуля, используют различные процедуры калибровки. Оценки параметров МЭМС, полученные в ходе калибровки, используются в качестве начальных в дальнейшей процедуре фильтрации. На данный момент существует несколько процедур калибровки, подробно описанных в [5]: шестипозиционный метод, модифицированный шестипозиционный метод, многопозиционный и модифицированный многопозиционный методы. Наибольшее распространение получил модифицированный многопозиционный метод, реализация которого предполагает наличие наклонно-поворотного стола (НПС), который позволяет с высокой точностью определить смещения нуля и масштабные коэффициенты как для акселерометров, так и для гироскопов. Недостаток лабораторных калибровок заключается в необходимости использования сложного оборудования, невозможности проведения калибровки в полевых условиях непосредственно перед началом решения навигационной задачи, что является очень важным фактором, так как параметры датчиков в значительной степени различаются от включения к включению датчика [5].

Постановка и решение задачи. В работе рассмотрен метод минимизации погрешностей фильтрации НВ, основанный на совместном оценивании НВ и неизвестных смещений нуля сигналов МЭМС, в предположении, что масштабные коэффициенты и неортогональности осей МЭМС точно известны и скомпенсированы. Для совместного оценивания может быть использован двухша-

24 ЭЛЕКТРОНИКА, ИЗМЕРИТЕЛЬНАЯ ТЕХНИКА, РАДИОТЕХНИКА И СВЯЗЬ

говый фильтр Калмана (TKF - Two Stage Kalman Filter) [2]. Методы теории байесовской фильтрации предполагают задание модели нелинейной дискретной динамической системы в терминах уравнений для вектора состояния в пространстве состояний [3]. В общем виде уравнения для СКНС,

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

Xk = fk -1 (xk-I,bk-i,nX-1,nb -1), (1)

x b

где bk - вектор неизвестных смещений нуля; nk - шум состояний, nk - шум вектора неизвестных смещений нуля; fk (•) - известная нелинейная функция, допускающая разложение в ряд Тейлора, а уравнения наблюдений, связывающие переменные состояния с наблюдаемыми сигналами:

z k = h k (xk ,bk ,nk ), (2)

где zk - вектор наблюдений; nZ - шум наблюдений; hk (•) - нелинейная функция, связывающая

наблюдения, векторы смещения и состояния и допускающая разложение в ряд Тейлора. Начальные

условия для системы (1) являются случайными с заданными корреляционными свойствами для xq .

Случайные процессы nx, nj^, n^ взаимно некоррелированные, с нулевыми средними значениями и гауссовой плотностью распределения вероятностей (ПРВ). Начальные условия: :xq = E[xq ] -начальная оценка вектора состояния; bo = E[bo] - начальная оценка вектора смещения нуля;

- ковариационная матрица смещений нуля в начальный момент времени;

R = E

bob0 Xo x0 "k “f

ковариационная матрица вектора состояния в начальный момент времени;

.x xT~

- ковариационная матрица шума наблюдений; Q = E

ковариационная

матрица шума состояний; Qb = E

ковариационная матрица шума в модели вектора неиз-

вестных смещений.

Алгоритм TKF позволяет оценить как вектор состояния, так и вектор смещения нуля. Однако TKF является оптимальным алгоритмом, обеспечивающим минимальную среднеквадратическую ошибку (СКО) оценок состояния лишь для линейных динамических систем. Задача оценивания НВ и смещений нуля МЭМС в СКНС является существенно нелинейной [8]. Для фильтрации нелинейных динамических систем используются различные алгоритмы, позволяющие аппроксимировать апостериорное распределение состояния гауссовской плотностью вероятностей. Наиболее прост и лаконичен подход расширенного фильтра Калмана (Extended Kalman Filter -EKF), в котором для прогноза и коррекции оценок используется разложение нелинейных функций в ряды Тейлора при ограничении линейными членами [6]. Для двухшаговых алгоритмов фильтра Калмана [7] применяется такой же подход. Данный алгоритм носит название расширенного двухшагового фильтра Калмана (Two Stage Extended Kalman Filter - TEKF) [5].

Основная идея TEKF состоит в разбиении процесса фильтрации на два практически обособленных процесса, связанных специальными матричными коэффициентами. Первый фильтр называется фильтром, свободным от смещений (в нем происходит оценивание вектора состояния без учета того факта, что оно может изменяться), второй - фильтр смещений (в нем происходит оценивание смещений). В применяемой модификации TEKF принято, что смещения описываются системой линейных дифференциальных уравнений (в дискретном случае - разностных уравнений). Далее формируется оценка вектора состояния, учитывающая возможную эволюцию смещений. Как и в любой модификации фильтра Калмана, вычисления происходят в два этапа: предсказание и коррекция. Ниже приведены аналитические соотношения, используемые в TEKF [4].

Этап предсказания:

Фильтр, свободный от смещений:

x-= fk-1 (x+-1,b +-1,0,q); (3)

РГ=^+-1 (і+-і .ь+-і)+О*-і-

Фильтр смещений:

Ь- = А к-1ЬЛ-і; рк "=Ак-1ркк-1Ак-1+оь -1-

Коррекция оценок вектора состояния с учетом смещений:

х- = хк + ик (х Л-1,Ьк-1)Ь- к и к-1;

рк -=рг+ик (*+-1 ,ьк-1)рГ ик (хк-1 .ьк-1)-

Этап коррекции:

Фильтр. свободный от смещений:

Кх = РТнк (х- ж) (х- .Ь-)рГнк (х- ,ь--)

где К - коэффициент усиления.

к И к

-1

I -К к

Нк(х-,ь-)

где I - единичная матрица. Фильтр смещений:

% = х- к К к

-Ь к (х к ,ь - ,о)

Нк (х- ,ь-)ркх-нк (х- ,ь-) кИк к NkРкь“N1

рЬк

Рк

1 - кЬ N к

рЬ-.

рк ;

ьк=ь-ккЬ

(4)

(5)

(6)

(7)

(8)

(9)

(10)

(11)

(12)

(13)

(14)

2 к - ^ Ьк

Коррекция оценок вектора состояния с учетом смещений:

РГ =РГ+' (х- ДК+'[ (х - Д) (15)

х+= 'к (х - Д)Ь+. (16)

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

^к = Ик (хк ,Ь-)ик (х^-1 ,Ьк^) + ®к (х- ,Ь— ); ик (х+_1,Ь+_1) = )Г - ^ П_1

1 - ОЬ-1

ркь-

и к =

F (х (-1 ,^-1)-1 (х к-1 ,ь+-1)к Вк-1 (х к-1 ,ь +ч) Ук (і -,ь -)=и к (х к-1,Ьк-1)-К к Nk; ик =(ик+1- ик+1) А кькк;

1--1;

\Ьтт Т

ок=ок к ик к1ОЬ ит к1-

Дополнительные начальные условия, необходимые для работы ТЕКЕ: х+ = хо - УоЬо; Ьк = Ьо; Р0х+ = Р0х - УоР0ЬУ0Т; Уо = Р0хЬ

(17)

(18)

(19)

(20)

(21) (22)

(23)

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

*(+-,.■>+-,)) (к-1,Ьк-1,0,0)

Хк-1 =Хк-1 Ьк-1 =Ь+-1

(х+-1 ,>+-,)=) (х к-1 ,ь к-1 А°)

Нк (х- ,Ь-)=-дХь к (хк ,Ьк ,п|) Ок (- ,Ь- )=-дЬьк (хк ,Ьк ,п|)

х к-1=хк-1 Ь к-1 =Ь+-1

Хк =х- ’

Ьк =Ь-

Г5гк 1 8гк-1 +8у к-1т 03х3

8ук = [I - 2^з - Й - Й>ш]к-1 +Гі3х3 - С(8Як-1) а к-1т + > а т + т 7а

8як _ 8як-1 + О (8як-0ГІ3х3 - С(8як-1)>+Ью)т _О (8Чк -1^ _

хк=хк Ьк =Ь-

Для использования алгоритма ТЕКЕ в СКНС необходимо сформулировать задачу определения НВП в терминах марковской теории фильтрации. В СКНС происходит фильтрация не самого НВП, а поправок, которыми необходимо корректировать процесс решения навигационной задачи в БИНС [4]. В данном случае вектор состояния состоит из поправок к координатам (8г), скоростям (8у), кватерниону (Sq), отвечающему за ошибку поворота связанного базиса, в котором измеряются ускорения (а) и угловые скорости (ш), и навигационного базиса, в котором необходимо получить НВП, 8хТ = [г 8у Sq] и имеет размерность 10. Уравнения состояния, определяющие динамику развития погрешностей во времени, имеют следующий вид [5]:

(24)

где т - интервал поступления данных; йз - кососимметрическая матрица угловой скорости вращения Земли, вычисленная в связанном базисе; й - кососимметрическая матрица угловой скорости вращения связанной системы координат относительно навигационной системы координат; йью -кососимметрическая матрица смещений нуля гироскопа Ью ; Ьа - смещения нуля акселерометра; О ^к-1), С^кч) - матрицы, составленные из элементов кватерниона по специальным правилам,

описанным в [4]; У а - шум акселерометра; У^ - шум гироскопа.

Векторы Ьа, Ью образуют вектор смещений Ь . В данной работе принято, что смещения нуля остаются постоянными во времени, что справедливо для большинства современных МЕМС на интервале времени порядка нескольких минут. Целесообразность оценивания данных параметров в том, что они могут значительно отличаться от включения к включению [8].

В СКНС в качестве наблюдений используется разность между показаниями координат (ГБИНС - ГСНС) и скоростей ( убинс - УСНС), определенными в двух системах. С учетом этого уравнения наблюдений имеют следующий вид:

"Аг] = Г ГБИНС - ГСНС '

Ау уБИНС - уСНС

= Ннав8х,

(25)

где матрица Ннав определена следующим образом:

Нн

Е3х3 03х3 04х3 03х3 03х3 03х3 03х3

_03х3 Е3х3 °4х3 03х3 03х3 О3Х3 03х3_

Уравнения (24), (25) представляют собой конкретную реализацию уравнений (1), (2) в СКНС. Отметим особенность сформулированной задачи совместной фильтрации НВП и неизвестного смещения в СКНС. Она состоит в том, что наблюдения (25) напрямую не зависят от вектора смещений Ь . Это может быть причиной больших величин среднеквадратической погрешности оценки вектора смещений и приводить к неустойчивости процесса фильтрации.

Проведенное моделирование, результаты которого представлены ниже, показывает, что ТЕКЕ не обладает приемлемой точностью, оценки параметров МЭМС обладают дисперсией, сравнимой с

абсолютной величиной данных параметров. Нами было выдвинуто предположение, которое объясняет полученные результаты следующим образом. Низкая точность обусловлена значительными изменениями параметров МЭМС как между двумя включениями, так и в процессе решения навигационной задачи. Для устранения вышеперечисленных факторов было решено применить адаптивный алгоритм фильтрации (адаптивный двухшаговый расширенный фильтр Калмана (ATEKF — Adaptive Two Stage Extended Kalman Filter) [2]).

При выводе соотношений, определяющих оптимальный дискретный фильтр в линейной задаче, P. Калман использовал принцип ортогонального проецирования, согласно которому новая информация о векторе состояния (обновляющий процесс), содержащаяся в полученном наблюдении ортогональна всем полученным наблюдениям вплоть до данного момента времени. Из этого предположения при условии, что фильтр является оптимальным, следует, что обновляющий процесс представляет собой некоррелированный случайный процесс.

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

C; = E

Г Л -| \/ г „ -| \T 1

(z k - hk xk ,b k ,с )(zk - hk x k ,b - ,c )_

В линейных задачах динамической фильтрации расчетное значение ковариационной матрицы обновляющего процесса определяется соотношением [3]

Ck = Н k Pk hT + Rk. (26)

Эмпирическая оценка для (3) на k-м шаге может формироваться в скользящем окне по M выборочным значениям обновляющего процесса в виде

kT

C k =77^ I (zi - hi [x,-,b-,С )(zi - hi Tx- ,b-,C ) . (27)

M li=k -M +l ^ ^

В адаптивном двухступенчатом алгоритме ATEKF (Adaptive Two Stage Extended Kalman Filter),

рассмотренном в [2], матрица C; формируется с учетом расчетного соотношения в виде

C k =а; Ck =Xk Н k pk"Hk +R k, где а - скалярный коэффициент, называемый фактором забывания; X - скалярный коэффициент, связывающий ковариационную матрицу вектора состояния, вычисленную в фильтре на этапе предсказания и определенную эмпирически (P; = X;P; ).

Поскольку матрица Rk изменяется с течением времени по известному закону, то все различие между ковариационными матрицами обусловлено расхождением между ковариационной матрицей вектора состояний, вычисленной в фильтре и реально существующей. Следовательно, вполне оправдано предположение, что X; =а; . Фактор забывания определяется в виде [2]

а; = max <

l,

trace С; 1

trace Ck ]

(28)

где 1гаее[^] - оператор вычисления следа матрицы.

При использовании ЛТЕКР соотношения (4), (6), (21), (22) из [6] в соответствии с [2] изменяются и имеют вид

р(х+-1.4-1)^ (х+-1,ь+-1)+ог _

ь

px - =«,x pk = а;

pkb" =“k

А k-lpkb-+lAT-l +Q;-l

uk = Uk +lQ k

pb-pk+l

qx = Qx+Uk+lQb Uk+l.

(29)

(Зс)

(31)

(32)

Факторы забывания для фильтра свободного от смещения и фильтра смещений вычисляются с использованием соотношения (5). ковариационные матрицы определяются соотношениями

C k =

l

k

I

M li=k -M +1

T

(zi- Nib-)(zi- Niъ-) ;

Ck = h ; Pkx"

iT

ъ-*тг

Hk + Nk РГ Nk + R;

для фильтра смещений и

C k =

k

I

M li=k -M +1

T

(z- - hi ( ,b- ,0)i - hi (-,b- ,0)) ;

Ck = H k Pk~ HT + R k

для фильтра, свободного от смещений.

Математическое моделирование. Было проведено моделирование работы предложенного алгоритма. Параметры моделирования приведены в табл. 1 и соответствуют реальным устройствам. В качестве MЭMC использован датчик Analog Devices ADIS 16354, параметры которого получены при лабораторной калибровке [5].

Параметры приемника CHC соответствуют приемнику среднего ценового диапазона. CKO определения координат по каждой из осей составляет 3 м, CKO оценки скорости - 0,03 м/с.

Таблица 1

Параметры моделирования

Параметр/ Датчик Акселерометр Гироскоп

Масштабный коэффициент 1,2% 0,3%

Смещение -0,025 м/с2 0,068 град/с

Неортогональность осей -0,014 град -0,01 град

СКО шума (1 с) 0,046 м/с2 0,016 град/с

Моделировалось сложное криволинейное движение с ускорением по каждой из осей 1 м/с2 и угловой скоростью по каждой из осей 6 град/с. Общее время работы принято равным 600 с. Для определения статистических характеристик получен ансамбль из 150 реализаций. Усреднением были найдены СКО-оценок координат, скорости, смещения нуля акселерометра, смещения нуля гироскопа.

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

Таблица 2 сти всех получаемых оценок. Во избежа-

Результаты моделирования ние подобной ситуац™ фактор забыт-

ния в фильтре смещений искусственно

ограничивался.

Результаты моделирования при использовании алгоритма ТЕКЕ представлены в табл. 2.

Исследована зависимость СКО-оценки НВ от ширины окна М в (4), используемого для определения ковариационной матрицы. Результаты представлены в табл. 3.

CKO оценок координат, м 1,97

CKO оценок скорости, м/с 0,0061

CKO оценок смещения нуля гироскопа, град/с 0,0102

CKO оценок смещения нуля акселерометра, м/с2 0,028

Т аблица 3

Зависимость СКО оценок параметров от ширины скользящего окна при

Ширина окна, отсчеты 10 25 50 75 100 150

CKO координат, м 0,6721 0,5972 0,4939 0,2544 0,2453 0,2441

CKO скорости, м/с 0,0014 0,0014 0,0012 0,0011 0,0011 0,0011

CKO смещения нуля акселерометра, м/с2 0,0058 0,0079 0,0048 0,0032 0,0038 0,0036

CKO смещения нуля гироскопа, град/с 0,0139 0,0185 0,0128 0,0127 0,0105 0,0103

При использовании окна длительностью более 75 отсчетов уменьшение CKO оценок местоположения несущественно, однако вычислительная нагрузка значительно увеличивается. Вследствие этого целесообразно использовать окно длительностью 75 отсчетов.

Результаты моделирования алгоритма ATEKF с выбранной шириной окна представлены в табл. 4. Для получения более точных оценок использован ансамбль из 300 реализаций.

Cрaвнение данных табл. 4 с соответствующими величинами табл. 2 показывает уменьшение CKO оценок координат и скорости в адаптивном алгоритме до 6-7 раз.

На рис. 1 приведена одна реализация ошибки оценивания координаты по оси ОХ, определенная с использованием алгоритма ТЕКГ (а), и одна реализация, определенная с использованием алгоритма ЛТЕКЕ (б). Из рис. 1 следует, что адаптивный алгоритм способен обеспечить меньшую погрешность определения НВ.

Таблица 4

СКО-координат, м 0,25

СКО-скорости, м/с 0,0011

СКО смещения нуля акселерометра, м/с2 0,0035

СКО смещения нуля гироскопа, град/с 0,013

600

Рис.

время, с а

1. Ошибка оценивания координаты по оси ОХ с помощью алгоритма ТЕК¥ (а) и ЛТЕКГ (б)

600

На рис. 2 приведены: а - одна реализация оценки смещения нуля акселерометра; б - одна реализация смещения нуля гироскопа для оси ОХ. Истинное значение смещения нуля акселерометра составляет 0,1 м/с2, гироскопа - 0,005 град/с.

20 время, с 40 60

а

Рис. 2. Оценка смещений нуля акселерометра (а) и гироскопа (б)

Заключение. Для совместного оценивания НВ и смещений нуля МЭМС в СКНС предложено использование алгоритмов TEKF, ATEKF. Выполнено прямое вероятностное моделирование процесса фильтрации с использованием предложенных алгоритмов и определены вероятностные характеристики точности оценок в виде СКО оценок координат, скорости, смещений нуля гироскопа и акселерометра. Показано, что СКО оценки НВ для алгоритма ATEKF значительно меньше (до 6-7 раз), чем для TEKF. СКО погрешности также меньше чем в случае использования традиционного подхода, предполагающего расширение вектора состояния неизвестными смещениями, однако алгоритм ATEKF обладает наибольшей вычислительной сложностью среди всех алгоритмов. В сравнении с традиционно применяемыми в таких случаях решениями, основанными на проведении лабораторной калибровки, предложенные алгоритмы не требуют использования специального дорогостоящего массивного оборудования, а также позволяют определять неизвестные параметры МЭМС для каждой конкретной реализации процесса решения навигационной задачи.

Литература

1. Hayal A. Static calibration of the tactical grade inertial measurement: Thesis Master of Science. -Columbus, USA: The Ohio State University, 2010. - 118 p.

2. Kim Kwang Hoon. Adaptive Two-Stage EKF for INS-GPS Loosely Coupled System with Unknown Fault Bias / Kwang Hoon Kim, Jang Gyu Lee Chan Gook Park // Journal of Global Positioning Systems. - 2006. - Vol. 5, №1. - P. 62-69.

3. Сейдж Э.П. Теория оценивания и ее применение в связи и управлении / Э.П. Сейдж, Дж. Мелс. - М.: Связь, 1976. - 496 с.

4. Kong X. INS algorithm using quaternion model for low cost IMU // Robotics and Autonomous Systems - 2004. - Vol. 46, №1. - P. 221-246.

5. Шаврин В.В. Калибровка микроэлектромеханических датчиков ускорений и угловых скоростей в бесплатформенных инерциальных навигационных системах // В.В. Шаврин, А.С. Конаков, А.А. Савин, В.И. Тисленко // Доклады Том. гос. ун-та систем управления и радиоэлектроники. -2012. - № 1 (25), ч. 2. - С. 265-269.

6. Julier S.J. A new method for nonlinear transformation of means and covariance’s in filters and estimators / S.J. Julier, J.K. Ulhmann // IEEE Transactions on Automatic Control. - 2000. - Vol. 45. - P. 472-478.

7. Caglayan A.K. A separated bias identification and state estimation algorithm for nonlinear system / A.K. Caglayan, R.E. Lancraft // Automatica. - 1983. - Vol. 19, № 5. - P. 561-579.

8. Конаков А.С. Исследование статистических свойств оценок координат в бесплатформенной инерциальной навигационной системе с использованием кватернионного метода преобразования базисов / А.С. Конаков, В.В. Шаврин, А.А. Савин, В.И. Тисленко // Доклады Том. гос. ун-та систем управления и радиоэлектроники. - 2011. - № 2 (24), ч. 1. - С. 49-53.

Конаков Алексей Сергеевич

Аспирант каф. радиотехнических систем (РТС) ТУСУРа

Тел.: 8-(382-2) 41-36-70

Эл. почта: [email protected]

Шаврин Вячеслав Владимирович

Аспирант каф. РТС

Тел.: 8-(382-2) 41-36-70

Эл. почта: svv281088@ gmail.com

Ноздреватых Дарья Олеговна

Аспирант каф. РТС

Тел.: 8-(382-2) 41-36-70

Эл. почта: [email protected]

Савин Александр Александрович

Канд. техн. наук, доцент каф. РТС Тел.: 8-(382-2) 41-36-70 Эл. почта: [email protected]

Тисленко Владимир Ильич

Д-р техн. наук, профессор каф. РТС

Тел.: 8-(382-2) 413-670

Эл. почта: [email protected]

Konakov A.S., Shavrin V.V., Nozdrevatyh Б.О., Savin A.A., Tislenko VI.

Algorithm for joint estimation of the navigation vector and unknown bias in the loosely coupled integrated navigation system using adaptive two-stage extended Kalman filter

The algorithms joint estimation navigation consumer and unknown bias accelerometer and gyroscope in a loosely coupled navigation system, which is a union of two navigation systems: strapdown inertial and satellite based adaptive two-stage extended Kalman filter. There are given the results of modeling, statistical properties of the algorithm.

Keywords: two-stage extended Kalman filter, adaptive filtering, integrated inertial and satellite navigation systems, micro electro mechanical systems, bias of MEMS.