Полевые испытания фильтра Калмана

Я переписал фильтр Калмана для матриц размером 4x4, чтобы его можно было использовать для уточнения долготы и широты. Кроме того, в таком виде он сможет вычислять и скорость движения более точно. После отладки фильтра на Питоне, я переписал его на Objective-C++, выбрав матричную библиотеку GLM.

Матричный Калман показал себя очень хорошо. Вечером с Кеней ходили вокруг квартала 85% по вайфаю — все равно показывает фактически, как идешь, но с отставанием метров на 30-40. При этом измеряемый локейшен прыгает вокруг тебя чудовищно, от здания к зданию, а трек все равно выглядит, как по точному GPS-сигналу.

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

31.05.2013 • Метки: gps, iOS, Калман, математика, фильтр • Рубрики: Разработка ПО • Комментариев: 2

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

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

Рудольф Калман. Что бы мы без него делали. А ведь когда он придумал свой фильтр в 1960 году, то встретился с таким скептицизмом, что был вынужден опубликовать первые работы о нем в журнале, связанном с механикой, хотя сам занимался электротехникой.

Применений множество. Представьте датчик топлива в баке автомобиля. Он же болтается там внутри, если просто записать его показания, а потом попытаться проанализировать, получится, что в какой-то момент в баке было 30 л, через секунду 50, а потом 27. А это просто волны внутри гуляют. Сколько же в этот момент было в баке? ФК может дать более вероятный ответ.

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

Материалов по ФК много, мне показались лучшими эти документы: раз и два. Но ими лучше пользоваться потом, когда суть работы фильтра уже станет понятной.

Если очень кратко, то суть в том, что большинство явлений подчиняются Гауссовскому распределению вероятностей.

Например, мы думаем, что находимся в точке x. Но мы ведь можем ошибаться, потому, например, что отмеряли расстояние до x каким-нибудь прибором, а он, конечно, врет. Поэтому мы считаем, что находимся в точке x с какой-то вероятностью p. А может, мы сейчас в точке x', но вероятность этого меньше — p'. Ну такое вот распределение вероятностей.

Наиболее вероятный x посредине — это μ, математическое ожидание. Ширина этой "шляпы" зависит от σ² — это дисперсия, квадрат стандартного отклонения. Чем больше отклонение, тем шляпа шире и неопределенность больше.

У этой кривой такое уравнение:

p = \frac{1}{σ \sqrt{2 π}} e^{- \frac{{(x - μ)}^{2}}{2 σ^{2}}}

(1)

Но его бояться не надо, т.к. нам надо искать середину, самый вероятный x. А в середине он равен μ, в числителе степени разница между x и μ дает 0, это многое потом упрощает.

Что же делает фильтр? Он помнит только одно значение x1 и его дисперсию σ². Когда поступает новое значение x2 со своим отклонением r², он умножает их (по правилам распределения, см. формулу 2) друг на друга и в результате будет новое значение x3 с погрешностью меньшей, чем у двух предыдущих!

Например, текущее значение фильтра — синее распределение. Поступает новое значение, со своим отклонением (красное). Фильтр перемножает их по правилам распределения (см. формулу 2) и получает новое распределение (зеленое). Оно гораздо точнее двух изначальных, т.к. его дисперсия меньше (см. формулу 3) и становится текущим значением фильтра.

Умножение распределений делается так:

x_{3} = \frac{r^{2} x_{1} + σ^{2} x_{2}}{r^{2} + σ^{2}}

(2)

Дисперсия тоже уточняется:

σ^{2} ’ = \frac{1}{\frac{1}{r^{2}} + \frac{1}{σ^{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()

А это скалярная, упрощенная реализация. Я сразу попробовал ее на раздельном сглаживании широты и долготы. Тренировался на треках, которые люди загружают на OpenStreetMap. Вот она:

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, зеленая — сглаженный путь):

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

28.05.2013 • Метки: gps, iOS, Калман, фильтр • Рубрики: Разработка ПО • Комментариев: 11

Эмодзи

В iOS 5.1 поддерживаются эмодзи — такие картинки-буквы, типа Windings, только цветные, клевые и покрывают больше понятий.

Народ их использует для смайликов и проч.:

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

Выглядит потом этот текст так:

13.06.2012 • Метки: emoji, iOS, эмодзи • Рубрики: Разработка ПО • Комментариев: 1

Apple снимает ограничения

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

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

Ну ничего, придумают что-нибудь. Пипл в демагогии всё равно слабо разбирается — схавает как-нибудь.

А мне подачки эпплов не нужны. Сняли они ограничения, понимаешь. Я уже скилы по Objective-C себе прокачал, можете свои ограничения и снятия засунуть себе обратно в лицензионное соглашение.

P.S. iOS отличная платформа для разработки, кстати.

11.09.2010 • Метки: apple, flash, iOS • Рубрики: Разработка ПО • Комментировать

Абсолютно неофициальная страница Павла Малинникова

Малинников.ru

Полевые испытания фильтра Калмана

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

Эмодзи

Apple снимает ограничения

Рубрики

Свежие записи

Свежие комментарии

Архивы

Реклама

Абсолютно неофициальная страница Павла Малинникова

Малинников.ru

Полевые испытания фильтра Калмана

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

Эмодзи

Apple снимает ограничения

Рубрики

Ключевые слова

Свежие записи

Свежие комментарии

Архивы

Реклама