Фильтр Калмана
Вот и настал момент, когда пришлось с ним познакомиться поближе. Вкратце, это математический аппарат, который позволяет сглаживать данные на лету, не накапливая их для анализа. Используется для обработки показаний датчиков и любых приборов. Как правило, такие показания подвержены шумам и отклонениям, их нужно отсекать. Фильтра Калмана (ФК) позволяет отбрасывать пики и видеть усредненную, более вероятную картину процесса.
Рудольф Калман. Что бы мы без него делали. А ведь когда он придумал свой фильтр в 1960 году, то встретился с таким скептицизмом, что был вынужден опубликовать первые работы о нем в журнале, связанном с механикой, хотя сам занимался электротехникой.
Применений множество. Представьте датчик топлива в баке автомобиля. Он же болтается там внутри, если просто записать его показания, а потом попытаться проанализировать, получится, что в какой-то момент в баке было 30 л, через секунду 50, а потом 27. А это просто волны внутри гуляют. Сколько же в этот момент было в баке? ФК может дать более вероятный ответ.
Я сейчас делаю навигационные компоненты для iOS и столкнулся с проблемой биения сигнала и резкими скачками позиции при включении приемника. Переключение со спутникового сигнала на вайфай тоже дает неприятные перепады. Кроме того, моментальную и среднюю скорость тоже неплохо бы фильтровать. И начал разбираться.
Материалов по ФК много, мне показались лучшими эти документы:
Если очень кратко, то суть в том, что большинство явлений подчиняются Гауссовскому распределению вероятностей.
Например, мы думаем, что находимся в точке x. Но мы ведь можем ошибаться, потому, например, что отмеряли расстояние до x каким-нибудь прибором, а он, конечно, врет. Поэтому мы считаем, что находимся в точке x с какой-то вероятностью p. А может, мы сейчас в точке x', но вероятность этого меньше — p'. Ну такое вот распределение вероятностей.
Наиболее вероятный x посредине — это μ, математическое ожидание. Ширина этой "шляпы" зависит от σ2 — это дисперсия, квадрат стандартного отклонения. Чем больше отклонение, тем шляпа шире и неопределенность больше.
У этой кривой такое уравнение:
(1) |
Но его бояться не надо, т.к. нам надо искать середину, самый вероятный x. А в середине он равен μ, в числителе степени разница между x и μ дает 0, это многое потом упрощает.
Что же делает фильтр? Он помнит только одно значение x1 и его дисперсию σ2. Когда поступает новое значение x2 со своим отклонением r2, он умножает их (по правилам распределения, см. формулу 2) друг на друга и в результате будет новое значение x3 с погрешностью меньшей, чем у двух предыдущих!
Например, текущее значение фильтра — синее распределение. Поступает новое значение, со своим отклонением (красное). Фильтр перемножает их по правилам распределения (см. формулу 2) и получает новое распределение (зеленое). Оно гораздо точнее двух изначальных, т.к. его дисперсия меньше (см. формулу 3) и становится текущим значением фильтра.
Умножение распределений делается так:
(2) |
Дисперсия тоже уточняется:
(3) |
И тоже становится текущей. Поступает еще значение, состояние еще больше уточняется и так далее.
Я разобрался с принципом работы фильтра, пройдя
В процессе прохождения этого курса я написал скалярную и матричную версию фильтра, выкладываю их здесь.
Матричная версия, классический фильтр Калмана:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 | # Multi-dimensional Kalman Filter from online course # https://www.udacity.com/course/viewer#!/c-cs373/l-48723604/m-48670988 # written by Pavel Malinnikov, May 2013 from math import * from random import * import matplotlib.pyplot as plt class matrix: # implements basic operations of a matrix class def __init__(self, value): self.value = value self.dimx = len(value) self.dimy = len(value[0]) if value == [[]]: self.dimx = 0 def zero(self, dimx, dimy): # check if valid dimensions if dimx < 1 or dimy < 1: raise ValueError, "Invalid size of matrix" else: self.dimx = dimx self.dimy = dimy self.value = [[0 for row in range(dimy)] for col in range(dimx)] def identity(self, dim): # check if valid dimension if dim < 1: raise ValueError, "Invalid size of matrix" else: self.dimx = dim self.dimy = dim self.value = [[0 for row in range(dim)] for col in range(dim)] for i in range(dim): self.value[i][i] = 1 def show(self): for i in range(self.dimx): print self.value[i] print ' ' def __add__(self, other): # check if correct dimensions if self.dimx != other.dimx or self.dimy != other.dimy: raise ValueError, "Matrices must be of equal dimensions to add" else: # add if correct dimensions res = matrix([[]]) res.zero(self.dimx, self.dimy) for i in range(self.dimx): for j in range(self.dimy): res.value[i][j] = self.value[i][j] + other.value[i][j] return res def __sub__(self, other): # check if correct dimensions if self.dimx != other.dimx or self.dimy != other.dimy: raise ValueError, "Matrices must be of equal dimensions to subtract" else: # subtract if correct dimensions res = matrix([[]]) res.zero(self.dimx, self.dimy) for i in range(self.dimx): for j in range(self.dimy): res.value[i][j] = self.value[i][j] - other.value[i][j] return res def __mul__(self, other): # check if correct dimensions if self.dimy != other.dimx: raise ValueError, "Matrices must be m*n and n*p to multiply" else: # subtract if correct dimensions res = matrix([[]]) res.zero(self.dimx, other.dimy) for i in range(self.dimx): for j in range(other.dimy): for k in range(self.dimy): res.value[i][j] += self.value[i][k] * other.value[k][j] return res def transpose(self): # compute transpose res = matrix([[]]) res.zero(self.dimy, self.dimx) for i in range(self.dimx): for j in range(self.dimy): res.value[j][i] = self.value[i][j] return res # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions def Cholesky(self, ztol = 1.0e-5): # Computes the upper triangular Cholesky factorization of # a positive definite matrix. res = matrix([[]]) res.zero(self.dimx, self.dimx) for i in range(self.dimx): S = sum([(res.value[k][i]) ** 2 for k in range(i)]) d = self.value[i][i] - S if abs(d) < ztol: res.value[i][i] = 0.0 else: if d < 0.0: raise ValueError, "Matrix not positive-definite" res.value[i][i] = sqrt(d) for j in range(i + 1, self.dimx): S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)]) if abs(S) < ztol: S = 0.0 res.value[i][j] = (self.value[i][j] - S) / res.value[i][i] return res def CholeskyInverse(self): # Computes inverse of matrix given its Cholesky upper Triangular # decomposition of matrix. res = matrix([[]]) res.zero(self.dimx, self.dimx) # Backward step for inverse. for j in reversed(range(self.dimx)): tjj = self.value[j][j] S = sum([self.value[j][k] * res.value[j][k] for k in range(j + 1, self.dimx)]) res.value[j][j] = 1.0 / tjj ** 2 - S / tjj for i in reversed(range(j)): res.value[j][i] = res.value[i][j] = -sum([self.value[i][k] * res.value[k][j] for k in range(i + 1, self.dimx)]) / self.value[i][i] return res def inverse(self): aux = self.Cholesky() res = aux.CholeskyInverse() return res def __repr__(self): return repr(self.value) ######################################## result = [] def kalman(x, P): for n in range(len(positions)): # measurement update z = matrix([[positions[n]]]) y = z - H * x S = H * P * H.transpose() + R K = P * H.transpose() * S.inverse() x = x + (K * y) P = (I - K * H) * P # prediction x = F * x + u P = F * P * F.transpose() result.append(x.value[0][0]) speed.append(x.value[1][0]) ######################################## seed() data = [5. + x * .3 for x in range(50)] positions = [x + random() * randint(-1, 1) * 5 for x in data] x = matrix([[0.], [0.]]) # initial state (location and velocity) P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty u = matrix([[0.], [0.]]) # external motion F = matrix([[1., 1.], [0, 1.]]) # next state function H = matrix([[1., 0.]]) # measurement function R = matrix([[4.]]) # measurement uncertainty I = matrix([[1., 0.], [0., 1.]]) # identity matrix speed = [] kalman(x, P) plt.plot(data, 'b') plt.plot(positions, 'r') plt.plot(result, 'g') plt.plot(speed, 'y') plt.show() |
А это скалярная, упрощенная реализация. Я сразу попробовал ее на раздельном сглаживании широты и долготы. Тренировался на
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 | # One-dimensional Kalman Filter testing for geo-track data smoothing # Tracks were taken from http://www.openstreetmap.org/traces # Pavel Malinnikov, May 2013 import matplotlib.pyplot as plt import xml.etree.ElementTree as ET tree = ET.parse('/Users/pavelmalinnikov/Downloads/1464857.gpx') root = tree.getroot() namespaces = {'ns':"http://www.topografix.com/GPX/1/1"} # namespaces = {'ns':"http://www.topografix.com/GPX/1/0"} points = root.findall(".//ns:trkpt", namespaces = namespaces) if len(points) == 0: print 'Track loading failed' exit() resultLng = [] resultLat = [] def update(mean1, var1, mean2, var2): new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2) new_var = 1 / (1 / var1 + 1 / var2) return [new_mean, new_var] def predict(mean1, var1, mean2, var2): new_mean = mean1 + mean2 new_var = var1 + var2 return [new_mean, new_var] positions = [[float(p.get('lon')), float(p.get('lat'))] for p in points] longitudes = [position[0] for position in positions] latitudes = [position[1] for position in positions] motion_sig = 1. measurement_sig = 10. lng = longitudes[0] lat = latitudes[0] sigLng = 1. sigLat = 1. for i in range(len(positions)): lng, sigLng = update(lng, sigLng, longitudes[i], measurement_sig) lng, sigLng = predict(lng, sigLng, 0, motion_sig) resultLng.append(lng) lat, sigLat = update(lat, sigLat, latitudes[i], measurement_sig) lat, sigLat = predict(lat, sigLat, 0, motion_sig) resultLat.append(lat) kalman = [[resultLng[i], resultLat[i]] for i in range(len(positions))] plt.plot(*zip(*positions), color = 'r') plt.plot(*zip(*kalman), color = 'g') plt.show() |
Вообще для таких целей как раз должна использоваться матричная версия, но для этого ее нужно переписать для матриц размером 4*4.
Скалярный вариант тоже справляется неплохо. Вот сглаженный трек (он специально упрощен больше чем надо, для наглядности):
Неплохо обрабатывает путь, убирает шум (красная линия — данные gps, зеленая — сглаженный путь):
Я думаю, ФК еще пригодится мне не раз. Он нужен везде. Так что выходные были потрачены не зря.
← Яндекс-острова | Полевые испытания фильтра Калмана → |
Комментариев: 11
Интересный пост!!! Достаточно просто, понятно и доходчиво говорить о сложном не каждому дано.
Так держать!!!
Отличная статья. Автору почет и уважение
Замечательная статья! Я её использую на занятиях со студентами (специальность не техническая, будущие менеджеры). Но надо немного поправить текст в этом месте "Например, текущее значение фильтра — синее распределение. Поступает новое значение, со своим отклонением (красное). Фильтр перемножает их и получает новое распределение (зеленое)". После "...перемножает их" надо добавить и умножает на нормировочную константу. Если это не сделать, зеленое распределение будет ниже синего и красного, соответственно площадь под кривой меньше единицы - нарушается условие нормировки. Эти три красивые кривые наглядно демонстрируют, что фильтр Калмана реализует байесовский подход. Я всегда объясняю формулу Байеса так - апостериорное распределение равно произведению трёх сомножителей: 1-нормировочная константа, 2-априорное (в реализации Калмана эктраполированное) распределение, 3- функция правдоподобия. С уважением к автору Павлу Малинникову, спасибо за статью!
Сергей, большое спасибо за комментарий. У меня сейчас совсем нет времени, но я обязательно свяжусь с вами позже, чтобы обсудить это более подробно.
" Что же делает фильтр? Он помнит только одно значение x1 и его дисперсию σ2. Когда поступает новое значение x2 со своим отклонением r2,"
r2 - r в квадрате ето же дисперсия. или я ошибаюсь?
Спасибо
Не ошибаетесь, все правильно. Везде используется дисперсия. "... поступает новое значение x2 со своим отклонением r (в квадрате)". Квадрат отклонения — это дисперсия и есть.
Возможно стоит переписать этот абзац, чтобы не допускать двоякой трактовки.
Добрый день,
искал вчера реализацию калмановского фильтра на питоне, взял вашу, примерил - работает. Теперь хочу разобраться почему :)
Правильно ли я понимаю, что в случае фильтрации функции y(x), на вход широты нужно подавать x, а на вход долготы - y? При фильтрации лишь одной переменной результаты много хуже (кстати, подскажите, пожалуйста, почему выгоднее фильтровать именно пару значений [x;y]?). С реализацией по скользящему среднему, конечно, не идет ни в какое сравнение - калман держит сглаженной как саму функцию, так и ее производную.
По иксу — долгота, широта — по игреку.
На питоне я просто проверял идею. Для промышленного применения я потом сделал матричную реализацию на Objective-C и там я фильтрую не только пару координат X, Y, но и вектор скорости по X, Y, так что в матричных вычисления участвует матрица 4*1, остальные матрицы 4*4.
Я собираюсь выложить еще один документ по ФК, где собрано все, что я знаю о влиянии каждой матрицы и ее роли в вычислениях и реализация с матрицами 4*4 (позиция и скорость).
Здравствуйте. Спасибо за статью. У меня вопрос есть три значения широта, долгота, высота. Они подаются от датчиков с ошибкой. Фильтр должен их фильтровать но при этом я не вывожу из другие значения тогда получается, что здесь матричная реализация не нужна ведь я их фильтрую по отдельности и управляющей компоненты нет.
Я пробовал фильтровать позицию двумя способами. Каждую координату по отдельности (скалярно) и как вектор (матричным методом). Надо сказать, что матричный метод дает лучший результат.
Добрый день, спасибо за статью.
Одного не могу понять.
Когда приходит новое значение с показания датчика, откуда узнать какого его отклонение (т.е. дисперсия) ? Я могу только предположить, что эти параметры задаются самостоятельно на основе технических характеристик датчиков. Т.е., например, температура с точностью до одной десятой градуса. Значит ли это, что у такого датчика дисперсия равна 0.1? Или как, например, определить дисперсию показания вайфай локатора? Вряд ли там какая-то точность указана - слишком уж все зависит от взаимного располодения точек доступа вайфай да и силы сигнала.
Ваш комментарий: