GPS с гироскопом и одометром.

OverQuantum · August 13, 2013, 7:29pm

А можно мне (1) комплект сырых данных? Лучше если в текстообразном виде.
Ну и нужна оценка точности используемого гироскопа и одометра (можно ссылкой).
Может я Калмана умучаю или сам что-нить придумаю.

Идея такая - у нас есть 3 источника данных, каждый со своим видом искажений

GPS - выдаёт точные кординаты, но со случайным шумом и плавающим искажением.
При долгой экспозиции на одном месте получается “ромашка” с центром в точных координатах.
Гироскоп - выдаёт точный поворот, но направление медленно уплывает со времением (ошибка накапливается)
Одометр - выдаёт точное пройденное расстоение, но с некоторой погрешностью
Грубо говоря, из каждого источника надо брать самое точное.

SviMik · August 13, 2013, 7:44pm

Не совсем верно - часто он на поворотах не доворачивает, или наоборот, может переборщить.

Интересует комплект с хорошим GPS треком (на крыше), или с плохим (в салоне)?
Также уточни, нужен ли пример с ездой внутри здания, или чисто наружный вариант?

OverQuantum · August 13, 2013, 7:53pm

Понял.
Я с гироскопами и одометрами особо не работал - видел кто-нить модель их погрешности?
Гироскоп - пропорционально скорости поворота?
Одометр - пропорционально скорости? Больше ошибается при поворотах?

Лучше пока по улице, где есть GPS, который не шибко ровный.
Этот вариант http://svimik.com/gpsvsimu.png или этот http://svimik.com/speedsensor10.jpg или что-то такое же - где GPS заносит с дороги

usm78-gis · August 13, 2013, 8:26pm

Сырые данные с CAN шины у меня есть, но они
(a) без синхронного GPS трека с сырыми GPS данными
(b) у них нет временного миллисекундного маркера для сообщений (внутренние часы сами имеют
дрифт и переполняются, т.к. это 16 битный счетчик).
Тут надо будет еще подумать, как
генерировать “абсолютное” точное время.
Записывая эти данные я больше интересовался насколько хорошо работает CAN адаптер (какие CAN сообщения),
и какие цифры записываются, если ехать быстрее 200 kм/ч.
Выглядит это так ( t4A0+8+msg, где 8 байт = 4колеса по 2 байта)


t4A080000000000000000
t4A080000000000000000
t4A080000000000000000
t4A087000000000000000
t4A087000000000000000
t4A087000000000000000
t4A087000000000000000
t4A087000000000000000
t4A087000000000000000
t4A089600000000000000
t4A089600000000000000
t4A089600780000000000
t4A089600780000000000
t4A08A000780088000000
t4A08A000780088000000
t4A08A000C80088000000
t4A08A000C80088009000
t4A08CA00C800D6009000
t4A08CA00EC00D6009000
t4A08CA00EC00D600F800
t4A08EA001001EC00F800
t4A08EA001001EC000E01
t4A080C01100110010E01
t4A080C01280110010E01
t4A080C01280110012A01
t4A0828013E012A012A01
t4A0828013E012A013E01

16битный маркер времени добавит 2 байта в конце.
Если предположить, что такт = 20 ms, то можно попробовать проинтегрировать траекторию.
Лог поездки ~ 100 км (+100 km обратно).

“оценка точности используемого гироскопа” есть в даташите. Модель для Калмана есть
в статье u-blox, там только опущены важные детали, которые придется додумывать самим:
модель температурного дрифта гироскопа, какие переменные попадают в вектор “u” и т.д.

Для борьбы с “уплыванием” используется акселерометр: его абсолютные ошибки велики, но “в среднем”
он не плывет.

OverQuantum · August 13, 2013, 8:33pm

Покумекал, надумал такое - минимизировать “энтропию” отклонения измерений от вычисляемой траектории.
Каждые измерения - это вычисляемые данные + погрешность. Для погрешности можно оценить её вероятность (грубо - чем больше погрешность, тем меньше вероятность, и больше “энтропия” этой погрешности, но не строго - для GPS отклонение в 2-3 метра почти также вероятно как и нулевое, грубо говоря).
Сначала берём данные гироскопа и одометра, считаем по ним координаты новой точки и “энтропию” для GPS сигнала. Дальше подбираем координаты (методом деления пополам на плоскости или ещё как-нить) для минимизации общей “энтропии” измерений.
Думаю, если правильно 1) задать функцию оценки “энтропии” для каждого типа сигнала и 2) выбрать алгоритм поиска координат - будет неплохо сглаживать (ужа с ежом), может быть даже в реальном времени.

usm78-gis · August 13, 2013, 8:43pm

Это делает rtklib (хотя есть и другие opensource варианты). Он также использует Калмана для не “single point” решений:

Документация могла быть лучше, но она и так неплохая для тех кто хочет понять что к чему.

OverQuantum · August 13, 2013, 8:45pm

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

P.S. Например, високосная секунда, ловится в логе GPS.

chnav · August 13, 2013, 9:08pm

Я понял, у всех форумчан просто разная терминология. Вы прямо-таки описали Метод наименьших квадратов.
Погрешность в вашей фразе это невязка (residual).
“Энтропия” - видимо это гауссовское (нормальное) распределение.
“Модель” - ожидаемое стандартное отклонение, оно же estimated SD, оно же “точность устройства” в сообщениях некоторых форумчан, оно же “вес измерения” если взять 1/SD^2.

Можно даже пренебречь Кальманом, т.е.

Приводим все данные к единому моменту времени (годится линейная интерполяция) - тут см. мой предыдущий пост про синхронизацию времени. Требуется калибровка latency (задержка поступления данных) каждого устройства.
Уравнивание по методу наименьших квадратов. Тут либо написать свою математику, либо воспользоваться готовой библиотекой. Если МНК непонятен - думаю совместно можем объяснить всё и вся.

Про синхронизации времени и применяемые для этого решения отписал тут (достаточно ограничиться пп. 1 и 2).

Калибровка latency устройств для бытовой точности довольно проста - проехаться встречными курсами по сложной траектории.

PS: извините если повторяю какие-то известные вещи, лучше перестраховаться и назвать термины своими именами, так проще для гугления.

usm78-gis · August 13, 2013, 9:11pm

Сыпать можно, но проблема в следующем: у спутника свои атомные часы, у GPS приемника свои
(хорошо если TCXO), у CAN адаптера свои, у компьютера свои (RTC + что-то с более высоким разрешением)
Сдвиг + дрифт для спутников знает центр управления, и они есть в навигационном сообщении,
для нас это вообще на практике = 0.
GPS приемник знает свой сдвиг + дрифт из решения навигационной задачи,
а вот для CAN и компьютера придется считать самим + (кое-где) использовать сигнал PPS
от GPS приемника для синхронизации внутренних часов.
При “сыпании” будут фактически использованы внутренние часы компьютера,
так как он “сыпет” в файл, а все остальные сыпют когда считают нужным, и этот процесс
фактически не контролируем.

OverQuantum · August 13, 2013, 10:05pm

Нет, не квадратов. Я же написал “для GPS отклонение в 2-3 метра почти также вероятно как и нулевое, грубо говоря”.
МНК хорош если вы давите шум, который распределён строго по гауссу. Для отклонения GPS от точного положения, наверное надо брать что-то посложнее, грубо говоря более ровное вблизи нуля.
Может быть придётся учитывать динамические эффекты для борьбы с медленным дрифтом (GPS уползший в одну сторону на 15 метров, не бросит сразу же на 20 метров в другую сторону - оно скорее сначала приползёт к правильному положению).

chnav · August 13, 2013, 10:19pm

К сожалению это не так - достаточно измениться созвездию, что в городских условиях обыденное явление.
Что касается “отклонение 0м так же вероятно как отклонение 3м” - это тоже неверная модель: внутри приемника тот же МНК (прописано в базовых документах NAVSTAR) и трактовать результаты его рассчетов следует по ~~случайному~~ (поправлено) нормальному распределению. Подтверждено многолетней статистикой.
Ну и в-третьих у пользователя GPS-приемника нет абсолютно никаких способов узнать когда приемник показывает “правильное положение”. Поэтому мне непонятно желание иметь данные с частотой 100Гц - при подобной точности нет никакой разницы 10Гц или 100Гц, разве что картинка красивее. Поясню: например оцениваем точность абсолютной GPS-координаты (пусть это будет 5м), используя сенсоры получаем 100 промежуточных координат с чуть низшей точностью (она априори не может быть лучше чем точность опорных точек). При скорсти 120км/ч это точки через 0.3м. А теперь нарисуем в каждой точке окружность радиусом 5м - где-то в указанной полосе находится наше реальное положение. Нет никакой разницы между 2-5Гц и 100Гц.

Vitalts · August 14, 2013, 6:41am

ИМХО, обсуждаемые выкладки справедливы для реалтайм привязки треков, однако, пакетная постобработка сырых данных должна дать гораздо лучшие результаты. Погрешность ГПС 5м на концах ничто для трека в несколько км. Возможно, наличие информации о положении руля и мгновенной скорости тоже могут послужить во благо. По всей видимости, погрешности обсуждаемой системы (без ГПС) на прямой и в поворотах будут разными.

usm78-gis · August 14, 2013, 7:18am

Чем быстрее измеренные данные о “реальном” состоянии динамической системы попадают
в фильтр, тем точнее конечный результат интегрирования (особенно на поворотах).
Я бы посоветовал всем все-таки прочитать (уравнения фильтра Калмана)
Technical paper: “Low-cost Sensor Fusion Dead Reckoning using a Single-Frequency GNSS Receiver Combined with Gyroscope and Wheel Tick Measurements”
и (картинка для параметров + результаты)
Technical paper: “Performance of low-cost real-time navigation system using single frequency GNSS measurements combined with wheel-tick data”
вот с этой страницы
http://www.u-blox.com/de/dead-reckoning.html
Все уже сделано до нас, надо только этим с умом воспользоваться.

chnav · August 14, 2013, 7:57am

Так вечь речь шла не о частоте данных с гироскопов, а о частоте готовых координат… Впрочем я понял, тут скорее академический интерес и желание сделать своими руками. Если заработает то проредить 100-герцовые координаты до 5-10Гц всегда можно.

Но вообще, конечно, странно что u-Blox в чипах 6R “зажал” координаты до 1 Гц. Что им, жалко было оставить 5 Гц… Этого хватило бы под любые автомобильные запросы. Ценовая политика u-Blox тоже отпугивает.

PS: всё не дойдут руки выложить скриншоты сравнения GPS vs DGPS на основе данных SirfStarIII. Там хорошо видно что для привязки снимков частоты 1Гц и DGPS более чем достаточно. Естественно речь об открытых местах. Съемку делал крутясь по круговому перекрёстку в Химках, стараясь удерживать заднее колесо на одном и том же расстоянии от бордюра. Очень забавные треки, особенно когда сравниваешь два круга собранных с разницей по времени.

SviMik · August 15, 2013, 3:45pm

Ну и заработался же я, даже данные выложить не было времени.

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

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

Пример номер 17:

http://svimik.com/imudata17.rar (1.16мб)
Качество GPS трека - отличное, от начала до конца.
Трек инерциальной системы получился идеальным на фоне остальных.
Пример примечателен тем, что во-первых короткий, во-вторых начало и конец расположены на одном парковочном месте, и в идеале должны совпасть.

Пример номер 18:

http://svimik.com/imudata18.rar (2.44мб)
Качество GPS трека - отличное снаружи, и ужасное внутри помещения (на скрине обрезан по HDOP и числу спутников).
Трек инерциальной системы получился хорошим.
Пример примечателен ездой в помещении, в отсутствие GPS сигнала. Приёмник пытался что-то поймать из окон, но мы не будем это считать.

Пример номер 19:

http://svimik.com/imudata19.rar (3.65мб)
Качество GPS трека - отличное (но с оговоркой, что я приёмник включил и сразу поехал, так что раздуплиться он не успел - валидные GPS координаты появились не сразу).
Трек инерциальной системы тоже получился хорошим.

Пример номер 20:

http://svimik.com/imudata20.rar (5.85мб)
Качество GPS трека - отличное от начала до конца (приёмник был предварительно разогрет предыдущей поездкой).
Трек инерциальной системы почему-то получился отвратительным. Причины не понятны, ибо в условиях эксперимента ничего не менялось.

SviMik · August 15, 2013, 3:46pm

Архив содержит 3 лога:

Лог камеры. Пока можно игнорировать, ибо видео я не вкладывал в архив. Видео очень тяжёлое, могу сделать выдачу покадрово по примеру этой.
Фрмат лога простой - время и номер кадра.
Лог GPS. Обычный NMEA, дополненный метками времени приёма данных.
Лог датчиков. Формат придумал немного под стиль NMEA

$WXYZ - расчёты чипа MPU6050 в виде кватернионов. Разработчики там умные, так что за основу я беру именно их
Полный список функций расчётов с ними на Си можно взять здесь. Там же есть и пример, как взять показания акселерометра с вычетом гравитации.

$MAGR - магнетометр, сырые данные (x,y,z)
$ARAW - акселерометр, сырые данные (x,y,z)
$GRAW -гироскоп, сырые данные (x,y,z)

$WRAW - датчики колёс, сырые данные. Не помню, какой из них левый, какой правый, буду называть номер 1 и номер 2. Состоит из:
(каждый пункт - две цифры, для колеса 1 и 2 соответственно)

Пройденное расстояние (приблизительно 50 импульсов на метр, или 2см на импульс).
Длительность последнего импульса, выражаемого в 65535 единиц в секунду. Превращается в скорость (км\ч) по формуле 1/(ss_interval/6553550.4)/100060*60
Время, прошедшее с момента последнего импульса. Используется как детектор остановки (если время больше 200мс например). Также я заменяю им п.2, если это значение больше (в теории позволяет чуть более своевременно реагировать на замедление).

$WSPD - текущая скорость, колесо 1 и 2 соответственно. По алгоритму предыдущего пункта.
$WDST - пройденное расстояние в метрах. Усреднённое значение обоих колёс.
$MAGC - оси магнетометра с коррекцией по текущему крену\тангажу. Предположительно неправильно, компас нуждается в калибровке. Можно пересчитать самому из кватернионов $WXYZ и сырых показаний $MAGR (после их постобработки).
$YAW - курс по компасу (неверно, не использовать) и курс по кватернионам (его и использовать. можно считать самому из $WXYZ, но думаю нет нужды)
$YPR - курс, тангаж, крен. Курс здесь с попыткой корректировки по компасу (неверно, не использовать). Тангаж, крен - по кватернионам.

OverQuantum · August 15, 2013, 4:43pm

SviMik
Время синхронизировано? Ну и формат imu.log жду

SviMik · August 15, 2013, 6:17pm

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

Быстрый старт для тех, кто пишет на С++:

Классы Quaternion и VectorFloat: http://svimik.com/helper_3dmath.h

Функцию dmpGetYawPitchRoll я переделал, ибо та, которая была, выдавала что-то не совпадающее с моими ожиданиями (подробности уже не вспомню).

void dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
	float sqw = q->w*q->w;
	float sqx = q->x*q->x;
	float sqy = q->y*q->y;
	float sqz = q->z*q->z;
	data[0] = atan2(2.0f * ( q->x * q->y + q->z * q->w ) , (  sqx - sqy - sqz + sqw ));
	data[1] = atan(gravity->x / sqrt(gravity->y*gravity->y + gravity->z*gravity->z));
	data[2] = atan2(2.0f * ( q->y * q->z + q->x * q->w ) , ( -sqx - sqy + sqz + sqw ));
}

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

Quaternion * q=new Quaternion(0,0,0,0);
VectorFloat * gravity=new VectorFloat();
float ypr[3];

dmpGetGravity(gravity, q);
dmpGetYawPitchRoll(ypr, q, gravity);

SviMik · August 15, 2013, 6:41pm

Постобработку делал на пхп. Если кто будет строить трек, вот функция, которую я использовал:

define("RAD_TO_DEG", 57.295779513082320876798154814105);

function move(&$lat, &$log, $dist, $yaw){
	$x=(sin(($yaw-180)/RAD_TO_DEG)*$dist);
	$y=(cos(($yaw-180)/RAD_TO_DEG)*$dist);
	$lat+=$y/111111;
	$log+=$x/(111111*cos($lat/RAD_TO_DEG));
}

function getmove($lat1, $log1, $lat2, $log2){
	$y=($lat2-$lat1)*111111;
	$x=($log2-$log1)*(111111*cos($lat1/RAD_TO_DEG));
	$dist=sqrt(pow($x, 2)+pow($y, 2));
	if($dist==0){
		return false;
	}
	$yaw=atan2($x, $y)*RAD_TO_DEG;
	return array($dist, $yaw);
}

Перемещает текущие географические координаты на $dist метров в направлении $yaw градусов. И обратная функция кстати тоже имеется. Использовалась, когда надо сохранить изменённый трек в исходный формат

Библиотека работы с углами, на которую я потратил немало времени
http://svimik.com/deg_math.txt
Позволяет складывать, вычитать, и находить среднее между двумя углами.

OverQuantum · August 15, 2013, 8:08pm

Йо, кватернионы. Я знал, что теормех был не зря.

Рекомендую попробовать
QueryPerformanceCounter и http://ru.wikipedia.org/wiki/Rdtsc
На С как-то так (код 2005 года):

#define EMIT(x) __asm __emit 0##x##h 
#define rdtsc EMIT(0F) EMIT(31)

inline void GetRDTSC(__int64 * i64)
{
	DWORD i[2];
	_asm
	{
		rdtsc
		mov i+0, eax
		mov	i+4, edx
	}
	memcpy(i64,i,8);
}