Требуется построить фильтр калмана для 2-х величин. это курс от ИНС и от GPS.
накачал некоторое количество информации, и понял, что подход Бесерского почему-то другой, нежели у буржуев ( в Навгеокоме статья, если надо, выложу.) у буржуев отсутствует формирующий фильтр, что по Бесекерскому есть модель нашего процесса. Пересчет весов вклада компонент вектора измерений как я понял, на каждом шаге вычисляется их дисперсий предыдущих оценок выходного вектора оценки состояния. мне пока не ясно КАК ИМЕННО МАТЕМАТИЧЕСКИ вычислять дисперсии и веса. если кто занимался этим, помогите, пожалуйста.

Для каких целей собираетесь комплексировать ИНС и GPS?

линки http://www.navgeocom.ru/gps/kalman/
это почти понятно

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

надо качать документ, формулы плохо передаются в форум...

//---------------------------------------------------------------

Простой пример

Основные мысли предыдущего раздела поможет прояснить следующий простой пример. Рассмотрим задачу вывода реального сопротивления резистора номиналом 100 ом из повторных измерений омметром и их обработки фильтром Калмэна. В первую очередь необходимо установить модели, связывающие состояния нашей системы с измерениями и между собой, а также их статистические характеристики, служащие для вычисления весов. В данном случае имеет место всего одна величина, характеризующая неизменное состояние системы, — неизвестное сопротивление x. Таким образом, модель, описывающая состояние системы, выглядит как

xk+1 = xk. [1]

Заметим, что оно не нарушается каким-либо случайным возмущающим процессом. Цветовой код на резисторе указывающий класс его точности, позволяет установить, что данная партия характеризуется нормальным (гауссовым) распределением значений сопротивления вокруг номинала в 100 ом с дисперсией (2 ом)2. Таким образом, при отсутствии измерений наилучшей оценкой сопротивления является x0/­ = 100, а величина его погрешности P0/­ = 4.

Повторные измерения омметром

zk = xk + vk ,

непосредственно дают значения сопротивления, отягощенные инструментальными ошибками, которые принимаются некоррелированными от измерения к измерению. Завод-изготовитель омметра сообщает, среднее значение погрешности измерения составляет 0, с дисперсией Rk = (1 ом)2. При k = 0 вес поправки в начальные условия за первое поступающее измерение

P0/- 4
K0= ---------------- = ---------------,
P0/- + R0 4+1

а исправленная оценка состояния

x0/0 = (1 - K0)x0/- + K0z0,

где через x0/0 обозначена наилучшая оценка, относящаяся к моменту времени t0, полученная из обработки измерений, поступивших на момент времени t0 включительно. Поскольку измерения обладают меньшей дисперсией, чем исходная оценка состояния, первое измерение получает относительно высокий вес. Дисперсия, характеризующая неопределенность исправленной оценки состояния

1 4
P0/0 = (1 - K0)P0/- = ---------4= ---
4 + 1 5

Заметим, что всего одно хорошее измерение уменьшает дисперсию оценки состояния с 4 до 4/5.

Рассматриваемая нами система находится в неизменном состоянии, поэтому согласно уравнению [1]

4
x1/0 = x0/0 ; P1/0 = P0/0 = ---
5

На новом цикле вес поправок в начальные условия

P1/0 4/5
K1 = -------------= ------ , [2]
P1/0+R1 4/5+1

а дисперсия исправленной оценки

1 4 4
P1/1 = (1 - K1) P1/0 = (---------) --- = --- [3]
4/5 + 1 5 9

Рисунок 3 иллюстрирует процесс приближения оценки к истинному значению, при этом дисперсия построенной фильтром оценки стремится к нулю (рисунок 4).


//-------------------------------------------------------



http://www.basegroup.ru/filtration/kalmanfilter.htm
это более запутанно.


цель построения калмановского фильтра- улучшить динамические характериски автопилота ДПЛА и попытаться прогнозировать отставание курса GPS при больших угловых скоростях - более 35 °/c

Не читайте попсовых статей - только мозги себе запудрите. Потом будет трудно их прочищать, читая объективную литературу. Для начала - погуглите книжку "Global Positioning Systems-Inertial Navigation and Integration". Потом погуглите статьи по ключевым словам INS GPS Integration. Можно также погуглить Kalman filtering.

Структура фильтра следущая:
1. в момент времени i вы имеете оценку вектора состояния x(i;i), полученную по i предыдущим измерениям и оценку ковариационной матрицы P(i;i) этого вектора.

2. В течении временного интервала (i, i+1) вычисляется предсказаное значение x(i+1;i) вектора состояния для будущего момента времени i+1: x(i+1;i) = Ax(i;i)+Bu и ковариационная матрица предсказанного вектора P(i+1;i) = A'P(i;i) A+..ков.матрица шума (см. нормальные статьи).

3. Вычисляете коэффициент усиления калменовкого фильтра K(i+1).

4. В момент времени i+1 вы получаете очередное измерение y(i+1). C помошью этого измерения вычисляется уточнённая оценка x(i+1;i+1) вектора состояния:

x(i+1;i+1) = x(i+1;i) + K(i+1)[y(i+1) - H*x(i+1;i)]

5. Вычисляется оценка ковариационной матрицы P(i+1;i+1) уточненного вектора x(i+1;i+1) .

Оценки п.4. и п.5. являются окончательными. На следующем шаге повторяются п.1.-п.5., в качестве входных величин используются оценки, полученные в п.4. и п.5. предыдущего шага.

цель построения калмановского фильтра- улучшить динамические характериски автопилота ДПЛА и попытаться прогнозировать отставание курса GPS при больших угловых скоростях - более 35 °/c
[/quote]


Я не про это спрашивал: вам бумажную работу написать или практическую вещь сделать надо? Кстати, что за организация?

Запишем формулы для структуры измерений:

Ygps = PSI - L*PSIdot + ngps;
Yins = PSI + EPS + nins; (1)

PSI - курс, PSIdot - скорость изменения курса, L - коэффициент который отвечает за "отставание курса GPS при больших угловых скоростях", EPS - дрейф ИНС; ngps, nins - шумы измерений, к которым (с натягом можно отнести неучтенные ошибки). Дисперсия nins будет Nins~=(0.001 °/c)^2 (потом можно подобрать вручную). Дисперсия ngps будет Ngps=(рассчитывается на лету, зависит от горизонтальной скорости)

Введем вектор состояний:
X = [ PSI PSIdot L EPS EPSdot]; (2)

EPSdot - скорость дрейфа курса от ИНС.

Введем вектор измерений

Y = [Ygps; Yins]; (3)

Введем векторную сигнальную функцию на основании (1), (3):

S(X) = [PSI - L*PSIdot;
PSI + EPS]; (4)

N=[Nins 0;
0 Ngps]; - матрица дисперсий измерений.

Записываем переходную матрицу для вектора состояний:

F = [1 T 0 0 0;
0 1 0 0 0;
0 0 1 0 0;
0 0 0 1 T;
0 0 0 0 1]; (5)
T - шаг дискретизации по времени, сек.

Т.е.
X(k) = F*X(k-1) + g(k)(вектор формирующих шумов);
- аппроксимация вектора состояний Марковским процессом (чисто для справки).
g = [0 ksiPSIdot ksiL 0 ksiEPS];

ковариационная матрица g равна

G=diag([0; sigmaPSIdot^2; sigmaL^2 0 sigmaEPS^2]); (6)

sigmaPSIdot = T*(среднее угловое ускорение);
sigmaL = (что-то очень маленькое для вычислительной стабильности и демпфирования погрешностей, связанных с гауссовским приближением апостериорной плотности вероятности X);
sigmaEPS = T*(4e-6 рад/с/с - для микромеханической БИНС; 1е-9 рад/с/с для ИНС класса Litton LN-200)

Задаем начальные условия:

X(0) = [Ygps(0); 0; 0; 0; 0];
E(0) = diag([Ngps(0); 100; 1; 0.1; 0.001]); - ковариационная матрица оценок, или матрица дисперсий вектора сосотояний (кому как больше нравится).

Запускаем обсчет расширенного фильтра Калмана. На каждом шаге просчитываются 4 строчки:

Экстраполяция:
Eext(k) = F*E(k-1)*F + G; (7)
Xext(k) = F*X(k-1); (8)

Оценивание (учет измерений):
E(k) = inv(Eext(k) + (dS(Xext)/dX)'*inv(N(k))*(dS(Xext)/dX)); (9)
X(k) = Xest(k) + E(k)*(dS(Xext)/dX)'*inv( N(k) )*( Y(k) - S(Xext(k)) ); (10)

X(k) - и есть оценка вектора состояний, а его первый компонент (PSI) - оптимальная оценка курсового угла. Третий компонент (L) - коэффициент запаздывания курсовых измерений из GPS.

Задача решена.

Пояснения:

dS(Xext)/dX - это МАТРИЦА 2х5, производная сигнальной функции по вектору состояний. Считается на каждом шаге для экстраполированных значений X=Xext(k). В данном cлучае:

(dS(X)/dX) = [1 -L -PSIdot 0 0;
1 0 0 1 0]; (11)

Если темп GPS меньше темпа ФК, то для тех моментов времени, когда данные GPS недоступны полагается

inv( N(k) ) = [0 0; 0 (1/Nins)]; (12)


Желаю удачи.

Огромное спасибо за пояснения- буду изучать дальше

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

Но все- таки хочется сделать все, как говорится, "по корану" т.к в целом получилась не адаптивная система, а некое ее подобие

Сейчас просто оттачиваем метод наведения - динамическую компенсацию ветра на разной высоте... (например, круги с набором высоты)

Serj78

У меня проблема почти такая же.
Может поясните мне
Про фильтр Калмана ничего не знаю

Проблема у меня следуящая:
Есть три трехосевых акселерометра. КАздый измеряет ускорение по трем осям. И соответственно с ошибками.
Как используя сигналы от трех разных источников расположенных на разных плоскостях получить скорректированные более точные ускорения?
Можно ли использовать фильтр Калмана для етих целей?
И как он будет выглядеть?

Вук, вопрос сложный задал в связи с тем, что много материала придется выложить. Если кто-то возьмется за это, то хорошо.

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

Формально можно выписать выражения фильтра Калмана для любых целей. Только практической ценности, в Вашей постановке задачи, такой фильтр представлять не будет.

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

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

Получение из этих трех оценок одной, но с увеличенной точностью потребует привлечения данных о дисперсиях шумов отдельных датчиков. Методы совместной обработки показаний разноточных датчиков при аддитивных шумах хорошо известны.

Акселерометры ведь трех осевые. Измеряют ускорения по всем трем осям каждый. и находятся они очень близко друг к другу. В итоге получается, что у нас есть 9 ускорений. три по оси X, три по оси Y и три по оси Z.
Но вот какой вид фильтрации использовать, чтоб с трех датчиков, измеряемых одну и ту же величину выделить полезный сигнал,
Я думал фильтрация Калмана подойдет?
Использую акселерометры Thompson LIS3L02AS4. Трех осевые. Выбрал их потому-что можно заказать экземпляры абсолютно бесплатно.
Сигналы с акселерометров планирую подавать сперва на АЦП, а затем используя фильтрацию Калмана получить скорректированные ускорения.
Затем то же самое проделать с двух осевыми гироскопами Murata MEV-50A-R. Поскольку каждый меряет угловую скорость , то в итоге получаем шесть угловых скоростей, по две с каждого гироскопа.
Сигналы так же будут подаваться на АЦП.
Затем через USB порт все это планируется подавать в компьютер, и там отлаживать в MATLAB код программы бесплатформенной ИНС.
Как только код отлажу, планируется комплексирование ИНС и ГПС по средством фильтрации Калмана.
Написание кода и его отладка планируется сперва в MATLAB, а затем когда код будет вылезан планируется программирование контролера этим кодом.
Но это потом, а сейчас я не знаю, как наиболее точно измерить ускорения?

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

Правильный метод наведения и понимание аэродинамики-- тоже важно.

Здесь хочу сказать огромное спасибо NickNich - он посоветовал выяснить Математику зависимости положения углов от рулей- оказалось- что Даалеко не все так просто как в учебниках- модель системы получилась динамически изменяющейся...

А у Analog Devices можно заказать samples?
А то я пробовал заказать - а они мне отвечяют - Contact Your Local Distributer.
А Distributer сразу говорит что samples они предоставить не могут

увы, AD гироскопы на халяву не раздает- наверное, конкуренция маленькая...
А какая у вас цель построения ИНС? если требования по точности,уходам и температуре не велики- то и мюрата на первый раз для экспериментов сойдет.
у меня где-то лежит дока где описана математика инерциалки на голых акселерометрах( по паре разнесенных для измерения угловых ускорений) - пишите в на почту- вышлю... с точки зрения стоимости- экономнее- акселерометры дешевле. с точки зрения габаритов и уходов- конечно хуже.

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

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

А вот при комплексировании - там ФК просто необходим.

Три блока акселерометров ставлю, так как мне их три прислали ))
Думаю раз уж есть три источника одной о той же информации, то можно же как то улудшить измеряемую величину.
Гироскопы тоже имеются. Так же три штуки - двухосевые.
Драфт схемы установки находится в приложенном файле.
Жду васших советов, рекомендаций и помощи.

И еще вот, я тут нашел. Вроде то что нужно, но не могли бы вы мне помочь с этим?
А то я как то совсем ничего не понял

Погуглил я книжку GPS, Inertial Navigation and Integration.
Вообщем книжка написана на профессионала в этом деле
Я много от туда не понял

Да она и без исходников Если бы еще файлы к ней были, может быть на примерах и понял что к чему.

Хотя видел в Интернете GPS, Inertial Navigation and Integration. Second Edition. Оглавление отличается от того, что написано в оглавлении первого издания.

ММожет в этой книжке и более понятно радовому пользователю понатно все расписано.

Если у кого уже есть второйе изданийе, не поделитесь а?
Плиззззз.

П.С. Комы интересно могу выложить файлы исходников от книжки Kalman Filtering Theory And Practice Using Matlab - Second Edition.

От исходников бы не отказался

Вот еще интересный проект: Исходники Кальмана для Motion Tracking

Вот пожалуйста исходники от книги описанной выше.
Может у кого нибудь есть исходники от книжки Global Positioning System, Inertial navigation and Integration?

асколько я понял в моем случае надо воспользоваться Многомерным цифровым фильтром Калмана.
Для этого мне нужно получить:
1) Модель сообщения
2) Модель Наблюдения
3) Априорные данные
4) Уравнение Фильтра Калмана
5) Коеффициент Усиления
6) Апостериорная Матрица Дисперсий
7) Начальные условия

Но вот как это все применить для моего случая я не знаю

Алгоритм получается следующий:
[font=Courier New]


Ничего не забыл?
кто разбирается может подскажет а? Нигде не напортачил???

Удалось запустить?
А почему выбран именно EKF? Если верить литературе то UKF лучше работает на нелинейных уравнениях и в реализации проще.

Difference Equation надо давать?

Пока запустить не удалось (
Нашлось несколько ошибочек.
Должно быть вот так:

и вот так вот






А как UKF построить?

Вот две статьи:
"A New Extension of the Kalman Filter to Nonlinear Systems" Simon J. Julier K. Uhlmann
Введение. Информации достаточно для реализации.

"THE SQUARE-ROOT UNSCENTED KALMAN FILTER FOR STATE AND PARAMETER-ESTIMATION" Rudolph van der Merwe and Eric A. Wan
Более эффективная реализация. ( QR + Холецкий )
Похоже что в коде по приведенной выше ссылке именно эта реализация алгоритма.

И еще вот недостающая информация.
[font=Courier New]

Как мне из всего, что здесь написано сворганить код для фильтра?

Здравствуйте. Я новинький на этом сайте и форуме. Но меня заинтересовал ваш разговор. Где вы учитесь и на каких факультетах, или где работаете ? Вот я Калмановскую и Винеровскую фильтрацию изучаю у себя в институте: Киевский авиационный университет, Факультет аэрокосимческих систем управления. С вашего разговора можна понять что вы очень развиты в этой сфере.
Меня вот интересует, все хорошие книги на эту тему можно найти только на английском языке или есть уже перевод. Спасибо.

Есть книжка "Оптимальное управление движением" В.В. Александров. Много всего интересного и сложного, в том числе о дискретном и непрерывном фильтре Кальмана.

Спасибо. Найду и прочитаю

Советую почитать книжки: Шахтарин "Случайные процессы в радиотехнике" и Балакришнан "Теория фильтрации Калмана", Пугачёв "Стохастические дифференциальные системы. Анализ и фильтрация"

Могу помочь Вам компонентом на Borlan Delphi 6.0 с примером.

В данном примере Вы можете изменить режим фильтра (фильтр с конечной памятью или фильтр с бесконечной памятью), задать порядок фильтра (от 0 до 10), установить размер усредняющего окна( 0 до 189, по сути полосу фильтра) и включить или выключить шум. Тестовая последовательностьпредставляет собой период синусоиды с наложенным или отсутствующим шумом. Перед началом тестирования нажмите кнопку "Сбросить фильтр". Далее можно исследовать работу фильтра в пошаговом режиме (кнопка "Очередной шаг") или запустить цикл (кнопка "Цикл"). Можно исследовать прогнозирующие возможности фильтра. Понятно, что этому должен предшествовать этап фильтрации (с точки зрения оценки вектора состояния фильтра это очень напоминает процесс обучения устройств, реализующих класс нелинейных оценок и более известных как искусственные нейронные сети)

Всего доброго друг!