Модифицированный нелинейный фильтр калмана

 

(51)S Н 03 Н 21/00

I с

ОПИСАНИЕ ИЗОБРЕТЕНИЯ

Й АВТОРСИОМЪ СВИДЕТЕЛЬСТВЪ»

ГОСУДАРСТВЕИХЬ1Й КОМИТЕТ

ПО HBOSPE7ЕИ1 1ЫЖ И ОТНРЫТИЯМ

ГРИ ГКНТ СССР (21) 4647126/09 (22) 03.02.89 (46) 07.01.91. Бюл. Р 1 (72) В. В. Васильев, Н. Г. Щебанов, Э. В. Борисов и А. С. Бурый (53) 681. 32 (088. 8) (56) Авторское свидетельство СССР

Р 1434539, кл. Н 04 Н 21/00, 1986. (54) МОДИФИЦИРОВАННЫЙ НЕЛИНЕЙНЫЙ

ФИДЬТР КАЛМАНА (57) Изобретение относится к радиотехнике. Цель изобретения - повышение точности фильтрации. Модифицированный

„„SU„„1639377 А1

2 нелинеиныи фильтр Калмана содержи сумматоры 1,, 11, 14 и 20, умножители

2, 3, 7, 8 и 21, блоки 4, 5, 9 и 10 сравнения, вычитатели 6, 15 и 22, делитель 12 постоянных коэффициентов, блок 13 памяти, блок 13 памяти, матричные умнояители 16 и 18, блок элементов 17 задержки, регистр 19 памяти, г-р 23 тактовых импульсов и блок элементов ИЛИ 24. Цель достигается путем обеспечения уменьшения систематических погрешностей измерений физических параметров. 3 ил.

1619377

Изббретение относится к радиотехн):.:.-:..e и ;.:))(ет быть vñëot;tçoâ÷(3c) в радиотехнических устройствах для фильт= рации результатов измерений физичес5 кого параметра.

Цель изобретения является повышение точности фильтрации путем уменьшения систематических погрешностей измерений физических параметров. 1О На фиг. 1 представлена функциональная схема модифицированного нелинейного фильтра Калмана; на фиг, 2 — временные диаграммы; на фиг. 3 — схема блока сравнения, 15

Иодифицированньй нелинейный фильтр

Калмана (фиг. 1) содержит второй сумматор 1, второй 2 и четвертьй 3 умно)кит(1ли» первьй 4 и третий 5 блоки сравнения, второй вычитатель 6, третий 7 и пятый 8 умирители, второй 9 и четвертьй 10 блоки сравнения, чет -ертьй сумматор 11 и делитель 12 по.,-:тоянных коэффициентов, блок 13 памя:и, третий суюитор 14, третий вычи:гатель 15, первьй матричньй умножитель 16, блок 17 элементов задержки, второй матричный умножитель 18, регистр 19 памяти, первый сумматор 20, первый умножитель 21, первый вычитат пь 22, генерато»о 23 тактовых импульсов (. 7И)., блэк 24 элементов ИЛИ.

Блоки (4, 5„ 9, 10) сравнения (фиг, 3) содержат элемент 25 сравненля (цифровой компаратор) и коммутатор 26. Фил(ьтр имеет вход Сброс мо35 дифицированного нелинейного фильтра 27 команды (фиге 2а)о

Модифицированньй нелинеиньй фильтр

Калмана работает следующим образом.

В исходном состоянии все регистры фильтра обнулены, а в блок 13 памяти, включающий трн регистра записана(значения априорных систематических погрешностей Д и начальное значение

45 оцениваемого параметра фильтра g (два регистра постоянной памяти), а в третий (регистр сдвига) записачы коэффициенты усиления априорно обусловленного фильтра Калмана К (1

m).

Входной сигнал z (t; ) представляет собой значение измеренного физическо. э параметра в цифровом параллельном двоичном коде, первьй разряд отводится для признака информации (импульс

tÝ )!

55 сопровождения информации) . Значение физ)лчес)co3 0 p (p z(t ) поступает

;а сумматор i и вычитатель 6, а импульс rопровождения информации (фиг. 2б) запускает ГТИ (23), с второго выхода которого поступает имп".-.лье разрешения считывания g (фиг. 2в), о на первый вход блока t3 памяти, с это=рого выхода которого начальное значение я через блок элементов ИЛИ 24 записйвается в регистр 19 памяти посредством подачи с первого выхода

ГТИ 23 тактовых импульсов (фиг; 2г), на вход разрешения записи регистра 19 памяти. Значение я поступает на блок

17 линии задержки и далее на матричньй умножитель 18, на выходе которого формируется значение д(; /t.,). К моменту прихода последующего входного сигнала z(t ° ) оно поступает на сумма1 тор 20, а через матричный умножитель

16 формирует значение у (t;/t;,), коФ. торое подается на первый вход вычитателя 22 и на вторые входы сумматора

14 и вычитателя 15.

В то же время четвертьй тактовый импульс (фиг. 2д) поступает с третьего выхода ГТИ 23 на второй и третий входы считывания коэффициентов б и К, .

Значения Д записываются в блок 13 памяти до начала работы и определяются из выражения где ш — максимально возможная систе- . матическая погрешность входного сигнала z(t) она постоянна и не зависит от момента вращения

G среднее квадратичное отклонение случайного входного сигнала z(t), распределение ве роятностей которого считается нормальным.

Значения коэффициентов усиления К записанные в блок 13 памяти до начала работы, определяются расчетным путем из выражения

К, = q(t;)V(t, (t;), где q(t,) = (((>(г, lг(,)0(t; )() () /t;,Я+н (t; Уы{е;)H(t;)) ;

H (t, ), Ф (t; /t;, ), N(t;) " матрица измерений, переходная матрица и ковариационная матрица шумов измерений соответственно.

Если число рассчитанных коэффициентов К окажется меньше общего числа

77 6 ляющая собой оценку нелинейного преобразователя входного сигнала z(t, ): — 1,2,у=1,2.

55 .,+,, .. ь у (t;„,/t. ) = H(t; ) g (t; /а;), который подается иа сумматоры 14 и

;;5 и фильтр оказывается подготовленным к следующему циклу работы.

16193 поступивших значений измеряемого па— раметра z(t ), т.е. если m «п, то для всех m «i c n К," = К . Таким образом для каждого значения вхадноЭ

rn сигнала z(t, ), начиная с второго и заканчивая z(tz), будут подаваться на блоки 1, 6, 14, 15 и К,, на умножитель 71.

Фильтрация сиг. ала z(t ) заключает- 0

< < ся в следующем. На первые входы сумматора 1 и нычитателя 6 поступает входной сигнап z(t;) в двоичном параллельном коде, а на вторые входы поступают коды допустимых значений погреш- 15 ностей входного сигнала Ь.

На выходе сумматора 1 и вычитателя

6 формируются коды со значениями z

= z(t;) + 5 и z < = z(t<) - соответственно. Эти кодь. поступают на умно- 20 жители 2, 3 и 7, 8.

Аналогично с помощью блока 13 памяти, сумматора 14, вычитателя 15 и выходного сигнала у (t, /t;,) матричного умножителя 16 формируются коды 25 со значениями у = у (t /t;,) + и у = y1(t; !t „) —, которые также поступают на умножители 2, 3, 7 и 8

С выходов умножителей 2, 3, 7 и 8 сигналы в виде параллельных двоичных 30 кодов поступают на двухкаскадную схему сравнения, состоящую из четырех блоков 4, 9, 5 и 10 сравнения и позволяющую выделить максимальное и мичимальное значения парных произведей zyr, г

Эта часть схемы работает следующим образам.Ла блоки 4 и 9 сравнения представляюгг„ re собой первый каскад схемы сравнения, поступают кеды, численные зна- О :.ения которых pBBHbr е y z j2 и

z, у,, г, у, соответственно. На выходе формируются максимальные (первые выходы) и минимальные (нторые инверсные выходы) значения парных произведенчй величин zиу,,которые в виде двоичФ ных параллельных кодов поступают на второй каскад схемы сравнения, при этом максимальные значения подаются на вход блока 5 сравнения, а минималь50 ные значения — на входы блока 10 . сравнения. В результате на выхсде блока 5 формируется максимальное -.íàчение, а на выходе блока 10 - минимальное значение парных произведений величин г и у, которые поступа-r на

+ последовательно соединенные сумматор

11 и делитель 12, на выходе которого формируется оценка z (t ), представшах(г1 у1<) + min(zp .+} ю

На первый вход вычитателя 22 поступает с матричного умножителя 16 код параметра у (t;/t;,), а на второй — код параметра z+(t;). На выходе вычитателя 22 формируется сигнал невязки

Сигнал невязки 6 z(t;) пост< пает с выхода нычитателя 22 на первый вход умножителя 21, на второй вход которого поступает храняп;ийся в блоке 15 памяти коэффициент усиления K(t, ) .

С выхода умноллтеля 21 сигнал поступает на первый вход сумматора 20, на второй вход которого с выхода матричного умножителя 18 поступает корректирующий сигнал д (t,"/t; r).

С выхода сумматора 20 сигнал g (t; ) gr(t ) =- К+(е /t ) + K(t ) hz(t ) поступает на вход регистра 19, запись информации происходит при появлении на его входе тактового импульса (фиг. 2г), и таким образом формируется выходной сигнал модифицированного нелинейного фильтра Калмана g+(t;) в виде параллельного двоичного кода.

Этот же сигнал используетсч в цепи обратной связи фильтра. Он подается на вход блока 17 элементов задержки, осуществляющего задержку на один такт.

Сигнал с выхода блока 17 элементов задержки используется для получения н матричном умножителе 18 корректирующего сигнала g+(t r/t ), равного

Этот сигнал поступает на вход матричного умножителя 16, в котором производится вычисление сигнала

16: .",377 формула изобретения

Модифицированный нелинейный фильтр

Калмана, содержащий последовательно соединенные первые вычитатель умноЭ житель и сумматор, блок элементов ИЛИ, последовательно соединенные регистр памяти, выход которого является выходом модифицированного нелинейного фильтра Калмана, блок задержки, вто- О рой матричный умножитель, выход которого подключен к второму входу первого сумматора, и первый матричный умножитель, последовательно соединенные генератор тактовых импульсов, второй

1 выход которого подключен к первому вхоцу регистра памяти, и блок памяти постоянных коэффициентов, первый выход которого подключен к второму входу первого умножителя, о т л ич а ю шийся тем, что, с целью повьппения точности фильтрации путем уменьшения систематических погрешностей измерений физических параметров, введены последовательно соединенные 25 второй сумматор, второй умножитель, первый блок сравнения и третий блок сравнения, последовательно соединенные второй вычитатель„ к первому входу которого и первому входу второго сумма- п тора, являющихся входом модифицированного нелинейного фильтра Калмана, под: ключен вход запуска генератора тактовых импульсов, третий умножитель, вто- рой блок сравнения, прямой выход коЗэ торого подключен к второму входу третьего блока сравнения, и четвертый блок сравнения, к второму входу которого подключеч и:веро ый выход первого блока сравнения, пэследовате-1ьнс соединенные четвертый сумматор, к вхс» дам которого подключены соответственно прямой выход третьего блока сравнения и инверсный выход четвертого блока сравнения, и делитель частоты, выход которого подключен к первому входу первого вычитателя, к второму входу которого подключен выход первого матричного умножителя, а также третий сумматор, выход которого подключен к вторым входам второго и третьего умножителей, последовательно соединенные третий вычитатель,к первому входу которого и первому входу третьего сумматора подключен выход первого матриччоrо умножителя, и пятый умиритель, вь|ход которого подключен к второму входу второго блока сравнения, .четвертый умножитель, выход которого подключен к второму входу первого блока сравнения, при этом выход первого суммато— ра подключен к второму входу регистра памяти через элемент ИЛИ, к второму входу которого подключен второй выход блока памяти постоянных коэффициентов, третий выход которого подклю ен к вторым входам второго вычитателя, выход которого подключен к второму входу пятого умножителя, второго сумматора, выход которого подключен к первому входу четвертого умножителя, третьего сумматора и третьего вычитателя, выход которого подключен к второму входу четвертого умножителя.

1619377

Составитель Л. Тимошина

Редактор Н. Лазаренко Техред Л.Олийнык Корректор В. ЫK

Заказ 53 Тираж

ВНИИПИ Государственного комитета по изобретениям и о-.крытиям прн ГКНТ СССР

113035, Москва, Ж-35„ Раушская наб., д. 4/5

Производственно-издательский комбинат "Патент", r. Ужгород, ул. Гагарина, 161