Привет всем

Вот краткое руководство для реализации фильтра Калмана. Первоначально я написал это для статьи общества Robot несколько лет назад. Я пересмотрел это немного более четко и исправил некоторые ошибки в исходном сообщении.

Наслаждайтесь!

Введение

Фильтрация Калмана используется для многих применений, включая Фильтрация зашумленных сигналов, генерирование не наблюдаемых состояний, и прогнозирование будущих состояний. Фильтрация зашумленных сигналов имеет важное значение, так как многие датчики имеют выход, который слишком шумный, чтобы использоваться непосредственно, и фильтрация Калмана позволяет учесть неопределенность в сигнале / состоянии. Одной из важных особенностей генерации не наблюдаемых состояний является оценка скорости. Как правило, имеются датчики положения (энкодеры) на различных соединениях; Однако, простая дифференциация позиции, чтобы получить скорость, производит шумные результаты. Чтобы исправить это фильтрация Калмана может быть использована для оценки скорости. Еще одной приятной особенностью фильтра Калмана является то, что он может быть использован для прогнозирования будущих состояний. Это полезно, когда у вас есть большие временные задержки в вашей обратной связи датчика, поскольку это может привести к нестабильности в системе управления двигателем.

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

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

Фильтр

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

(У меня вопрос о том, почему я перечисляю положение и скорость. Ответ прост, если вы подумаете о квадрокоптере, его можно отметить в одном направлении во время полета / движения в другом направлении.)

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

УРАВНЕНИЯ ПРОГНОЗИРОВАНИЯ

УРАВНЕНИЯ ОБНОВЛЕНИЯ

Эти уравнения могут выглядеть страшно, так как есть много переменных, так что мы теперь уточним, что из себя представляет каждая переменная.
x , X = Эти переменные представляют свое состояние (то есть вещи, о которых Вы заботитесь и / или пытаетесь фильтровать). Например, если вы управляете механическим манипулятором с тремя шарнирами ваше состояние может быть:

Это должно инициализироваться в любом состоянии, с какого бы вы не хотели начать. Если вы рассматриваете свое начальное местоположение в качестве начала координат, тогда оно может быть инициализирована:

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

u = какое бы ни было действие для трех соединений механической руки, действия могут быть:

P , p = Эти числа представляют, насколько уверенно себя чувствует фильтр с решением. Лучший способ сделать это - инициализировать его как диагональную матрицу, когда фильтр работает она станет заселена. После запуска фильтра вы можете посмотреть на то, как сходятся значения и использовать их, чтобы инициализировать фильтр в следующий раз. Причина, по которой она должна быть инициализирована как диагональ в том, что каждая запись непосредственно соответствует дисперсии состояния в этой строке так что для указанной выше механического манипулятора, с шестью состояниями ковариационная матрица может быть инициализирована как:

где отклонение - variance - представляет собой квадрат стандартного отклонения ошибки.

Q = Является ковариацией шума процесса (т.е. действие). Он формируется по аналогии с выше за исключением того, что она является матрицей, которую вы определяете, и не обновляется с помощью фильтра. Эта матрица рассказывает фильтру Калмана, сколько ошибок в каждом действии с момента выдачи вами заданного напряжения, пока он на самом деле не происходит.
K = Эта матрица обновляется как часть измерения стадии обновления.
H = Является моделью датчиков, но ее трудно определить. Простой подход- инициализировать ее как диагональную матрицу идентичности и настроить, чтобы улучшить конечные результаты фильтрации.
R = Аналогично Q за исключением того, это определяет уверенность в каждом из датчиков. Это ключевая матрица для проведения синтеза датчика
z = Это измерения, которые возвращаются из датчиков в каждом месте. Фильтр Калмана обычно используется для очистки шума от этих сигналов или для оценки этих параметров, когда нет датчика.
I = единичная матрица (также диагональная)
Следующие переменные, которые мы должны определить, являются A и B. Эти матрицы - это модель, которая показывает, как ваша система переходит из одного состояния в другое.
Так как фильтр Калмана предназначен для линейных систем, мы можем предположить, что, если начать с самого начала (t = 0) и запустить вашу систему к следующему состоянию (t = 1), и запустите ее снова (t = 2), количество изменения будет таким же, так что мы не можем использовать это изменение, независимо от того, где (t = везде) в системе мы на самом деле находимся.

Самый простой способ найти матрицы А и В, если вы в состоянии собирать данные из вашей системы, это выполнить эксперименты перед созданием фильтра с системой без принуждения (входы должны быть 0). newState и lastState являются матрицы, чьи столбцы являются входные и выходные состояния, измеренные в ходе экспериментов. Для того, чтобы это работало, вы должны быть в состоянии измерить полное состояние, что часто не представляется возможным, если вы используете фильтр Калмана в качестве оценки состояния (который является частым в использовании). Это позволяет решить для А и В в цифровой форме. Вам нужно повторить эксперимент n-ное количество раз, где n есть размерность матрицы A.

тогда мы можем записать его в виде:

Следуя этой логике, как только у вас есть А вы можете найти B

Зная текущее состояние, какое последнее состояние было, A, и действие, которое вызвало изменение состояния, вы можете решить для B.

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

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

Матрица B может быть сделана таким же образом, за исключением того, что вместо взбудораживания состояния взбудораживаем действия и записываем состояния.

Таким образом, в псевдокоде это будет:

X 0 = состояние, которое вы линеаризуете;
U 0 = Напряжения, необходимые для производства (обратная динамика);

delta = малое число;
// Нахождение матрицы A

для ii =1 через # ofStates {
X = X 0;
X (ii )= X (ii )+ delta ; //беспокойное состояние x (i ) посредством delta
X 1 = f (X , U 0); // f () является моделью системы
A (:, ii ) = (X 1- X 0)/ delta ;
}

// Нахождение матрицы B

для ii =1 посредством # ofInputs {
U = U 0;
U (ii )= U (ii )+ delta ; // беспокойное состояние U (i ) посредством delta
X 1 = f (X 0, U ); // f () является моделью системы
B (:, ii ) = (X 1- X 0)/ delta ;
}

f(X0,U) это ваша модель. Эта модель может занять состояние и действие и определить, какое следующее состояние будет. Этот шаг включает в себя кинематику и динамику ровера. Помните, что если ваша модель не прогнозирует того, что происходит в реальной жизни, то ваша модель ошибается, и вы должны это исправить!

Теперь, когда мы знаем, что это все за переменные, я хочу повторить алгоритм со словами. Начнем с нашего текущего состояния x k -1 , текущей ковариационной матрицы P k -1 , а также текущего входа u k -1 чтобы получить ваше прогнозируемое состояние X и предсказанную ковариационную матрицу p. После этого вы можете получить измерения y k и использовать уравнения обновления, чтобы исправить свои предсказания для того, чтобы получить новое состояние матрицы x k и новый ковариационный P k . После того, как вы сделаете все это вы просто держите на итерации те шаги, где вы сделали прогноз, а затем обновите предсказание на основе некоторой известной информации.

Современные методы фильтров

Для нелинейной системы существует два основных подхода. Первый заключается в разработке расширенного фильтра Калмана (EKF - Extended Kalman Filter). Для EKF вам необходимо линеаризовать модель, а затем сформировать ваши матрицы А и В. Этот подход включает в себя немного математики и нечто, называемое Jacobean, который позволяет масштабировать различные значения по-разному. Второй и более простой подход состоит в использовании кусочного приближения. Для этого вы делаете разбивку данных в регионах, близких к линейным и образуете различные матрицы А и В для каждого региона. Это позволяет проверить данные и использовать соответствующие матрицы А и В в фильтре, чтобы точно предсказать, что переходное состояние будет.

Образец кода

Вот c++ код для фильтра Калмана предназначенный для PUMA 3 DOF роботизированного манипулятора. Этот код используется для оценки скорости, так как он является гораздо более точным, чем просто дифференциация позиции.

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

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

/*******************************************************
* роботизированный манипулятор PUMA 3 DOF фильтра Калмана
********************************************/
# include < fstream >
# include < iostream >
# include < unistd . h >
# include < stdlib . h >
# include < string . h >
# include < stdio . h >
// моя матричная библиотека, вы можете использовать свою любимую матричную библиотеку
# include “ matrixClass . h ”
# define TIMESTEP 0.05

используя пространство имен std ;

индекс резерва времени, m _ a 1, m _ a 2, m _ a 3, c _ a 1, c _ a 2, c _ a 3, tau 1, tau 2, tau 3;
резерв a 1, a 2, a 3, a 1 d , a 2 d , a 3 d , a 1 dd , a 2 dd , a 3 dd ;
резерв new _ a 1 d , new _ a 2 d , new _ a 3 d ;
резерв TIME =0;
матрица A (6,6);
матрица B (6,3);
матрица C = matrix :: eye (6); // инициализировать их как 6×6 матриц идентичности
матрица Q = matrix :: eye (6);
матрица R = matrix :: eye (6);
матрица y (6,1);
матрица K (6,6);
матрица x (6,1);
матричное состояние(6,1);
матричное действие(3,1);
матрица lastState (6,1);
матрица P = matrix :: eye (6);
матрица p = matrix :: eye (6);
матричный замер(6,1);

пустой initKalman (){

резерв a ={
{1.004, 0.0001, 0.001, 0.0014, 0.0000, -0.0003 },
{0.000, 1.000, -0.00, 0.0000, 0.0019, 0 },
{0.0004, 0.0002, 1.002, 0.0003, 0.0001, 0.0015 },
{0.2028, 0.0481, 0.0433, 0.7114, -0.0166, -0.1458 },
{0.0080, 0.0021, -0.0020, -0.0224, 0.9289, 0.005 },
{0.1895, 0.1009, 0.1101, -0.1602, 0.0621, 0.7404 }
};

резерв b = {
{0.0000, 0.0000 , 0.0000 },
{0.0000, 0.0000, -0.0000 },
{0.0000, -0.0000, 0.0000 },
{0.007, -0.0000, 0.0005 },
{0.0001, 0.0000, -0.0000 },
{0.0003, -0.0000, 0.0008 }
};

/* загружает матрицы А и В из выше */
для (int i = 0; i < 6; i ++){
для (int j = 0; j < 6; j ++){
A [ i ][ j ]= a [ i ][ j ];
}
}
для (int i = 0; i < 6; i ++){
для (int j = 0; j < 3; j ++){
B [ i ][ j ]= b [ i ][ j ];
}
}

/* инициализирует состояние*/
state=0.1;
state=0.1;
state=0.1;
state=0.1;
state=0.1;
state=0.1;

lastState= состояние ;

пустой kalman(){
lastState= состояние ;
state=c_a1;
state=c_a2;
state=c_a3;
state=a1d;
state=a2d;
state=a3d;

measurement=m_a1;
measurement=m_a2;
measurement=m_a3;
measurement=a1d;
measurement=a2d;
measurement=a3d;

action=tau1;
action=tau2;
action=tau3;

matrix temp1(6,6);
matrix temp2(6,6);
matrix temp3(6,6);
matrix temp4(6,1);
/************ Уравнения п рогнозировани я *****************/
x = A*lastState + B*action;
p = A*P*A’ + Q;
/************ Уравнения обновления **********/
K = p*C*pinv(C*p*C’+R);

y=C*state;

состояние = x + K*(y-C*lastState);

P = (eye(6) – K*C)*p;

a1=state;
a2=state;
a3=state;
a1d=state;
a2d=state;
a3d=state;
}

/* Эта функция не используется, так как я использую позиции, чтобы получить скорости (т.е. дифференциацию).
* Тем не менее я считаю, что это полезно включить, если вы хотите скорости и позицию
* от ускорения вы будете использовать его */
пустой integrate(){
new_a1d = a1d + a1dd*TIMESTEP;
a1 += (new_a1d + a1d)*TIMESTEP/2;
a1d = new_a1d;
new_a2d = a2d + a2dd*TIMESTEP;
a2 += (new_a2d + a2d)*TIMESTEP/2;
a2d = new_a2d;
new_a3d = a3d + a3dd*TIMESTEP;
a3 += (new_a3d + a3d)*TIMESTEP/2;
a3d = new_a3d;
TIME+=TIMESTEP;
}

/*Это дает мне скорость от позиции*/
пустой differentiation(){
a1d=(state-lastState)/TIMESTEP;
a2d=(state-lastState)/TIMESTEP;
a3d=(state-lastState)/TIMESTEP;
TIME+=TIMESTEP;
}

int main () {
initKalman();
char buffer;
ifstream readFile (“DATA.txt”); // вот тут я читал мои данные, так как я обрабатывал все это оффлайн

while (!readFile.eof()){
readFile.getline (buffer,500);
sscanf(buffer, “%f %f %f %f %f %f %f %f %f %f “,&timeIndex,&m_a1,&m_a2,&m_a3,&c_a1,&c_a2,&c_a3,&tau1,&tau2,&tau3);

kalman();
differentiation();
//integrate();

/* вот тут я вношу в журнал результаты и / или выношу их на экран */
FILE *file=fopen(“filterOutput.txt”, “a”);
fprintf(file,”%f %f %f %f %f %f %f %f %f %f \n”,TIME,a1,a2,a3,a1d,a2d,a3d,tau1,tau2,tau3);
fprintf(stderr,”%f %f %f %f %f %f %f %f %f %f \n”,TIME,a1,a2,a3,a1d,a2d,a3d,tau1,tau2,tau3);
fclose(file);
}
return 1;
}

Заключение

Этот пост был сосредоточен на реализации фильтра Калмана. Надеюсь, вы сможете принять эту информацию, чтобы улучшить и усовершенствовать ваши робототехнические проекты. Для получения дополнительной информации я рекомендую Введение в фильтрацию Калмана Грега Уэлча и Гэри Бишопа http://www.cs.unc.edu/~welch/kalman/ и пост из TKJ Electronics .

Этот фильтр применяют в разных областях – от радиотехники до экономики. Здесь мы обсудим основную идею, смысл, суть данного фильтра. Излагаться она будет максимально простым языком.
Предположим, что у нас есть необходимость в измерениях некоторых величин некоего объекта. В радиотехнике чаще всего имеют дело с измерениями напряжений на выходе некоего устройства (датчика, антенны и т.д.). В примере с электрокардиографом (см. ) мы имеем дело с измерениями биопотенциалов на теле человека. В экономике, например, измеряемой величиной могут быть курсы валют. Каждыё день курс валют разный, т.е. каждый день “его измерения” дают нам разную величину. А если обобщать, то можно сказать, что большая часть деятельности человека (если не вся) сводится именно к постоянным измерениям-сравнениям тех или иных величин (см. книгу).
Итак, предположим, что мы что-то постоянно измеряем. Так же предположим, что наши измерения всегда идут с некоторой ошибкой – оно и понятно, ведь нет идеальных измерительных приборов, и каждый выдаёт результат с ошибкой. В простейшем случае описанное можно свести к следующему выражению: z=x+y, где x – истинное значение, которое мы хотим измерить и которое измерили бы если бы у нас был идеальный измерительный прибор, y – ошибка измерения, вносимая измерительным прибором, а z – измеренная нами величина. Так вот задача фильтра Калмана состоит в том, чтобы по измеренной нами z всё-таки догадаться (определить), а какое же истинное значение x было, когда мы получали нашу z (в которой "сидит" истинное значение и ошибка измерения). Необходимо отфильтровать (отсеять) из z истинное значение x – убрать из z искажающий шум y. То есть, имея на руках только лишь сумму нам необходимо догадаться о том, какие слагаемые дали эту сумму.
В свете вышеописанного сформулируем теперь всё следующим образом. Пусть есть всего лишь два случайных числа. Нам даётся только их сумма и от нас требуется по этой сумме определить, какими являются слагаемые. Например, нам дали число 12 и говорят: 12 – это сумма чисел x и y, вопрос – чему равны x и y. Чтобы ответить на этот вопрос, составляем уравнение: x+y=12. Мы получили одно уравнение с двумя неизвестными, поэтому, строго говоря, найти два числа которые и дали эту сумму не возможно. Но кое-что об этих числах мы всё-таки можем сказать. Мы можем сказать, что это были либо числа 1 и 11, либо 2 и 10, либо 3 и 9, либо 4 и 8 и т.д., также это либо 13 и -1, либо 14 и -2, либо 15 и -3 и т.д. То есть мы можем по сумме (в нашем примере 12) определить множество возможных вариантов, которые дают в сумме именно 12. Один из этих вариантов – это искомая нами пара, которая на самом деле прямо сейчас и дала 12. Нелишне так же отметить, что все варианты пар чисел дающих в сумме 12 образуют прямую, изображённую на рис.1, которая и задаётся уравнением x+y=12 (y=-x+12).

Рис.1

Таким образом, искомая нами пара лежит где-то на этой прямой. Повторюсь, выбрать из всех этих вариантов ту пару, которая была на самом деле – которая дала число 12, не владея какими-либо дополнительными подсказками, невозможно. Однако, в ситуации, для которой изобретён фильтр Калмана, такие подсказки есть . Там заранее о случайных числах кое-что известно. В частности там известна так называемая гистограмма распределения для каждой пары чисел. Она обычно бывает получена после достаточно длительных наблюдений за выпадениями этих самых случайных чисел. То есть, например, из опыта известно, что в 5% случаев обычно выпадает пара x=1, y=8 (обозначим эту пару так: (1,8)), в 2% случаев пара x=2, y=3 (2,3), в 1% случаев пара (3,1), в 0.024% случаев пара (11,1) и т.д. Повторюсь, эта гистограмма задана для всех пар чисел, в том числе и для тех, что образуют в сумме 12. Таким образом, для каждой пары, что даёт в сумме 12, мы можем сказать, что, например, пара (1, 11) выпадает в 0.8% случаев, пара (2, 10) – в 1% случаев, пара (3, 9) – в 1.5% случаев и т.д. Таким образом, мы можем по гистограмме определить, в скольких процентах случаев сумма слагаемых пары равна 12. Пусть, например, в 30% случаев сумма даёт 12. А в остальных 70% выпадают остальные пары – это (1,8), (2,3), (3,1) и т.д. – те, что в сумме дают числа отличные от 12. Причём пусть, например, пара (7,5) выпадает в 27% случаев в то время, как все остальные пары, что дают в сумме 12, выпадают в 0.024%+0.8%+1%+1.5%+…=3% случаев. Итак, по гистограмме мы выяснили, что числа дающие в сумме 12 выпадают в 30% случаев. При этом мы знаем, что если выпало 12, то чаще всего (в 27% из 30%) причиной этого является пара (7,5). То есть если уже выпало 12, то мы можем сказать, что в 90% (27% из 30% – или, что то же самое 27 раз из каждых 30-ти) причиной выпадения 12 является пара (7,5). Зная, что чаще всего причиной получения суммы равной 12 является пара (7,5) логично предположить, что, скорее всего, она выпала и сейчас. Конечно, всё-таки не факт, что на самом деле сейчас число 12 образовано именно этой парой, однако, в следующие разы, если нам попадётся 12, и мы опять предположим пару (7,5), то где-то в 90% случаев из 100% окажемся правы. А вот если мы будем предполагать пару (2, 10), то окажемся правы лишь в 1% из 30% случаев, что равно 3.33% правильных догадок по сравнению с 90% при предположении пары (7,5). Вот и всё – в этом и состоит смысл алгоритма фильтра Калмана. То есть фильтр Калмана не гарантирует, что не ошибётся в определении слагаемого по сумме, однако он гарантирует, что ошибётся минимальное количество раз (вероятность ошибки будет минимальна), так как использует статистику – гистограмму выпадения пар чисел. Так же необходимо подчеркнуть, что часто в алгоритме фильтрации Калмана используется так называемая плотность распределения вероятности (ПРВ). Однако необходимо понимать, что смысл там тот же, что и у гистограммы. Более того, гистограмма – это функция, построенная на основе ПРВ и являющаяся её приближением (см., например, ).
В принципе мы эту гистограмму можем изобразить в виде функции двух переменных – то есть в виде некоей поверхности над плоскостью xy. Там, где поверхность выше, там выше и вероятность выпадения соответствующей пары. На рис.2 изображена такая поверхность.

рис.2

Как видно над прямой x+y=12 (которая есть варианты пар дающих в сумме 12) расположены точки поверхности на разной высоте и наибольшая высота у варианта с координатами (7,5). И когда нам встречается сумма равная 12, в 90% случаев причиной появления этой суммы является именно пара (7,5). Т.е. именно эта пара, дающая в сумме 12, имеет наибольшую вероятность появления при условии, что сумма равна 12.
Таким образом, здесь описана идея лежащая в основе фильтра Калмана. Именно на ней и построены всевозможные его модификации – одношаговые, многошаговые рекуррентные и т.д. Для более глубокого изучения фильтра Калмана рекомендую книгу: Ван Трис Г. Теория обнаружения, оценок и модуляции.

p.s. Для того, кто интересуется объяснениями понятий математики что называется "на пальцах" можно посоветовать вот эту книгу и в частности главы из её раздела "Математика" (саму книгу или отдельные главы из неё вы можете приобрести ).

25 марта 2012 в 20:42

Фильтр Калмана - Введение

• Алгоритмы ,
• Программирование

Фильтр Калмана - это, наверное, самый популярный алгоритм фильтрации, используемый во многих областях науки и техники. Благодаря своей простоте и эффективности его можно встретить в GPS-приемниках, обработчиках показаний датчиков, при реализации систем управления и т.д.

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

Для чего он нужен?

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

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

Рассмотрим простейший пример - предположим нам необходимо контролировать уровень топлива в баке. Для этого в бак устанавливается емкостный датчик, он очень прост в обслуживании, но обладает некоторыми недостатками - например, зависимость от заправляемого топлива (диэлектрическая проницаемость топлива зависит от многих факторов, например, от температуры), большое влияние «болтанки» в баке. В итоге, информация с него представляет типичную «пилу» с приличной амплитудой. Такого рода датчики часто устанавливаются на тяжелой карьерной технике (не смущайтесь объемам бака):

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

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

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

Разберемся сначала в обозначениях: подстрочный индекс обозначает момент времени: k - текущий, (k-1) - предыдущий, знак «минус» в верхнем индексе обозначает, что это предсказанное промежуточное значение.

Описание переменных представлены на следующих изображениях:

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

Опробуем в деле

Вернемся к примеру с датчиком уровня топлива, так как состояние системы представлено одной переменной (объем топлива в баке), то матрицы вырождаются в обычные уравнения:

Определение модели процесса

Для того, чтобы применить фильтр, необходимо определить матрицы/значения переменных определяющих динамику системы и измерений F, B и H:

F - переменная описывающая динамику системы, в случае с топливом - это может быть коэффициент определяющий расход топлива на холостых оборотах за время дискретизации (время между шагами алгоритма). Однако помимо расхода топлива, существуют ещё и заправки… поэтому для простоты примем эту переменную равную 1 (то есть мы указываем, что предсказываемое значение будет равно предыдущему состоянию).

B - переменная определяющая применение управляющего воздействия. Если бы у нас были дополнительная информация об оборотах двигателя или степени нажатия на педаль акселератора, то этот параметр бы определял как изменится расход топлива за время дискретизации. Так как управляющих воздействий в нашей модели нет (нет информации о них), то принимаем B = 0.

H - матрица определяющая отношение между измерениями и состоянием системы, пока без объяснений примем эту переменную также равную 1.

Определение сглаживающих свойств

R - ошибка измерения может быть определена испытанием измерительных приборов и определением погрешности их измерения.

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

Реализуем в коде

Чтобы развеять оставшиеся непонятности реализуем упрощенный алгоритм на C# (без матриц и управляющего воздействия):

Class KalmanFilterSimple1D { public double X0 {get; private set;} // predicted state public double P0 { get; private set; } // predicted covariance public double F { get; private set; } // factor of real value to previous real value public double Q { get; private set; } // measurement noise public double H { get; private set; } // factor of measured value to real value public double R { get; private set; } // environment noise public double State { get; private set; } public double Covariance { get; private set; } public KalmanFilterSimple1D(double q, double r, double f = 1, double h = 1) { Q = q; R = r; F = f; H = h; } public void SetState(double state, double covariance) { State = state; Covariance = covariance; } public void Correct(double data) { //time update - prediction X0 = F*State; P0 = F*Covariance*F + Q; //measurement update - correction var K = H*P0/(H*P0*H + R); State = X0 + K*(data - H*X0); Covariance = (1 - K*H)*P0; } } // Применение... var fuelData = GetData(); var filtered = new List(); var kalman = new KalmanFilterSimple1D(f: 1, h: 1, q: 2, r: 15); // задаем F, H, Q и R kalman.SetState(fuelData, 0.1); // Задаем начальные значение State и Covariance foreach(var d in fuelData) { kalman.Correct(d); // Применяем алгоритм filtered.Add(kalman.State); // Сохраняем текущее состояние }

Результат фильтрации с данными параметрами представлен на рисунке (для настройки степени сглаживания - можно изменять параметры Q и R):

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

Надеюсь описание получилось не сильно утомительным и сложным, если остались вопросы и уточнения - добро пожаловать в комментарии)

UPD: Список источников.

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

Рис. 4.2. Оптимальный фильтр Калмана.

На рис. 4.2 это показано для непрерывного случая.

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

модель входного сигнала описывается системой линейных разностных уравнений.

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

К оценке предъявляется требование несмещенности, т. е. ее математическое ожидание

Выражение (4.9) записывается также в другом виде. При заданных измерениях величины от момента до момента I оценка в некоторый момент времени должна обладать свойством

Кроме того, накладывается условие минимума дисперсии ошибки оценки, которое записывается в виде

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

При использовании фильтров Калмана возможны следующие случаи.

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

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

В установившемся состоянии фильтр Калмана совпадает с фильтром Винера и дает то же значение оценки.

2. В дискретных системах возможна постановка задачи оптимального определения оценки для времени по данным измерений входного сигнала в временных точках от до , т. е. задачи оптимального предсказания на один (или несколько) такт вперед. Эта задача имеет смысл и в случае равенства нулю помех измерительного устройства.

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

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

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

такт вперед в некоторых случаях теряет свой смысл и может привести к неправильным решениям конкретной технической задачи. Однако прогнозирование на несколько тактов вперед обычно не теряет своего смысла и при малых периодах дискретности. Но в этом случае оно практически совпадает со случаем прогнозирования в непрерывных системах.

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

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

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

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

Использование реальных фильтров. В некоторых случаях построения систем управления входной сигнал задан своими

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

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

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

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

Учет влияния малых постоянных времени накладывает на проектируемую систему некоторые ограничения, которых обычно нет при решении задачи оптимального синтеза. Эти ограничения в принципе могут быть учтены в виде

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

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

При отсутствии взаимной корреляции между полезным сигналом и помехой дисперсия ошибки

Дифференцирование (4.14) по коэффициенту усиления и приравнивание производной нулю дает условие минимума дисперсии ошибки

Подстановка (4.15) в (4.14) дает минимальное значение дисперсии ошибки, соответствующее оптимальному значению общего коэффициента усиления:

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

где - суммарное временное запаздывание, которое не может быть устранено в системе управления при выбранных

элементах. Такое суммарное временное запаздывание может быть введено, например, при наличии нескольких апериодических звеньев с малыми постоянными времени. Дисперсия ошибки в этом случае

Интеграл (4.18) не берется элементарным образом. Приближенное его вычисление дает

Минимум дисперсии ошибки будет при выполнении условия

Подстановка этого значения коэффициента усиления в (4.19) дает минимальную дисперсию ошибки

Приравнивание (4.16) и (4.21) позволяет определить уровень эквивалентного белого шума

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

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

использование типовых передаточных функций, типовых переходных характеристик, типовых логарифмических частотных характеристик и т. п.

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

Рис. 4.3. Реальный одномерный фильтр.

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

Таким образом, здесь приходится иметь дело с полужесткой структурой системы управления. Заметим, что сформулированное выше понятие неизменяемой части системы несколько отличается от обычно используемого в литературе, когда под неизменяемой частью просто понимается объект управления с его передаточной функцией. Дело в том, что применение различных корректирующих средств (последовательного типа, параллельного типа, обратных связей) позволяет активно влиять на передаточнуюфункциюобъекта, меняя ее нужным образом. Однако это может делаться только

до некоторого предела, который характеризуется минимальными значениями «остаточных» постоянных времени или временных запаздываний, которыми располагает конструктор. Поэтому неизменяемая часть системы определяется здесь в этом смысле.

При построении реальных фильтров возможны следующие случаи.

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

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

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

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

В то же время задача построения системы с требуемой точностью при имеющихся исходных данных может и не

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

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

Рис. 4.4. Реальный многомерный фильтр.

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

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

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

И сегодня мы поговорим о фильтре Калмана , разберемся, что это такое, для чего и как он применяется.

Начнем с небольшого примера. Пусть перед нами стоит задача определять координату летящего самолета. Причем, естественно, координата (обозначим ее ) должна определяться максимально точно.

На самолете мы заранее установили датчик, который и дает нам искомые данные о местоположении, но, как и все в этом мире, наш датчик неидеален. Поэтому вместо значения мы получаем:

где – ошибка датчика, то есть случайная величина. Таким образом, из неточных показаний измерительного оборудования мы должны получить значение координаты (), максимально близкое к реальному положению самолета.

Задача поставлена, перейдем к ее решению.

Пусть мы знаем управляющее воздействие (), благодаря которому летит самолет (пилот сообщил нам, какие рычаги он дергает 😉). Тогда, зная координату на k-ом шаге, мы можем получить значение на (k+1) шаге:

Казалось бы, вот оно, то что надо! И никакой фильтр Калмана тут не нужен. Но не все так просто.. В реальности мы не можем учесть все внешние факторы, влияющие на полет, поэтому формула принимает следующий вид:

где – ошибка, вызванная внешним воздействием, неидеальностью двигателя итп.

Итак, что же получается? На шаге (k+1) мы имеем, во-первых, неточное показание датчика , а во-вторых, неточно рассчитанное значение , полученное из значения на предыдущем шаге.

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

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

А теперь предположим, что связи с пилотом нет, и мы не знаем управляющее воздействие . Казалось бы, в этом случае фильтр Калмана мы использовать не можем, но это не так 😉 Просто “выкидываем” из формулы то, что мы не знаем, тогда

Получаем максимально упрощенную формулу Калмана, которая тем не менее, несмотря на такие “жесткие” упрощения, прекрасно справляется со своей задачей. Если представить результаты графически, то получится примерно следующее:

Если наш датчик очень точный, то естественно весовой коэффициент K должен быть близок к единице. Если же ситуация обратная, то есть датчик у нас не очень хороший, то K должен быть ближе к нулю.

На этом, пожалуй, все, вот так вот просто мы разобрались с алгоритмом фильтрации Калмана! Надеюсь, что статья оказалась полезной и понятной =)