E (о₁Ч о₂) = E о₁ЧE о₂ (3.5)

Доказательство:

Например, иметь голубые глаза и окончить школу с золотой медалью - независимые случайные величины. Если голубоглазых, скажем 20%=0.2, а золотых медалистов 5%=0.05, то голубоглазых медалистов 0.2Ч0.005=0.01=1%. Этот пример подсказывает нам, что если случайные величины о₁ и о₂ заданы своими плотностями вероятности с₁ (x) и с₂ (y), то независимость этих величин выражается в том, что плотность вероятности с (x,y) (первая величина выпала x, а вторая y) находится по формуле:

с (x,y) = с₁ (x) Чс₂ (y) (3.6)

Из этого сразу же следует, что:

Доказательство проведено для случайных величин, которые имеют непрерывный спектр значений и заданы своей плотностью вероятности. В других случаях идея доказательства аналогичная. [8]

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

Постановка задачи

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

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

Ри. 19 Движущийся объект

Тогда координата объекта будет изменяться по закону:

x_k+1 = x_k + v_kdt (3.7)

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

x_k+1 = x_k + v_kdt +о_k (3.8)

У нас есть установленный на объекте GPS сенсор, который пытается мерить истинную координату x_k объекта, и, конечно же, не может ее померить точно, а мерит с ошибкой з_л, которая является тоже случайной величиной.

В итоге с сенсора мы получаем ошибочные данные:

z_k = x_k + з_k (3.9)

Задача состоит в том, что, зная неверные показания сенсора z_k, найти хорошее приближение для истинной координаты объекта x_k. Это хорошее приближение мы будем обозначать как .

В формулировке же общей задачи, за координату x_k может отвечать все что угодно (температура, влажность и т.д.), а член, отвечающий за контроль системы извне, мы обозначим за u_k (в примере c объектом u_k = v_kdt).

Уравнения для координаты и показания сенсора будут выглядеть так:

x_k+1 = x_k + u_k +о_k

z_k = x_k + з_k (3.10)

И так, что нам известно:

· u_k - это известная величина, которая контролирует эволюцию системы. Мы ее знаем из построенной нами физической модели.

· Ошибка модели о_k и ошибка сенсора з_k - случайные величины. И их законы распределения не зависят от времени (от номера итерации k).

· Средние значения ошибок равны нулю: Eо_k = Eз_k = 0.

· Сам закон распределения случайных величин может быть нам и не известен, но известны их дисперсии и . Заметим, что дисперсии не зависят от k, потому что законы распределения не зависят от него.

· Предполагается, что все случайные ошибки независимы друг от друга: какая ошибка будет в момент времени k совершенно не зависит от ошибки в другой момент времени k'.

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

Алгоритм Калмана

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

x_k+1 = x_k + u_k +о_k,

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

Идея Калмана состоит в том, что чтобы получить наилучшее приближение к истинной координате x_k+1, мы должны выбрать золотую середину между показанием z_k+1 неточного сенсора и - нашим предсказанием того, что мы ожидали от него увидеть. Показанию сенсора мы дадим вес K, а на предсказанное значение останется вес (1-K):

(3.11)

Коэффициент K называют коэффициентом Калмана. Он зависит от шага итерации, поэтому правильнее было бы писать K_k+1, но пока, чтобы не загромождать формулы расчетах, мы будем опускать его индекс.

Мы должны выбрать коэффициент Калмана K таким, чтобы получившееся оптимальное значение координаты было бы наиболее близко к истинной координате x_k+1. К примеру, если мы знаем, что наш сенсор очень точный, то мы будем больше доверять его показанию и дадим значению z_k+1 больше весу (K близко единице). Если же сенсор, наоборот, совсем не точный, тогда больше будем ориентироваться на теоретически предсказанное значение .

В общем случае, чтобы найти точное значение коэффициента Калмана K, нужно просто минимизировать ошибку:

(3.12)

Используем уравнения (3.10), чтобы переписать выражение для ошибки:

(3.13)

Доказательство:

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

На самом деле не существует однозначного подхода к определению того, что означает, что ошибка минимальна.

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

(3.14)

Распишем последнее выражение:

(3.15)

Доказательство:

Из того что все случайные величины, входящие в выражение (5.13) для e_k+1, независимы, следует, что все "перекрестные" члены равны нулю: E (о_kз_k+1) = E (e_kо_k) = E (e_kз_k+1) =0.

Так же Eз_k+1=Eо_k=0, тогда формула для дисперсий выглядит намного проще:

(3.16)

Выражение (3.15) принимает минимальное значение, когда (приравниваем производную к нулю):

(3.17)

Подставляем в выражение (3.15) для среднеквадратичной ошибки минимизирующее ее значение коэффициента Калмана (5.17) . Получаем:

(3.18)

Наша задача решена. Мы получили итерационную формулу (3.18), для вычисления коэффициента Калмана. [9] Практическая реализация в приложении А.

Рис. 20 Данные обработанные фильтром Калмана

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

Рис. 21 Зависимость коэффициента Калмана от шага итерации.

Основная идея фильтра Калмана состоит в том, что надо найти коэффициент K такой, чтобы отфильтрованное значение:

(3.19)

в среднем меньше всего отличалось бы от реального значения координаты x_k+1. Мы видим, что отфильтрованное значение есть линейная функция от показания сенсора z_k+1 и предыдущего отфильтрованного значения . А предыдущее отфильтрованное значение является, в свою очередь, линейной функцией от показания сенсора z_k и предпредыдущего отфильтрованного значения . И так далее, пока цепь полностью не развернется. То есть отфильтрованное значение зависит от всех предыдущих показаний сенсора линейно:

(3.20)

Поэтому фильтр Калмана называют линейным фильтром.

3.2 Практическая реализация фильтра Калмана

Для практической реализации фильтра Калмана, необходимо получить данные со спутников. Для этих целей, мы будем использовать модуль L76, который передает данные с GPS и ГЛОНАСС спутников и модуль LEA-5, который передает данные с GPS спутников.

С помощью модуля L76 и LEA-5 в помещении были получены следующие данные (рис.22-23).

Рис. 22 Пакет данных, полученных с помощь модуля L76

Рис. 23 Пакет данных, полученных с помощью модуля LEA-5

Данные получены со спутника в формате NMEA. Это формат передачи сообщений между корабельными приборами. Он включает в себя систему сообщений для обмена информацией между навигационными GPS приёмниками и потребителями навигационной информации. Все команды и сообщения передаются в текстовом ASCII виде, относящиеся к GPS приёмникам начинаются с $GP, в конце строки сообщения должны быть символы <CR><LF>. В последнем поле сообщения может быть указана контрольная сумма текущего сообщения, начинающаяся с разделителя *. Контрольная сумма 8 - ми битная (исключающая ИЛИ) всех символов сообщения, включая пробелы, расположенных между разделителями $ и *, не включая последних. Шестнадцатеричный результат переводится в два ASCII символа (0-9, A-F).

Содержание некоторых сообщений протокола NMEA

$GPGGA Сообщение содержит GPS данные о местоположении, времени местоопределения, качестве данных, количестве использованных спутников, HDOP (Фактор Ухудшения Точности Плановых Координат), информацию о дифференциальных поправках и их возраст.

$GPGLL Сообщение содержит GPS-данные о географической широте, долготе и времени определения координат.

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

$GPGSV В сообщении указывается количество видимых спутников, их номера, возвышение, азимут, и значение отношения сигнал/шум для каждого из них.

$GPRMC Сообщение RMC содержит данные о времени, местоположении, курсе и скорости, передаваемые навигационным GPS приёмником. Контрольная сумма обязательна для этого сообщения, интервалы передачи не должны превышать 2 секунды. Все поля данных должны быть подготовлены, пока ещё нет самих данных. Недействительные поля могут быть использованы, пока данные временно не готовы.

$GPVTG Сообщение VTG передает текущее истинное направление курса (COG) и скорость относительно земли (SOG).

$GPZDA Сообщение ZDA содержит информацию о времени по UTC, календарный день, месяц, год и локальный часовой пояс.

GGA - GPS Данные о местоположении

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15

$GPGGA, hhmmss.ss, 1111.11, a, yyyyy.yy, a, x, xx, x.x, xxx, M, x.x, M, x.x, xxxx*hh

1. Гринвичское время на момент определения местоположения.

2. Географическая широта местоположения.

3. Север/Юг (N/S).

4. Географическая долгота местоположения.

5. Запад/Восток (E/W).

6. Индикатор качества GPS сигнала:

0 = Определение местоположения не возможно или не верно;

1 = GPS режим обычной точности, возможно определение местоположения;

2 = Дифференциальный GPS режим, точность обычная, возможно определение местоположения;

3 = GPS режим прецизионной точности, возможно определение местоположения.

7. Количество используемых спутников (00-12, может отличаться от числа видимых).

8. Фактор Ухудшения Точности Плановых Координат (HDOP).

9. Высота антенны приёмника над/ниже уровня моря.

10. Единица измерения высоты расположения антенны, метры.

11. Геоидальное различие - различие между земным эллипсоидом WGS-84 и уровнем моря(геоидом),

”-” = уровень моря ниже эллипсоида.

12. Единица измерения различия, метры.

13. Возраст Дифференциальных данных GPS - Время в секундах с момента последнего SC104 типа 1 или 9 обновления, заполнено нулями, если дифференциальный режим не используется.

14. Индификатор станции, передающей дифференциальные поправки, ID, 0000-1023.

15. Контрольная сумма строки.

Пример сообщения:

$GPGGA,004241.47,5532.8492,N,03729.0987,E,1,04,2.0 ,-0015,M,,,,*31

GLL - географическое положение -- Широта/Долгота

1 2 3 4 5 6 7

$GPGLL, 1111.11, a, yyyyy.yy, a, hhmmss.ss, A*hh < CR><LF>

1. Географическая широта местоположения.

2. Север/Юг (N/S).

3. Географическая долгота местоположения.

4. Запад/Восток (E/W).

5. Гринвичское время на момент определения местоположения.

6. Статус A = данные верны

V = данные не верны

7. Контрольная сумма строки.

Пример сообщения:

$GPGLL,5532.8492,N,03729.0987,E,004241.469,A*33

GSA - GPS факторы точности и активные спутники

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

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18

$GPGSA, a, x, xx, xx, xx, xx, xx, xx, xx, xx, xx, xx, xx, xx, x.x, x.x, x.x*hh <CR><LF>

1. Режим: M = Ручной, принудительно включен 2D или 3D режим;

A = Автоматический, разрешено автомат. выбирать 2D/3D.

2. Режим: 1 = Местоположение не определено, 2 = 2D, 3 = 3D

3-14. PRN номера спутников, использованных при решении задачи местоопределения (нули для неиспользованных).

15. Фактор PDOP.

16. Фактор HDOP.

17. Фактор VDOP.

18. Контрольная сумма строки.

Пример сообщения:

$GPGSA,A,3,01,02,03,04,,,,,,,,,2.0,2.0,2.0*34

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

1 2 3 4 5 6 7 8 15 16 17 18 19 20 $GPGSV, x, x, xx, xx, xx, xxx, xx..., xx, xx, xxx, xx*hh <CR><LF> 1. Полное число сообщений, от 1 до 9.

2. Номер сообщения, от 1 до 9.

3. Полное число видимых спутников.

4. PRN номер спутника.

5. Высота, градусы, (90° - максимум).

6. Азимут истинный, градусы, от 000° до 359°.

7. Отношение сигнал/шум от 00 до 99 дБ, ноль - когда нет сигнала.

8-11. Тоже, что в 4-7 для второго спутника.

12-15. Тоже, что в 4-7 для третьего спутника.

16-19. Тоже, что в 4-7 для четвертого спутника.

20. Контрольная сумма строки.

Пример сообщения:

$GPGSV,3,1,12,02,86,172,,09,62,237,,22,39,109,,27,37,301,*7A $GPGSV,3,2,12,17,28,050,,29,21,314,,26,18,246,,08,10,153,*7F $GPGSV,3,3,12,07,08,231,,10,08,043,,04,06,170,,30, 00,281,*77 RMC - pекомендуемый минимум GPS / навигационных данных 1 2 3 4 5 6 7 8 9 10 11 12 $GPRMC, Hhmmss. ss, A, 1111.11, A, yyyyy. yy, a, x. x, x. x, ddmmyy, x. x, A *hh <CR><LF> 1. Время фиксации местоположения UTC 2. Состояние: А = действительный, V = предупреждение навигационного приёмника 3,4. Географическая широта местоположения, Север/Юг 5,6. Географическая долгота местоположения, Запад/Восток (E/W)

7. Скорость над поверхностью (SOG) в узлах

8. Истинное направление курса в градусах

9. Дата: dd/mm/yy

10. Магнитное склонение в градусах

11. Запад/Восток (E/W)

12. Контрольная сумма строки (обязательно) Пример сообщения:

$GPRMC,113650.0,A,5548.607,N,03739.387,E,000.01,25 5.6,210403,08.7,E*69 VTG - истинное направление курса и скорость относительно земли 1 2 3 4 5 $GPVTG, x. x, T x. x, M x. x, N x. x, K *hh <CR><LF> 1. Направление курса в градусах, T 2. Магнитное склонение в градусах, М 3. Скорость над поверхностью (SOG) в узлах, N = узлы 4. Скорость над поверхностью (SOG) в км/ч, К = км/ч 5. hh Контрольная сумма строки (обязательно) Пример сообщения:

$GPVTG,217.5,T, 208.8,M,000.00,N,000.01,K*4C ZDA - время и дата 1 2 3 4 5 6 7 $GPZDA, hhmmss. s, xx, xx, xxxx, xx, xx *hh <CR><LF > 1. Время UTC 2. День (01до 31) 3. Месяц (01 to 12) 4. Год 5. Часовой пояс, смещение от GMT, от 00 до ± 13 часов 6. Часовой пояс, смещение от GMT, минуты 7. hh Контрольная сумма строки Пример сообщения:

$GPZDA,172809,12,07, 1996,00,00*45

Обработка данных

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

Полученные данные необходимо перевести из формата DDD MM. MMM (D-градусы, M - минуты) в формат DDD. DDDDD, к примеру:

W 087° 38.185 38.185 ч 60 = 0.636417 тогда угол будет равен: 87.636417°

Для Севера (N) и Востока (E) градусы принимают положительное значение, а для Юга (S) и Запада (W) - отрицательные.

Запад t" имеет отрицательный угол: - 87.636417°

Для отображения координат на плоскости, необходимо было перевести их из сферических полярных в декартовые:

a=6378137 - экваториальный радиус

e = 8.181919084Е-2 - эксцентриситет

lat = геодезическая широта (рад.)

lon = долгота (рад.)

alt = высота над WGS84 эллипсоида (м.)

(3.21)

x= (N+alt) Чcos (lat) Чcos (lon)

y= (N+alt) Чcos (lat) Чsin (lon) (3.22)

z= ( (1-e²) ЧN+alt) Чsin (lat)

Рис. 24 Полученные и отфильтрованные данные с модуля L76

Рис. 25 Полученные и отфильтрованные данные с модуля LEA-5

На рис. 24 и 25 хорошо видно, что графики с отфильтрованными данными более гладкие и имеет меньшее количество пиков. Это говорит о том, что соотношение сигнал/шум уменьшилось и мы получили более точные значения.

Для оценки данных необходимо посчитать среднее квадратичное отклонение:

(3.23)

где:

(3.24)

При сравнении 2х графиком, мы видим, что данные полученные с помощью модуля L70 более точные, чем данные полученные с модуля LEA-5. Это связано с тем, что модуль L76 использует совмещенную систему позиционирования GPS/ГЛОНАСС, а так же с недостатками системы GPS, данные с которой получает модуль LEA-5.

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

Невысокое наклонение орбит GPS (примерно 55) серьёзно ухудшает точность в приполярных районах Земли, так как спутники GPS поднимаются над горизонтом невысоко. Существенной особенностью GPS считается полная зависимость условий получения сигнала от Министерства обороны США.

Совмещенный приемник спутниковых радионавигационных систем GPS/ГЛОНАСС отвечает всем рыночным требованиям к приемникам GPS и добавляет новые качества - надежность и точность местоопределений в сложных условиях, привносимые за счет увеличения числа принимаемых сигналов.

Современные показатели точности системы GPS для невоенных пользователей имеют погрешность по долготе и широте 2.00-8.76 метров при использовании навигатором 6-11 спутников. У ГЛОНАСС погрешность по долготе и широте составляет 4.46-7.38 метров при использовании 7-8 спутников. При этом, при одновременном использовании навигатором двух систем погрешность снижается до 1.5-3 метров. В бытовом применении важнее даже не точность, а надежность приема. Будь то автомобильный навигатор или туристический - главное, чтобы связь со спутниками не терялась и вы всегда могли однозначно определить, где вы находитесь в настоящий момент времени. В условиях неуверенного приема (на узких улицах между высоких домов, или на дороге, окруженной высоким лесом) обе системы будут подстраховывать друг друга. И там, где навигаторы, использующие только GPS или только ГЛОНАСС, потеряли бы спутники, устройство, параллельно использующее обе системы, показало хорошие результаты.

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

Заключение

В ходе работы, данные полученные со спутника были обработаны фильтром Калмана. На рисунках мы видим, что графики с отфильтрованными данными более гладкие и имеет меньшее количество пиков. Это говорит о том, что соотношение сигнал/шум уменьшилось и мы получили более точные значения. Так же уменьшилось среднее квадратичное отклонение.

В результаты опыта было показано, что применения приемника, в котором используется второй СРНС дает значительное улучшение качества навигации, так как в нем задействовано большее количество спутников.

Литература

Д.Ю. Першин, А.С. Щербак, Определение высокой точности для одночастотных приемников спутниковой навигации с использованием инерциальных датчиков // Институт систем информатики СО РАН

Свободная энциклопедия http://ru. wikipedia.org/

Международный сервис GNSS, http://igscb. jpl. nasa.gov/

Открытая библиотека GPSTk, http://www.gpstk.org/

Alfred Leick, GPS Satellite Surveying // John Wiley & Sons, Inc. 2004

А.С. Щебраков, Д-Ю. Першин, Определение местоположения высокой точности для одночастотных приёмников ГЛОНАСС/GPS // Новосибирский государственный университет, МНСК-2009

7. Навигационное оборудование http://www.gsm-rainbow.ru/

8. Информационный портал http://habrahabr.ru/

9. Mohinder S. Grewal, Angus P. Andrews, Kalman Filtering: Theory and Practice, New York

Приложения

Приложение А

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

clear all;

N=100 % number of samples

a=0.1 % acceleration

sigmaPsi=1

sigmaEta=50;

k=1: N

x=k

x (1) =0

z (1) =x (1) +normrnd (0,sigmaEta);

for t=1: (N-1)

x (t+1) =x (t) +a*t+normrnd (0,sigmaPsi);

z (t+1) =x (t+1) +normrnd (0,sigmaEta);

end;

%kalman filter

xOpt (1) =z (1);

eOpt (1) =sigmaEta; % eOpt (t) is a square root of the error dispersion (variance). It's not a random variable.

for t=1: (N-1)

eOpt (t+1) =sqrt ( (sigmaEta^2) * (eOpt (t) ^2+sigmaPsi^2) / (sigmaEta^2+eOpt (t) ^2+sigmaPsi^2))

K (t+1) = (eOpt (t+1)) ^2/sigmaEta^2

xOpt (t+1) = (xOpt (t) +a*t) * (1-K (t+1)) +K (t+1) *z (t+1)

end;

plot (k,xOpt,k,z,k,x)

Приложение В

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

MathHelper. cs

using System;

namespace _KalmanFilterApp

{

public static class MathHelper

{

public static double ToRadian (double degreeAngle)

{

return degreeAngle * Math. PI / 180;

}

public static Coord3D ToCartesian (double h, double lat, double lon)

{

const double a = 6378140.0000;

const double f = 6356755.2881575287;

const double eqr = (a * a - f * f) / (a * a);

var b = ToRadian (lat);

var l = ToRadian (lon);

var sinB = Math. Sin (b);

var cosB = Math. Cos (b);

var sinL = Math. Sin (l);

var cosL = Math. Cos (l);

var n = a / Math. Sqrt (1.0 - eqr * sinB * sinB);

var coord = new Coord3D { X = (n + h) * cosB * cosL, Y = (n + h) * cosB * sinL, Z = (n * (1.0 - eqr) + h) * sinB };

return coord;

}

public static double ToDecimalAngle (double angle, double minutes, Direction direction)

{

var retAngle = angle + minutes / 60;

if (direction. HasFlag (Direction. W) || direction. HasFlag (Direction. S))

{

return - retAngle;

}

return retAngle;

}

}

}

DataReaderHelper. cs

using System;

using System. Collections. Generic;

using System. Globalization;

using System. IO;

using System. Linq;

namespace _KalmanFilterApp

{

public static class DataReaderHelper

{

public static IList<Coord3D> Read (Stream stream)

{

try

{

using (var reader = new StreamReader (stream))

{

var coords = new List<Coord3D> ();

string data = reader. ReadToEnd ();

var separators = new [] { '\r', '\n' };

var lines = data. Split (separators, StringSplitOptions. RemoveEmptyEntries)

. Where (dt =>! string. IsNullOrEmpty (dt) && dt. Contains ("$GPGGA")). ToArray ();

if (lines. Length == 0)

{

throw new Exception ("Data were not found");

}

for (int i = 0; i < lines. Length; i++)

{

var parts = lines [i]. Split (",". ToArray (), StringSplitOptions. RemoveEmptyEntries);

double result;

if (! double. TryParse (parts [2], NumberStyles. AllowDecimalPoint, CultureInfo. InvariantCulture, out result)

||! double. TryParse (parts [4], NumberStyles. AllowDecimalPoint, CultureInfo. InvariantCulture, out result))

{

Logger. Warning (string. Format ("Line {0} is wrong {1}", i + 1, lines [i]));

continue;

}

Direction latitudeDirect, longitudeDirect;

if (! Enum. TryParse (parts [3], out latitudeDirect)

||! Enum. TryParse (parts [5], out longitudeDirect))

{

Logger. Warning (string. Format ("Line {0} is wrong {1}", i + 1, lines [i]));

continue;

}

var geoCoordY = parts [2]. TrimStart ('0');

var geoCoordX = parts [4]. TrimStart ('0');

var latitudeAngle = MathHelper. ToDecimalAngle (

GetDegreeAngle (geoCoordY),

GetMinutes (geoCoordY),

latitudeDirect);

var longitudeAngle = MathHelper. ToDecimalAngle (

GetDegreeAngle (geoCoordX),

GetMinutes (geoCoordX),

longitudeDirect);

double h = double. Parse (parts [9], CultureInfo. InvariantCulture);

coords. Add (MathHelper. ToCartesian (h, latitudeAngle, longitudeAngle));

}

return coords;

}

}

catch (Exception exc)

{

Logger. Error (exc);

throw;

}

}

private static double GetDegreeAngle (string geoCoord)

{

var part = geoCoord. Split (".". ToCharArray (), StringSplitOptions. RemoveEmptyEntries) [0];

var strDegreeAngle = part. Substring (0, part. Length - 2);

var degreeAngle = double. Parse (strDegreeAngle, CultureInfo. InvariantCulture);

return degreeAngle;

}

private static double GetMinutes (string geoCoord)

{

var index = geoCoord. IndexOf ('. ') - 2;

var minutes = double. Parse (geoCoord. Substring (index), CultureInfo. InvariantCulture);

return minutes;

}

}

}

KalmanFiltreModel. cs

using System;

using System.componentModel;

using System. Linq;

using Emgu. CV;

namespace _KalmanFilterApp

{

public class KalmanFilterModel: IKalmanFilterModel, INotifyPropertyChanged

{

private double _sigmaPsi = Constants. SigmaPsi;

private double _sigmaEta = Constants. SigmaEta;

public Coord3D [] GetFilteredData (Coord3D [] points)

{

int count = points. Length;

if (count == 0)

{

return new Coord3D [0];

}

var coordsX = GetFilteredCoords (points. Select (pt => pt. X). ToArray ());

var coordsY = GetFilteredCoords (points. Select (pt => pt. Y). ToArray ());

var coordsZ = GetFilteredCoords (points. Select (pt => pt. Z). ToArray ());

var coords = new Coord3D [count];

for (int i = 0; i < count; i++)

{

coords [i]. X = coordsX [i];

coords [i]. Y = coordsY [i];

coords [i]. Z = coordsZ [i];

}

return coords;

}

public double SigmaPsi

{

get

{

return _sigmaPsi;

}

set

{

_sigmaPsi = value;

OnPropertyChanged ("SigmaPsi");

}

}

public double SigmaEta

{

get

{

return _sigmaEta;

}

set

{

_sigmaEta = value;

OnPropertyChanged ("SigmaEta");

}

}

private double [] GetFilteredCoords (double [] coords)

{

var count = coords. Length;

var xOpt = new double [count];

xOpt [0] = coords [0];

var eOpt = new double [count];

eOpt [0] = SigmaEta;

for (int i = 0; i < count - 1; i++)

{

eOpt [i + 1] =

Math. Sqrt (Math. Pow (SigmaEta,

2) *

(Math. Pow (eOpt [i],

2) +

Math. Pow (SigmaPsi,

2)) /

(Math. Pow (SigmaEta,

2) + Math. Pow (eOpt [i],

2) + Math. Pow (SigmaPsi,

2)));

double k = Math. Pow (eOpt [i + 1],

2) / Math. Pow (SigmaEta,

2);

xOpt [i + 1] = xOpt [i] * (1 - k) + k * coords [i + 1];

}

return xOpt;

}

public event PropertyChangedEventHandler PropertyChanged;

protected virtual void OnPropertyChanged (string propertyName)

{

if (PropertyChanged! = null)

{

PropertyChanged (this, new PropertyChangedEventArgs (propertyName));

}

}

}

}

Размещено на Allbest.ru