Фильтр Калмана - реализация, параметры и настройка

Прежде всего, я впервые пытаюсь сделать фильтр Калмана.

Ранее я опубликовал следующий вопрос « Отфильтровать шум и отклонения от значений скорости» в StackOverflow, в котором описывается фон для этого поста. Это типичный образец значений, которые я пытаюсь отфильтровать. Они не обязательно должны уменьшаться, что имеет место здесь. Но скорость изменения, как правило, так

X ------- Y
16 --- 233,75
24 --- 234,01
26 --- 234,33
32 --- 234,12
36 --- 233,85
39 --- 233,42
47 --- 233,69
52 --- 233,68
55 --- 233,76
60 --- 232,97
66 --- 233,31
72 --- 233,99

Я реализовал свой фильтр Калмана в соответствии с этим руководством: Фильтр Калмана для чайников .

Моя реализация выглядит так (псевдокод).

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

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

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

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

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

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

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

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

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

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

« Эй, у меня есть объект, который движется с постоянной скоростью, и я хочу оценить эту постоянную скорость »

$x_k$$k$$y_k$

$$x_k = x_{k-1}$$ $$y_k = x_k + q_k$$

Но ваш объект движется не так. Его скорость меняется, и вы не знаете, как и когда это изменится.

Вместо этого вы должны сказать:

« Эй, у меня есть объект, который движется со скоростью, но я не уверен, как он меняет свою скорость »

Есть много способов сделать это: Самый простой способ - добавить неопределенность в ваше состояние.

$$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$$ $$y_k = x_k + q_k$$ $q_k$$v_k$

Ваши уравнения фильтра Калмана будут выглядеть так:

$$\hat y_{k|k-1} = \hat x_{k|k-1}$$ $$K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$$ $$\hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$$ $$P_{k|k} = P_{k|k-1} - K_kP_{k|k-1}$$ $$P_{k+1|k} =P_{k|k} + Q^s$$

0.05$Q^o$$Q^s$

В вашем коде небольшое изменение будет:

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

Не добавляя stateVarianceили$Q^s$

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

Таким образом, ваше усиление Калмана не пойдет на ноль.

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

Я бы предложил

$$y = \sum\limits_{i =0}^n a_i\, x^i$$

$$a[k+1] = a[k] + w$$ $$cov(w) = Q$$ $$z = \sum\limits_{i =0}^n a_i\, x^i = y$$

$x$$a$

Я думаю, что вы могли бы использовать некоторые идеи из классической теории управления, например, ПИД-регулятор .

Ваш сигнал Y может быть уставкой контроллера u (t). У технологической установки всего 1 и у (т) будет отфильтрован выход. Все, что вам нужно сделать, это установить параметры (мелодию) P, I и D, чтобы получить то, что вы хотите.

Выход y (t) будет пытаться «следовать» за входом u (t), но параметры управляют тем, каким будет это отслеживание.

Дифференциальное усиление D сделает ваш ответ чувствительным к быстрым изменениям ошибок. В вашем случае, я думаю, что D должно быть маленьким. Вы не хотите, чтобы y (t) изменился, если u (t) внезапно изменится.

Интегральное усиление «I» сделает ваш ответ чувствительным к накопленной ошибке. Вы должны поставить высокую ценность там. Если u (t) изменяет уровень и сохраняет его там, ошибка будет накапливаться, а затем вы хотите, чтобы y (t) делал то же самое.

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

Хотя есть и сложные методы настройки, но я не верю, что они вам понадобятся.

Удачи.

Вот простая и понятная реализация фильтра Калмана, которая соответствует обозначениям, как на странице Википедии. https://github.com/zziz/kalman-filter