Comments 20
У всех таких систем проблема с тем, что ошибка накапливается. Без решения этой проблемы у тебя треккер не получится сделать.
Понимаю
Я уточнил, что хотел бы отслеживать траекторию за несколько секунд. Например, между пакетами от жипиэс модуля.
Или же прикрепить датчики к частям тела, нарисовать кинематическую модель человека и определять взаимные расроложения членов. Даже видел рекламу чего-то подобного. В узком смысле это тоже будет трекер, например, руки относительно туловища.
Например, между пакетами от жипиэс модуля
Это dead readkon есть в GPS модулях UBlox и еще не помню какой фирмы (LC29 вроде модули)
прикрепить датчики к частям тела, нарисовать кинематическую модель человека и определять взаимные расроложения членов
есть вот такое https://github.com/SlimeVR/SlimeVR-Tracker-ESP оно вроде немножко по другому но близко
Был отличный открытый проект mahowii, для того чтобы разобраться самое то.
Вам надо просто комплементарный фильтр взять. А лучше фильтр Мэджвика. Он работает в кватернионах.
/*
* Madgwick AHRS Arduino library
* Math author: Sebastian Madgwick
* Written by Oleg Evsegneev for RobotClass.
*/
extern volatile float beta;
extern volatile float q0, q1, q2, q3;
void MadgwickAHRSupdate(float tdelta, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
void MadgwickAHRSupdateIMU(float tdelta, float gx, float gy, float gz, float ax, float ay, float az);
float invSqrt(float x);
void quat2Euler( float q[4], float e[3] );/*
* Madgwick AHRS Arduino library
* Math author: Sebastian Madgwick
* Written by Oleg Evsegneev for RobotClass.
*/
#include "madgwickahrs.h"
#include <math.h>
#define betaDef 0.2f // 2 * proportional gain
volatile float beta = betaDef; // 2 * proportional gain (Kp)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
void MadgwickAHRSupdate(float tdelta, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float _8bx, _8bz;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MadgwickAHRSupdateIMU(tdelta, gx, gy, gz, ax, ay, az);
return;
}
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
_8bx = 2.0f * _4bx;
_8bz = 2.0f * _4bz;
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * (q1q3 - q0q2) - ax) + _2q1 * (2.0f * (q0q1 + q2q3) - ay) - _4bz * q2 * (_4bx * (0.5 - q2q2 - q3q3) + _4bz * (q1q3 - q0q2) - mx) + (-_4bx * q3 + _4bz * q1) * (_4bx * (q1q2 - q0q3) + _4bz * (q0q1 + q2q3) - my) + _4bx * q2 * (_4bx * (q0q2 + q1q3) + _4bz * (0.5 - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * (q1q3 - q0q2) - ax) + _2q0 * (2.0f * (q0q1 + q2q3) - ay) - 4.0f * q1 * (2.0f * (0.5 - q1q1 - q2q2) - az) + _4bz * q3 * (_4bx * (0.5 - q2q2 - q3q3) + _4bz * (q1q3 - q0q2) - mx) + (_4bx * q2 + _4bz * q0) * (_4bx * (q1q2 - q0q3) + _4bz * (q0q1 + q2q3) - my) + (_4bx * q3 - _8bz * q1) * (_4bx * (q0q2 + q1q3) + _4bz * (0.5 - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * (q1q3 - q0q2) - ax) + _2q3 * (2.0f * (q0q1 + q2q3) - ay) + (-4.0f * q2) * (2.0f * (0.5 - q1q1 - q2q2) - az) + (-_8bx * q2 - _4bz * q0) * (_4bx * (0.5 - q2q2 - q3q3) + _4bz * (q1q3 - q0q2) - mx) + (_4bx * q1 + _4bz * q3) * (_4bx * (q1q2 - q0q3) + _4bz * (q0q1 + q2q3) - my) + (_4bx * q0 - _8bz * q2) * (_4bx * (q0q2 + q1q3) + _4bz * (0.5 - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * (q1q3 - q0q2) - ax) + _2q2 * (2.0f * (q0q1 + q2q3) - ay) + (-_8bx * q3 + _4bz * q1) * (_4bx * (0.5 - q2q2 - q3q3) + _4bz * (q1q3 - q0q2) - mx) + (-_4bx * q0 + _4bz * q2) * (_4bx * (q1q2 - q0q3) + _4bz * (q0q1 + q2q3) - my) + (_4bx * q1) * (_4bx * (q0q2 + q1q3) + _4bz * (0.5 - q1q1 - q2q2) - mz);
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
// normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * tdelta;
q1 += qDot2 * tdelta;
q2 += qDot3 * tdelta;
q3 += qDot4 * tdelta;
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
void MadgwickAHRSupdateIMU(float tdelta, float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 , _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * tdelta;
q1 += qDot2 * tdelta;
q2 += qDot3 * tdelta;
q3 += qDot4 * tdelta;
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i >> 1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
}
void quat2Euler( float q[4], float e[3] ) {
double sqx = q[1] * q[1];
double sqy = q[2] * q[2];
double sqz = q[3] * q[3];
e[0] = atan2f(2.f * (q[2] * q[3] + q[1] * q[0]), 1 - 2.f * (sqx + sqy)); // -sqx - sqy + sqz + sqw);
e[1] = asinf(-2.f * (q[1] * q[3] - q[2] * q[0]));
e[2] = atan2f(2.f * (q[1] * q[2] + q[3] * q[0]), 1 - 2.f * (sqy + sqz)); //sqx - sqy - sqz + sqw);
}Тут две функции — с магнитометром и без.
Вот использование без магнитометра.
dt- период обработки в секундах, обратно частоте вызова фильтра.
gx,gy,gz — гироскопы
ax,ay,az — акселерометры
float imu[3];
float quat[4];
MadgwickAHRSupdateIMU(dt,GRAD_TO_RAD(gx),GRAD_TO_RAD(gy),GRAD_TO_RAD(gz),ax,ay,az);
quat[0]=q0;
quat[1]=q1;
quat[2]=q2;
quat[3]=q3;
quat2Euler(&quat[0],&imu[0]);
float ma1=RAD_TO_GRAD(imu[0]);//крен
float ma2=RAD_TO_GRAD(imu[1]);//тангаж
float ma3=RAD_TO_GRAD(imu[2]);//рысканье
Эту же задачу решают и фильтром Калмана, но тут у меня нет готового решения.
Ну и ещё есть задача управления двигателями аппарата — тут уже регуляторы (ПИД, наверное) потребуются.
Имейте в виду, все гироскопы и акселерометры должны быть привязаны друг к другу, отмасштабированы и без смещений. Для привязки можно использовать матрицы привязки (строятся по трём углам вращения). Но вот процедура калибровки этих углов дома может оказаться сложной.
Очень интересная идея. Подобная штука есть в устройстве woo для трекинга прыжков кайтсерферов. Вот насколько она точно работает проверить не довелось. Был еще стартап shadowbox для подобного но он так и не был реализован вроде.
Кстати, качество ваших гироскопов и акселерометров вам покажет девиация Аллана (обычно, график есть в даташите). Выбирайте датчики с лучшими характеристиками! Вот тут описано.
В даташитах Аллан был только в датчиках стоимость от 500 евро. Все что до 20 баксов одна маленькая строка, аля шум. Алана можно самому посчитать. Собрать лог на 2-4 чтобы наверняка. И любой скрипт гуглиться легко, matlab allan mems
Не знаю, сколько сейчас стоят CRM200/300 и adxl355, но год назад стоили меньше 500 евро. Но и не 20$ точно. С дешёвыми датчиками ничего хорошего не выйдет.
Алана можно самому посчитать.
Дома — только для довольно грубых датчиков. Для прецизионных вам потребуется хорошая развязка от колебаний и температурных деформаций. Иначе получатся левые цифры.
Кватернионы разве не решают задачу в теории? На практике в авиации и судоходстве давно используют инерционную навигацию.
О сново инерциалка на хабре. И Яндекс свою приблуду выпустил и Ваша статья. Фильтр мэджвика, маст хэв. Книга Матвеева маст хэв и распопава чтобы понять како гэ мэмс. Ну там уже фильтры Калмана, слабо-связные и т.д -:)
Главная проблема в том что акселерометры в мемс датчиках имеют довольно большую погрешность. Пусть она много меньше процента, но это вторая производная от положения все-таки. Ошибка акселерометра в 0.1% даст через минуту ошибку по положению больше 30 метров. Так что скорости измерять через такие датчики весьма проблематично. А вот для вычисления ориентации вполне годится.
Я а этом году примерно таким же занимался, мне надо было положение велосипедиста найти. GPS, даже со всеми базами, прыгает в довольно большом радиусе, особенно во дворах. С помощью датчика на колесе и 6доф удалось довольно точно трек получить. Получилось, что локально направление очень точно выдаётся, а глобальное уплывание легко корректируется по GPS (если взять удаленные точки трека, то даже если у gps будет большая ошибка, в целом, угол будет правильный, и относительно него можно гироскоп скорректировать). В результате, когда в Москве в центре GPS совсем выключили, оказалось что он не особо то и нужен - достаточно задать несколько ключевых точек трека, что бы он лег на карту
Теперь с картиночками!!
Мой первый околонаучный проект: инерциальный трекер