Устройство для калмановской фильтрации

(19) 03 Н 21/00 ПИСАНИЕ ИЗОБРЕТЕНИВТОРСКОМУ СВИДЕТЕЛЬСТВУ ий инсти- тябрьской Ку ратЙошТИ,и ап мана о ВН ГОСУДАРСТВЕННЫЙ КОМИТЕТПО ИЗОБРЕТЕНИЯМ И ОТКРЬТИЯПРИ ГКНТ СССР(56) Лебедев Е.К., МясникСтарыгин С.В. Программнаяная реализация фильтра Какар-Ола, 1985, Рук. деп,Ю 2737-85 Дп.(54) УСТРОЙСТВО ДЛЯ КАЛМАНОВСКОЙФИЛЬТРАЦИИ(57) Изобретение относится к вычислительной технике и может быть использовано в устройствах, работающих вреальном масштабе времени при создании информационно-поисковых систем,систем обработки данных и изображений. Цель изобретения - повышение быстродействия. Устройство калмановской фильтрации содержит блоки 1, 4и 9 памяти произведений, первый блок2 сумматоров, первый блок 3 регист3 156471 ров, первый регистр 5, второй блок 6 сумматоров, второй блок 7 регистров, блок 8 вычитателей, третий блок 10 сумматоров, третий блок 11 регистров, блок 12 сдвиговых регистров,второй регистр 13, блок 14 синхронизации. Цель изобретения достигается за счет введения регистров 5, 13и блока 12 сдвиговых регистров.1 з.п. Ф-лы, 2 ил.Изобретение относится к вычислительной технике и может быть использовано в устройствах, работающих в реальном масштабе времени,при создании информационно-поисковых систем, систем15 обработки данных и изображений.Цель изобретения - повышение быстродействия устройства.На Фиг. 1 представлена структурная20 схема устройства для калмановскои Фильтрации; на фиг. 2 - временные диаграммы работы блока синхронизации.Устройство для калмановской фильт" рации содержит первый блок 1 памяти произведений, первыйблок 2 сумматоров, первый блок 3 регистров, второй блок 4 памяти произведений, первь 1 й регистр 5, второй блок 6 сумматоров, второй блок 7 регистров, блок 8 вычитателей, третий блок 9 памятиЗО произведений, третий блок 10 сумматоров, третий блок 11 регистров,1 блок 12 сдвиговых регистров, второй ре"- ,гистр 13, блок 11 синхронизации.Блок 1 ч синхронизации содержит ге- З 5 нератор 15 тактовых импульсов, счет. чик 16 тактовых импульсов, генератор 17, инвертор 18.Реализация алгоритма фильтрации40 осуществляется в соответствии с выражением:Х(К + 1) = ф(К) Х(К)+К(К) 1 У(к)-Н(к)45 "Ф(к)х(к)1 = х+(к) + к(к) Ь(к) - Н(К) Х+(К)1 = Х(к) + К(К) П(К)Х+(К) + С(К), . (1) где Х(к). - Н - мерный вектор оценки50параметров процесса наК-том шаге, определяемый из описания процесса;ф(К) - МхН- мерная переходная матрица, значения которой- 55также определены;Н(К)-ИхИ 3 в , мерная матрица наблюдения;,К(К)-ИхМ 3 - мерная матрица коэффициентов усиления, рассчитанная для каждогошага вычислений, исходя из априорных статистических данных опроцессе;У(к) - М - мерный вектор измерения, определяемый изописания процесса.В общем случае матрицы ф (К),Н(К), К(К) изменяютсвои значениянакаждом шаге работы устройства. Накаждом шаге обеспечивается вычисление выражения (1).При умножении матриц на векториспользуется метод Пеледу-Лиу, заключающийся в последовательном Формировании сумм частичных произведений элементов матрицы на одноименныеразряды элементов вектора. Поэтомушаг работы устройства содержит числотактов гь, равное разрядности операндов вектора оценок Х(К), Для обеспечения правильной работы устройстванеобходимо равенство разрядности элементов векторов У(к) и Х(К), На адресные входы блока 1 постоянной памяти параллельно поступают одноименные разряды всех И элементов вектораоценок Х(К), начиная с младших. Навыходе устройства параллельно получаем одноименные разряды всех элементов вектора Х(К + 1), начиная с младших.В блоках 1, Й,. 9 памяти произведений записаны не непосредственные зна-,чения соответственно элементов матрицф(К), Н(К) и К(К), а все возможныесуммы частичных произведений строкэтих матриц на столбец, образованныйодноименными разрядами элементов векторов соответственно Х(К), Х (К)и Р(к),Для представления -й суммы частичных произведений требуется г== 1 оц И+и 1 выходных разрядов блока 1постоянной памяти, где п 1- разряд 5 15647ность элементов матрицы ф(К), а таккак таких сумм И, то разрядность блока 1 составляет Б = Иг . Количествовыходных разрядов блоков 4 и 9, ис 5ходя из тех же предположении, составляет ь 2 = (1 оИ + пг)М = Мгг иЯ = 1 оВ,И + и Ч = ИгТак как разрядность операндов матриц ф(К), Н(К) и К(К) не влияет наописание принципа работы устройства,считаем, что г= г = г= гПустьприем во все регистры выполняется попереднему фронту синхроимпульсов, поступающего на их синхровходы, установка в нуль выполняется отрицательным значением на входе установки внуль регистров блоков 3, 7, 11, асдвиг - отрицательным значением навходе управления сдвигом, так как 20регистры блока 12 работают только врежиме сдвига, считаем, что сдвигвыполняется по переднему фронту синхроимпульса25Обозначим через Х (1), Х (2), Х (г ) вектора размерности М, элес оменты которых образованы одноименными разрядами элементов .вектора оценки Х(К), Х к(1) у Х (2) з, Хк(го) э 30 последовательно поступают на адресный вход блока(1), Р 1(2)Р (г ) - значения сумм частичных произведений 1-й строки матрицы ф(К) на вектора соответственно Х,(1), Х (2) Х (г,); жуточные результаты умножения матрицы ф(К) на вектор .Х(К);Х"(1), Х (2) Х(г,) - вектора размерности И, образованные одноимен- . ными разрядами элементов вектора экстраполяции Х+(К), которые поступают в качестве адреса на входы блока 4 ирегистра 5;Н;(1), Н;(2) Н.;(г,) - значе-, ния суммы частичных произведений -й строки матрицы Н(К) на вектор Х (1), Х(2) Хк(г ), образованный одноименными разрядами вектора экстраполяции Х(К);Н .(1), Н(2) Н.(г,) - промежуточные результаты умйажения матрицы Н(К) на вектор Х (К);Ь (1), Ь,(2), Ь(г,) - вектор55 разрядности И, оЪразованный одноименными разрядами элементов вектораВ(К) = Н(К) Х(К); 11 6У (1), у (2),у (г, ) - вектораразмерности М, образованные одноименными разрядами вектора измерений Ъ(К);С(1), С(2) С(г,) - промежуточные результаты умйожения матрицы К(К) на вектор В(К);В (1), В .(2) В;(г ) - промежуточные результаты вычисления вектора Х(К + 1),Рассмотрим работу устройства наК-м шаге.В исходном состоянии счетчик 16тактовых импульсов обнулен. Регистрыблока 3 сигналом с первого выходаблока 14 синхронизации устанавливаются в нулевое состояние, по входу задания шага работы устройства поступает код номера страницы К, и образует старшие разряды адреса для блоков1, 4, 9 памяти,На младшие разряды адресного входа блока 1 поступает вектор Х(1),образованный младшими разрядами(1-м) всех И элементов вектора Х(К),Из блока 1 считывается значениеГ(1) Ц = 1,М) представляющеесобой сумму частичных прозведений1-й строки матрицы ф(К) на векторХ (1), Р(1) суммируется в 1-м сумматоре блока 2 с нулевым содержанием,1 -го регистра блока 3 и поступает навход этого регистра. Считаем исходное состояние первым тактом первымполутактом работы устройства.В первом такте во втором полутакте состояние счетчика 16 тактов001, передним фронтом синхроимпульса в 1-й регистр блока 3 принимается Р(1).Во втором такте в первом полутакте состояние счетчика 16 тактовыхимпульсов 0010. В-м регистреблока 3 по нулевому значению на входе управления сдвигом осуществляетсясдвиг содержимого Г;(1) на один разряд вправо. В (го + .1)-м разряде 1 го регистра блока 3 (1 = 1, И) получаем значение младшего разряда всехэлементов вектора Хф(К) - векторХ(1). В блоке 7 регистр д-й (1, И) сигналом с второго выходагенератора 17 блока 14 синхронизацииустанавливается в нулевое состояние.Из блока 4 считывается значение Н .(1)по адресу Х(1), которое являетсясуммой частичных произведений"-йстроки матрицы Н(К) на вектор Х (1) и, суммируясь в сумматоре 6. 1 с ну 1561711левым содержанием регистра 7 поСтупает на вход регистра 7 На адресные входы блока 1 поступает вектор Х (2), образованный вторыми разядами вектора оценок Х(К). Из блока5считывается значение Р .(2) и суммируется в сумматоре 2,1 с содержи-3мым г,-разрядов регистра 3.,1, Р(1)2- . Ыа входе регистра 3.,1 получаемРф(2) = Р (2) + Р (1) 21Во втором такте, во втором полу,гакте состояние счетчика 16 тактовых 15 импульсов 0011.В регистры. 3,1, 7. 1, 5 соответственно принимаются Р (2), Н,(1), Х(1),В третьем такте в первом палутакге состояния счетчика 16 00100. 20 В регистрах 7,.д, 31 по нулевому знаению на входе управления сдвигомсуществляется сдвиг содержимого соотетственно Н.,(1) и Р.(2) на один разяд вправо. 25В (г + 1) -м.разрядах регистров 7, 3.1/ 3.,1 получаем соответственно значения младшего разряда произведенияатрицы Н(К) на вектор Хф(К) - вектор(1) и вектор Хф(г), вектор Х (1) ЗО из регистра 5 переписывается в реистр 13 по сигналу с выхода инверора 18 блока 1 ч. Отрицательным знаением с третьего выхода дешифратоа блока 14 синхронизации регистры , 2.,1 (1 = 1, М) устанавливаются в нуевое значение, вектор Ь(1) постуПает на вход вычитателя 8, туда же Поступают младшие (первые) разрядывсех элементов вектора измерений 40 У(К), образуя вектор у (1). В вычитателе 8 получаем й поразрядныхразностей с 1 (1) = у (1) - 1 (1)Векторы й(1) образуют значения младшего разряда элементов вектора 45 О = у(К) - Н(К)Х+(К) и поступают наадресные входы блока 9 Значение С)(1), (3 = 1, В) проходит через сумматор 1 О 1 и суммируется с нулевымсодержанием регистра 11, 1 и содержимым регистра 13 - Х (1), разряды которого поступают в цепь переноса вмладший разряд сумматоров 10.1Получаем В (1) =.Х(1) + С (1), котороеПоступает на вход регистра 11.1. Работа остальных узлов устройства ана-.логична первому и третьему тактам. На входе регистра 31 получаем вектор Р (3) регистра 7, - Н,(2)= Н,(1) 2 + Н,(2) регистра 5 - Х(2),В третьем такте во втором полутактесостояние счетчика 16 00101, ВРегистре 3.1, 7. 1, 5 и 11. 1 принимаемсоответственно Р (3), Н , (2), Х (2),В;(1) .3В четвертом такте в первом полутакте состояние счетчика 16 00110.В регистре 11.1 выполняется сдвиг содержимого регистра 11,1 на один разразряд вправо - В (1) 2 - значения в(г,+ 1)-м разрядах регистров 11.1 образуют вектор Х(1) и равны младшимразрядам вектора оценки Х(К + 1), вектор Х к,(1) поступает на вход устройства и на входы регистров 12,1.Остальные узлы устройства работаютаналогично первому, третьему и пятомутактам.В четвертом такте во втором полутакте состояние счетчика 16 00111,в сдвиговые регистры 12 принимаетсявектор Х.,;(1) и считывается векторХ (5), который поступает на адресныйвход блока 1. В регистры 3,1, 75 и 11,1 принимаются соответственноР,(й), Н(З), Х(З) и В (2),Далее работа устройства аналогична приведенным выше тактам,Таким образом, за счет введениярегистров 5 и 15 время выполнения одного шага алгоритма Калмана удалосьсократить до Т = ег , а использование входа переноса в младший разрядблока 10 сумматоров позволило исключить последний сумматор в прототипе,что дает возможность повысить быстродействие устройства.Формула изобретения1. Устройство для калмановской Фильтрации, содержащее блок синхронизации, три блока памяти произведений, подключенные адресными входами задания номера страницы к входу задания шага работы устройства, с разрядно" стью равной логарифму по основанию два от максимального числа шагов работы устройства, первый блок регистров, соединенный информационным входом с выходом первого блока суммато; ров, первым выходом - с адресным входом второго блока памяти произведений, а вторым выходом " с входом первого блока сумматоров, подключенного группой входов к группе выходов первого блока памяти произведен й с1564711 Ьяяфйе наумом АеюяиююмююИУЬж/ЯАм/аЬюРР ФюиМ АвюФМ Фиа г Составитель В. Башкиров Редактор И. Бандура Техред Л.Сердюкова Корректор Э, ЛончаковаЗаказ 1165 Тираж 649 ПодписноеВНИИПИ Государственного комитета по изобретениям и открытиям при ГКНТ СССР113035, Москва, Ж, Раушская наб., д. 4/5 Производственно-издательский комбинат "Патент", г. Ужгород, ул. Гагарина, 191 разрядностью каждого из выходов, равной сумме разрядностей операнда переходной матрицы системы и логарифма по основанию два от размерностивектора состояния, блок вычитателей,соединенный первым входом с информационным входом устройства, вторымвходом - с первым выходом второгоблока регистров, информационный вход 1 Окоторого соединен с выходом второгоблока сумматоров, а второй выход -с входом второго блока сумматоров,подключенного группой входов к группе выходов второго блока памяти произведений, с разрядностью каждогоиз выходов, равной сумме разрядностей операнда переходной матрицы системы и логарифма по основанию два отразмерности вектора состояния, третий 20блок регистров, соединенный информационным входом с выходом, а выходом "с входом третьего блока сумматоров,подключенного группой входов к группе выходов третьего блока памяти произведений, с разрядностью каждоговхода, равной сумме разряднос-ей операнда переходной матрицы системы илогарифма по основанию два от размерности вектора состояния, выходблока вычитателей соединен с адресным входом третьего блока памяти произведений, о т л и ч а ю щ е е с ятем, что, с целью повышения быстродействия устройства, введены два регистра и блок сдвиговых регистров,первый выход первого блока регистров через последовательно соединенные первый и второй регистры подключен к входу переноса третьего блока сумматоров, второй выход третьего блока регистров, .являющийся выходом устройства, через блок сдвиговых регистров соединен с адресным входом первого блока памяти произведений, первый, второй и третий выходы блока синхронизации подключены к входам сброса соответственно первого, второго и третьего блоков регистров, соединенных входами синхронизации и входами управления сдвигом с входом синхронизации. первого регистра и четвертым выходом синхронизации, пятый выход которого подключен к входам синхронизации второго регистра и блока сдвиговых регистров.2. Устройство по и, 1, о т л и ч а ю щ е е с я тем, что блок синхронизации содержит генератор тактовых импульсов, счетчик тактовых импульсов, дешифратор и инвертор, выход генератора тактовых импульсов соединен с тактовым входом счетчика тактовых импульсов, подключенного выходами поразрядно через дешифратор к выходам блока синхронизации с первого по третий, выход генератора тактовых импульсов соединен с четвертым выходом блока синхронизации и через инвертор - с пятым выходом блока синхронизации.