Фильтр Калмана – Руководство по практической реализации (с кодом!)

автор Дэвид Коханбаш 30 января, 2014

 

 

Привет всем

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

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


 

Введение

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

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

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

Фильтр

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

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

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

УРАВНЕНИЯ ПРОГНОЗИРОВАНИЯ
X=Ax_{k-1} + Bu_{k-1}
p=APA^{T} + Q

УРАВНЕНИЯ ОБНОВЛЕНИЯ
K=pH^{T} (HpH^{T}+R)^{-1}
x_{k}=X + K(z-HX)
Px_{k}=(I-KH)p

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

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

\begin{bmatrix}0\0\0\0\0\0\end{bmatrix}

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

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.

Тогда: newState=A*lastState

тогда мы можем записать его в виде: A=newState * psudoinverse(lastState)

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

x=A*lastState + B*Action

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

B=[x-(A*lastState)]*psudoinverse(action)

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

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

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

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

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

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

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

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

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

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

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

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

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

Образец кода

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

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

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

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

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

индекс резерва времени,m_a1,m_a2,m_a3,c_a1,c_a2,c_a3,tau1,tau2,tau3;
резерв
a1,a2,a3,a1d,a2d,a3d,a1dd,a2dd,a3dd;
резерв
new_a1d, new_a2d, new_a3d;
резерв
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[6][6]={
{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[6][3] = {
{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][0]=0.1;
state[1][0]=0.1;
state[2][0]=0.1;
state[3][0]=0.1;
state[4][0]=0.1;
state[5][0]=0.1;

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

}

пустой kalman(){
lastState=
состояние;
state[0][0]=c_a1;
state[1][0]=c_a2;
state[2][0]=c_a3;
state[3][0]=a1d;
state[4][0]=a2d;
state[5][0]=a3d;

measurement[0][0]=m_a1;
measurement[1][0]=m_a2;
measurement[2][0]=m_a3;
measurement[3][0]=a1d;
measurement[4][0]=a2d;
measurement[5][0]=a3d;

action[0][0]=tau1;
action[1][0]=tau2;
action[2][0]=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[0][0];
a2=state[1][0];
a3=state[2][0];
a1d=state[3][0];
a2d=state[4][0];
a3d=state[5][0];
}

/* Эта функция не используется, так как я использую позиции, чтобы получить скорости (т.е. дифференциацию).
* Тем не менее я считаю, что это полезно включить, если вы хотите скорости и позицию

* от ускорения вы будете использовать его */
пустой 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[0][0]-lastState[0][0])/TIMESTEP;
a2d=(state[1][0]-lastState[1][0])/TIMESTEP;
a3d=(state[2][0]-lastState[2][0])/TIMESTEP;
TIME+=TIMESTEP;
}

int main () {
initKalman();
char buffer[500];
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.

Подкаст Создание встраиваемых систем имеет большое обсуждение инерциальных датчиков и фильтрации Калмана, которое вы можете послушать.

Ссылки

[1] – Мэйбек, Питер С. 1979. Стохастические модели, оценка и контроль, Том 1, Academic Press, Inc.
[2] – Уэлч, Грег. Бишоп, Гэри. Фильтр Калманаhttp://www.cs.unc.edu/~welch/kalman/

 

Оригинал статьи находится здесь. Сайт http://robotsforroboticists.com/