Фикс лидара (ttyUSB0, парсинг пакетов), новые расчёты (CF + ZUPT + bias), улучшенная визуализация

imu_module/imu_reader.py

@@ -1,40 +1,53 @@
+"""
+IMU Reader для MPU-9250.
+Улучшения относительно прежней версии:
+  - реальные timestamp (time.monotonic), а не счётчик
+  - блочное чтение 14 байт за один I2C-запрос
+  - пачечная запись в SQLite (commit раз в 5 семплов = ~50 мс)
+  - WAL-режим для параллельного чтения
+  - устойчивость к временным ошибкам I2C
+  - точное удержание 100 Гц с компенсацией дрейфа цикла
+"""
 import smbus2
 import time
 import sqlite3
 import signal
-import sys
+import os
 
-# Константы
+# === Конфигурация ===
 MPU_ADDRESS = 0x68
 I2C_BUS = 2
-DB_PATH = '../inertial_data.db'
+DB_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'inertial_data.db')
 
 # Регистры MPU-9250
 PWR_MGMT_1 = 0x6B
 ACCEL_XOUT_H = 0x3B
-GYRO_XOUT_H = 0x43
 
 # Масштабные коэффициенты
-ACCEL_SCALE = 16384.0
+ACCEL_SCALE = 16384.0  # LSB/g для диапазона ±2g
-GYRO_SCALE = 131.0
+GYRO_SCALE = 131.0     # LSB/(°/с) для диапазона ±250°/с
+
+# Частота и буферизация
+SAMPLE_PERIOD = 0.01   # 10 мс = 100 Гц
+COMMIT_EVERY_N = 5     # коммит раз в 5 семплов (~50 мс)
 
-# Глобальные переменные для завершения
 running = True
-conn = None
+
-bus = None
 
 def shutdown(signum, frame):
-    """Обработчик сигнала завершения"""
     global running
-    print("Завершение IMU модуля...")
+    print("Завершение IMU модуля...", flush=True)
     running = False
 
-# Перехватываем сигналы завершения
+
 signal.signal(signal.SIGTERM, shutdown)
 signal.signal(signal.SIGINT, shutdown)
 
+
 def init_db():
     conn = sqlite3.connect(DB_PATH)
+    conn.execute('PRAGMA journal_mode=WAL')
+    conn.execute('PRAGMA synchronous=NORMAL')
     conn.execute('''CREATE TABLE IF NOT EXISTS imu_data (
         timestamp REAL,
         ax REAL, ay REAL, az REAL,
@@ -44,49 +57,96 @@ def init_db():
     conn.commit()
     return conn
 
+
 def init_mpu(bus):
+    # Снимаем sleep-бит, MPU начинает работать
     bus.write_byte_data(MPU_ADDRESS, PWR_MGMT_1, 0x00)
     time.sleep(0.1)
 
-def read_raw(bus, reg):
+
-    high = bus.read_byte_data(MPU_ADDRESS, reg)
+def to_signed(high, low):
-    low = bus.read_byte_data(MPU_ADDRESS, reg + 1)
+    v = (high << 8) | low
-    value = (high << 8) | low
+    return v - 65536 if v > 32767 else v
-    if value > 32767:
+
-        value -= 65536
-    return value
 
 def read_imu(bus):
-    ax = read_raw(bus, ACCEL_XOUT_H) / ACCEL_SCALE
+    # Одним запросом читаем 14 байт: ax/ay/az(6) + TEMP(2) + gx/gy/gz(6)
-    ay = read_raw(bus, ACCEL_XOUT_H + 2) / ACCEL_SCALE
+    data = bus.read_i2c_block_data(MPU_ADDRESS, ACCEL_XOUT_H, 14)
-    az = read_raw(bus, ACCEL_XOUT_H + 4) / ACCEL_SCALE
+    ax = to_signed(data[0], data[1]) / ACCEL_SCALE
-    gx = read_raw(bus, GYRO_XOUT_H) / GYRO_SCALE
+    ay = to_signed(data[2], data[3]) / ACCEL_SCALE
-    gy = read_raw(bus, GYRO_XOUT_H + 2) / GYRO_SCALE
+    az = to_signed(data[4], data[5]) / ACCEL_SCALE
-    gz = read_raw(bus, GYRO_XOUT_H + 4) / GYRO_SCALE
+    # data[6], data[7] — температура, не используем
+    gx = to_signed(data[8], data[9]) / GYRO_SCALE
+    gy = to_signed(data[10], data[11]) / GYRO_SCALE
+    gz = to_signed(data[12], data[13]) / GYRO_SCALE
     return ax, ay, az, gx, gy, gz
 
-def save_to_db():
+
-    global conn, bus, running
+def main():
+    global running
     conn = init_db()
     bus = smbus2.SMBus(I2C_BUS)
     init_mpu(bus)
 
-    print("Запись данных IMU...")
+    print(f"IMU запущен. Целевая частота: {1/SAMPLE_PERIOD:.0f} Гц", flush=True)
-    t = 0.0
+
+    start_time = time.monotonic()
+    next_tick = start_time + SAMPLE_PERIOD
+    batch = []
+
     try:
         while running:
-            ax, ay, az, gx, gy, gz = read_imu(bus)
+            # Спим до следующего тика — без дрейфа цикла
-            conn.execute(
+            now = time.monotonic()
+            sleep_time = next_tick - now
+            if sleep_time > 0:
+                time.sleep(sleep_time)
+
+            # Читаем датчик с защитой от случайных ошибок I2C
+            try:
+                ax, ay, az, gx, gy, gz = read_imu(bus)
+            except OSError as e:
+                print(f"IMU read error: {e}", flush=True)
+                next_tick += SAMPLE_PERIOD
+                continue
+
+            # Реальное время от старта (монотонные секунды)
+            t = time.monotonic() - start_time
+
+            batch.append((
+                round(t, 4),
+                round(ax, 4), round(ay, 4), round(az, 4),
+                round(gx, 4), round(gy, 4), round(gz, 4),
+            ))
+
+            # Пачечный коммит
+            if len(batch) >= COMMIT_EVERY_N:
+                conn.executemany(
+                    'INSERT INTO imu_data VALUES (?, ?, ?, ?, ?, ?, ?)',
+                    batch
+                )
+                conn.commit()
+                batch.clear()
+
+            # Назначаем следующий тик
+            next_tick += SAMPLE_PERIOD
+
+            # Если сильно отстали (например, после долгой паузы) — ресинк,
+            # чтобы не было «штормового» догоняния.
+            if time.monotonic() - next_tick > SAMPLE_PERIOD * 5:
+                next_tick = time.monotonic() + SAMPLE_PERIOD
+    finally:
+        # Сбросим остаток батча
+        if batch:
+            conn.executemany(
                 'INSERT INTO imu_data VALUES (?, ?, ?, ?, ?, ?, ?)',
-                (round(t, 3), round(ax, 4), round(ay, 4), round(az, 4),
+                batch
-                 round(gx, 4), round(gy, 4), round(gz, 4))
             )
             conn.commit()
-            t += 0.01
-            time.sleep(0.01)
-    finally:
         bus.close()
         conn.close()
-        print("IMU модуль остановлен")
+        print("IMU модуль остановлен", flush=True)
 
-save_to_db()
+
+if __name__ == '__main__':
+    main()

lidar_module/lidar_reader.py

@@ -3,11 +3,12 @@ import time
 import sqlite3
 import struct
 import signal
+import os
 
 # Константы
-SERIAL_PORT = '/dev/ttyS0'
+SERIAL_PORT = '/dev/ttyUSB0'
-BAUD_RATE = 230400
+BAUD_RATE = 115200
-DB_PATH = '../inertial_data.db'
+DB_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'inertial_data.db')
 
 # Заголовок пакета LDS02RR
 HEADER = 0xFA
@@ -51,11 +52,16 @@ def parse_packet(data):
     points = []
     for i in range(4):
         offset = 4 + i * 4
-        distance_raw = struct.unpack_from('<H', data, offset)[0]
+        b0 = data[offset]
-        quality = struct.unpack_from('<H', data, offset + 2)[0] >> 2
+        b1 = data[offset + 1]
-        distance = distance_raw / 4.0
+        b2 = data[offset + 2]
+        b3 = data[offset + 3]
+        invalid = (b1 >> 7) & 1
+        distance = b0 | ((b1 & 0x3F) << 8)
+        quality = b2 | (b3 << 8)
         angle = index * 4 + i
-        points.append((angle, distance, quality))
+        if not invalid and distance > 0:
+            points.append((angle, distance, quality))
     return points
 
 def save_to_db():
@@ -101,4 +107,4 @@ def save_to_db():
         conn.close()
         print("Лидар модуль остановлен")
 
-save_to_db()
+save_to_db()

processing/calculations.cpp

@@ -7,113 +7,121 @@
 #include <csignal>
 #include <sqlite3.h>
 #include <Eigen/Dense>
+#include <unistd.h>
+#include <libgen.h>
 
 using namespace std;
 using namespace Eigen;
 
-// ===== ФЛАГ ЗАВЕРШЕНИЯ =====
 volatile sig_atomic_t running = 1;
+void shutdown(int signum) { running = 0; }
 
-void shutdown(int signum) {
+struct IMUData   { double t; Vector3d acc; Vector3d gyro; };
-    running = 0;
+struct LidarData { double t; double angle; double distance; };
-}
 
-// ===== СТРУКТУРЫ =====
+// Состояние системы
+Vector3d pos    = Vector3d::Zero();  // позиция (м)
+Vector3d vel    = Vector3d::Zero();  // скорость (м/с)
+Vector3d angles = Vector3d::Zero();  // углы roll, pitch, yaw (рад)
 
-struct IMUData {
+// Bias акселерометра (в единицах g).
-    double t;
+// При калибровке измеряется начальное значение,
-    Vector3d acc;
+// затем автоматически уточняется в каждую остановку (ZUPT).
-    Vector3d gyro;
+double ax_bias = 0.0;
-};
+double ay_bias = 0.0;
+double az_bias = 1.0;  // ~1g — гравитация по Z когда датчик горизонтальный
 
-struct LidarData {
+// Параметры ZUPT
-    double t;
+const double ZUPT_ACC_THR  = 0.08;  // отклонение нормы от 1g
-    double angle;
+const double ZUPT_GYRO_THR = 0.05;  // рад/с (~3 °/с)
-    double distance;
+const int    ZUPT_WINDOW   = 10;    // сэмплов подряд (~120 мс)
-};
 
-struct State {
+// Коэффициент комплементарного фильтра
-    VectorXd x;
+// 0.98 = 98% гироскоп (быстро, но дрейфует)
-    MatrixXd P;
+// 0.02 = 2%  акселерометр (медленно, но абсолютное значение)
-};
+const double CF_ALPHA = 0.98;
 
-// ===== ЧТЕНИЕ ИЗ SQLite =====
+// ─────────────────────────────────────────────
-
+// Чтение БД
-vector<IMUData> readIMU(sqlite3* db) {
+// ─────────────────────────────────────────────
+vector<IMUData> readIMU(sqlite3* db, double after_ts) {
     vector<IMUData> data;
     sqlite3_stmt* stmt;
-
+    sqlite3_prepare_v2(db,
-    const char* sql = "SELECT timestamp, ax, ay, az, gx, gy, gz FROM imu_data ORDER BY timestamp";
+        "SELECT timestamp, ax, ay, az, gx, gy, gz "
-    sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
+        "FROM imu_data WHERE timestamp > ? ORDER BY timestamp",
-
+        -1, &stmt, nullptr);
+    sqlite3_bind_double(stmt, 1, after_ts);
     while (sqlite3_step(stmt) == SQLITE_ROW) {
         IMUData d;
-        d.t = sqlite3_column_double(stmt, 0);
+        d.t        = sqlite3_column_double(stmt, 0);
-        d.acc.x() = sqlite3_column_double(stmt, 1);
+        d.acc.x()  = sqlite3_column_double(stmt, 1);
-        d.acc.y() = sqlite3_column_double(stmt, 2);
+        d.acc.y()  = sqlite3_column_double(stmt, 2);
-        d.acc.z() = sqlite3_column_double(stmt, 3);
+        d.acc.z()  = sqlite3_column_double(stmt, 3);
         d.gyro.x() = sqlite3_column_double(stmt, 4);
         d.gyro.y() = sqlite3_column_double(stmt, 5);
         d.gyro.z() = sqlite3_column_double(stmt, 6);
         data.push_back(d);
     }
-
     sqlite3_finalize(stmt);
     return data;
 }
 
-vector<LidarData> readLidar(sqlite3* db) {
+vector<LidarData> readLidar(sqlite3* db, double after_ts) {
     vector<LidarData> data;
     sqlite3_stmt* stmt;
-
+    sqlite3_prepare_v2(db,
-    const char* sql = "SELECT timestamp, angle, distance_mm FROM lidar_data ORDER BY timestamp";
+        "SELECT timestamp, angle, distance_mm "
-    sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
+        "FROM lidar_data WHERE timestamp > ? ORDER BY timestamp",
-
+        -1, &stmt, nullptr);
+    sqlite3_bind_double(stmt, 1, after_ts);
     while (sqlite3_step(stmt) == SQLITE_ROW) {
         LidarData d;
-        d.t = sqlite3_column_double(stmt, 0);
+        d.t        = sqlite3_column_double(stmt, 0);
-        d.angle = sqlite3_column_double(stmt, 1);
+        d.angle    = sqlite3_column_double(stmt, 1);
         d.distance = sqlite3_column_double(stmt, 2);
         data.push_back(d);
     }
-
     sqlite3_finalize(stmt);
     return data;
 }
 
-// ===== ЗАПИСЬ В SQLite =====
+// ─────────────────────────────────────────────
-
+// Запись в БД
+// ─────────────────────────────────────────────
 void initDB(sqlite3* db) {
-    const char* sql =
+    sqlite3_exec(db,
+        "PRAGMA journal_mode=WAL;"
+        "PRAGMA synchronous=NORMAL;"
         "CREATE TABLE IF NOT EXISTS trajectory ("
         "timestamp REAL, x REAL, y REAL, z REAL,"
         "roll REAL, pitch REAL, yaw REAL);"
         "DELETE FROM trajectory;"
-        "CREATE TABLE IF NOT EXISTS lidar_points ("
+        "CREATE TABLE IF NOT EXISTS lidar_points (x REAL, y REAL, z REAL);"
-        "x REAL, y REAL, z REAL);"
+        "DELETE FROM lidar_points;",
-        "DELETE FROM lidar_points;";
+        nullptr, nullptr, nullptr);
-    sqlite3_exec(db, sql, nullptr, nullptr, nullptr);
 }
 
-void writeTraj(sqlite3* db, double t, Vector3d pos, Vector3d ang) {
+void writeTraj(sqlite3* db, double t) {
     sqlite3_stmt* stmt;
-    const char* sql = "INSERT INTO trajectory VALUES (?,?,?,?,?,?,?)";
+    sqlite3_prepare_v2(db,
-    sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
+        "INSERT INTO trajectory VALUES (?,?,?,?,?,?,?)",
+        -1, &stmt, nullptr);
     sqlite3_bind_double(stmt, 1, t);
     sqlite3_bind_double(stmt, 2, pos.x());
     sqlite3_bind_double(stmt, 3, pos.y());
     sqlite3_bind_double(stmt, 4, pos.z());
-    sqlite3_bind_double(stmt, 5, ang.x());
+    sqlite3_bind_double(stmt, 5, angles.x());
-    sqlite3_bind_double(stmt, 6, ang.y());
+    sqlite3_bind_double(stmt, 6, angles.y());
-    sqlite3_bind_double(stmt, 7, ang.z());
+    sqlite3_bind_double(stmt, 7, angles.z());
     sqlite3_step(stmt);
     sqlite3_finalize(stmt);
 }
 
 void writeLidarPoint(sqlite3* db, Vector3d p) {
     sqlite3_stmt* stmt;
-    const char* sql = "INSERT INTO lidar_points VALUES (?,?,?)";
+    sqlite3_prepare_v2(db, "INSERT INTO lidar_points VALUES (?,?,?)",
-    sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
+                       -1, &stmt, nullptr);
     sqlite3_bind_double(stmt, 1, p.x());
     sqlite3_bind_double(stmt, 2, p.y());
     sqlite3_bind_double(stmt, 3, p.z());
@@ -121,60 +129,105 @@ void writeLidarPoint(sqlite3* db, Vector3d p) {
     sqlite3_finalize(stmt);
 }
 
-// ===== ФИЗИКА (без изменений) =====
+// ─────────────────────────────────────────────
-
+// Матрица поворота (roll, pitch, yaw)
+// ─────────────────────────────────────────────
 Matrix3d rotationMatrix(double roll, double pitch, double yaw) {
-    AngleAxisd rx(roll, Vector3d::UnitX());
+    AngleAxisd rx(roll,  Vector3d::UnitX());
     AngleAxisd ry(pitch, Vector3d::UnitY());
-    AngleAxisd rz(yaw, Vector3d::UnitZ());
+    AngleAxisd rz(yaw,   Vector3d::UnitZ());
     return (rz * ry * rx).toRotationMatrix();
 }
 
-void predict(State& s, const IMUData& imu, double dt) {
+Vector3d lidarToWorld(const LidarData& l) {
-    VectorXd& x = s.x;
+    double angle = l.angle * M_PI / 180.0;
-    Vector3d pos = x.segment<3>(0);
+    double dist  = l.distance / 1000.0;
-    Vector3d vel = x.segment<3>(3);
+    Vector3d local(dist * cos(angle), dist * sin(angle), 0);
-    Vector3d angles = x.segment<3>(6);
+    return pos + rotationMatrix(angles[0], angles[1], angles[2]) * local;
+}
 
-    Vector3d acc = imu.acc * 9.81;
+// ─────────────────────────────────────────────
+// predict() — формулы с доски + комплементарный фильтр
+//
+// Формулы преподавателя (для всех трёх осей OX, OY, OZ):
+//   V(nT) = V((n-1)T) + a·T
+//   X(nT) = V((n-1)T)·T + a·T²/2
+//   φ     = ω·T
+//
+// Комплементарный фильтр — решает проблему наклона:
+//   angle = α·(angle + gyro·dt) + (1-α)·angle_from_accel
+//   Гироскоп отслеживает быстрые повороты.
+//   Акселерометр корректирует накопленный дрейф.
+//
+// ZUPT — когда стоим: замораживаем всё, уточняем bias.
+// ─────────────────────────────────────────────
+void predict(const IMUData& imu, double dt) {
+    // Угловые скорости (рад/с)
     Vector3d gyro = imu.gyro * M_PI / 180.0;
 
-    Matrix3d R = rotationMatrix(angles[0], angles[1], angles[2]);
+    // ── Комплементарный фильтр ориентации ──────────────
-    Vector3d g(0, 0, 9.81);
+    // Углы наклона из акселерометра (абсолютные, но шумные)
-    Vector3d acc_world = R * acc - g;
+    double roll_acc  = atan2(imu.acc.y(), imu.acc.z());
+    double pitch_acc = atan2(-imu.acc.x(),
+                              sqrt(imu.acc.y() * imu.acc.y() +
+                                   imu.acc.z() * imu.acc.z()));
 
-    vel += acc_world * dt;
+    // Комбинируем: быстрая часть (гироскоп) + медленная коррекция (акселерометр)
-    pos += vel * dt + 0.5 * acc_world * dt * dt;
+    // φ = α·(φ + ω·T) + (1-α)·φ_акселерометр
-    angles += gyro * dt;
+    angles.x() = CF_ALPHA * (angles.x() + gyro.x() * dt) + (1.0 - CF_ALPHA) * roll_acc;
+    angles.y() = CF_ALPHA * (angles.y() + gyro.y() * dt) + (1.0 - CF_ALPHA) * pitch_acc;
+    angles.z() += gyro.z() * dt;  // yaw — только гироскоп (нет абсолютного источника)
 
-    x.segment<3>(0) = pos;
+    // ── Линейное ускорение в мировой системе ───────────
-    x.segment<3>(3) = vel;
+    // Шаг 1: вычитаем bias датчика и переводим в м/с²
-    x.segment<3>(6) = angles;
+    Vector3d acc_sensor(
+        (imu.acc.x() - ax_bias) * 9.81,
+        (imu.acc.y() - ay_bias) * 9.81,
+        imu.acc.z() * 9.81   // Z: bias не вычитаем, гравитация убирается через матрицу поворота
+    );
 
-    MatrixXd F = MatrixXd::Identity(15, 15);
+    // Шаг 2: поворачиваем в мировую систему координат
-    F.block<3, 3>(0, 3) = Matrix3d::Identity() * dt;
+    Matrix3d R = rotationMatrix(angles.x(), angles.y(), angles.z());
-    MatrixXd Q = MatrixXd::Identity(15, 15) * 0.01;
+    Vector3d acc_world = R * acc_sensor;
-    s.P = F * s.P * F.transpose() + Q;
+
+    // Шаг 3: вычитаем гравитацию (она направлена вниз = +Z в мировой системе)
+    acc_world.z() -= 9.81;
+
+    // ── ZUPT: детектор покоя ────────────────────────────
+    double acc_mag  = imu.acc.norm();
+    double gyro_mag = gyro.norm();
+    static int zupt_counter = 0;
+
+    if (abs(acc_mag - 1.0) < ZUPT_ACC_THR && gyro_mag < ZUPT_GYRO_THR) {
+        zupt_counter++;
+    } else {
+        zupt_counter = 0;
+    }
+
+    if (zupt_counter >= ZUPT_WINDOW) {
+        // ── СТОИМ ──────────────────────────────────────
+        // Замораживаем скорость
+        vel = Vector3d::Zero();
+        // Уточняем bias по всем трём осям (медленное скользящее среднее)
+        ax_bias = 0.99 * ax_bias + 0.01 * imu.acc.x();
+        ay_bias = 0.99 * ay_bias + 0.01 * imu.acc.y();
+        az_bias = 0.99 * az_bias + 0.01 * imu.acc.z();
+        // pos не меняем
+    } else {
+        // ── ДВИЖЕМСЯ ───────────────────────────────────
+        // Формулы с доски для OX, OY, OZ:
+        // X(nT) = V((n-1)T)·T + a·T²/2
+        // V(nT) = V((n-1)T) + a·T
+        pos += vel * dt + 0.5 * acc_world * dt * dt;
+        vel += acc_world * dt;
+    }
 }
 
-Vector3d lidarToWorld(const State& s, const LidarData& l) {
+// ─────────────────────────────────────────────
-    double angle = l.angle * M_PI / 180.0;
+// ICP — сопоставление сканов лидара
-    double dist = l.distance / 1000.0;
+// ─────────────────────────────────────────────
-    Vector3d local;
-    local << dist * cos(angle), dist * sin(angle), 0;
-    Vector3d pos = s.x.segment<3>(0);
-    Vector3d ang = s.x.segment<3>(6);
-    Matrix3d R = rotationMatrix(ang[0], ang[1], ang[2]);
-    return pos + R * local;
-}
-
-// ===== ICP (без изменений) =====
-
 struct KDNode {
-    Vector3d point;
+    Vector3d point; KDNode* left; KDNode* right; int axis;
-    KDNode* left;
-    KDNode* right;
-    int axis;
     KDNode(Vector3d p, int ax) : point(p), left(nullptr), right(nullptr), axis(ax) {}
 };
 
@@ -182,27 +235,26 @@ KDNode* buildKDTree(vector<Vector3d>& pts, int depth = 0) {
     if (pts.empty()) return nullptr;
     int axis = depth % 3;
     sort(pts.begin(), pts.end(),
-        [axis](const Vector3d& a, const Vector3d& b) { return a[axis] < b[axis]; });
+        [axis](const Vector3d& a, const Vector3d& b){ return a[axis] < b[axis]; });
     int mid = pts.size() / 2;
     KDNode* node = new KDNode(pts[mid], axis);
     vector<Vector3d> left(pts.begin(), pts.begin() + mid);
     vector<Vector3d> right(pts.begin() + mid + 1, pts.end());
-    node->left = buildKDTree(left, depth + 1);
+    node->left  = buildKDTree(left,  depth + 1);
     node->right = buildKDTree(right, depth + 1);
     return node;
 }
 
 void nearestSearch(KDNode* node, const Vector3d& target,
-    Vector3d& best, double& best_dist) {
+                   Vector3d& best, double& best_dist) {
     if (!node) return;
     double d = (node->point - target).squaredNorm();
     if (d < best_dist) { best_dist = d; best = node->point; }
-    int axis = node->axis;
+    KDNode* near_ = target[node->axis] < node->point[node->axis] ? node->left  : node->right;
-    KDNode* near = target[axis] < node->point[axis] ? node->left : node->right;
+    KDNode* far_  = target[node->axis] < node->point[node->axis] ? node->right : node->left;
-    KDNode* far = target[axis] < node->point[axis] ? node->right : node->left;
+    nearestSearch(near_, target, best, best_dist);
-    nearestSearch(near, target, best, best_dist);
+    double diff = target[node->axis] - node->point[node->axis];
-    double diff = target[axis] - node->point[axis];
+    if (diff * diff < best_dist) nearestSearch(far_, target, best, best_dist);
-    if (diff * diff < best_dist) nearestSearch(far, target, best, best_dist);
 }
 
 Matrix3d computeRotation(const vector<Vector3d>& src, const vector<Vector3d>& dst) {
@@ -216,7 +268,8 @@ Matrix3d computeRotation(const vector<Vector3d>& src, const vector<Vector3d>& ds
     return svd.matrixV() * svd.matrixU().transpose();
 }
 
-Vector3d computeTranslation(const vector<Vector3d>& src, const vector<Vector3d>& dst, const Matrix3d& R) {
+Vector3d computeTranslation(const vector<Vector3d>& src,
+                             const vector<Vector3d>& dst, const Matrix3d& R) {
     Vector3d c1 = Vector3d::Zero(), c2 = Vector3d::Zero();
     for (size_t i = 0; i < src.size(); i++) { c1 += src[i]; c2 += dst[i]; }
     c1 /= src.size(); c2 /= dst.size();
@@ -225,14 +278,13 @@ Vector3d computeTranslation(const vector<Vector3d>& src, const vector<Vector3d>&
 
 vector<Vector3d> downsample(const vector<Vector3d>& pts, int step = 5) {
     vector<Vector3d> out;
-    for (size_t i = 0; i < pts.size(); i += step)
+    for (size_t i = 0; i < pts.size(); i += step) out.push_back(pts[i]);
-        out.push_back(pts[i]);
     return out;
 }
 
-void ICP_fast(vector<Vector3d>& src, vector<Vector3d>& dst, Matrix3d& R, Vector3d& t) {
+void ICP_fast(vector<Vector3d>& src, vector<Vector3d>& dst,
-    R = Matrix3d::Identity();
+              Matrix3d& R, Vector3d& t) {
-    t = Vector3d::Zero();
+    R = Matrix3d::Identity(); t = Vector3d::Zero();
     vector<Vector3d> dst_copy = dst;
     KDNode* tree = buildKDTree(dst_copy);
     for (int iter = 0; iter < 10; iter++) {
@@ -251,41 +303,115 @@ void ICP_fast(vector<Vector3d>& src, vector<Vector3d>& dst, Matrix3d& R, Vector3
     }
 }
 
-// ===== MAIN =====
+// ─────────────────────────────────────────────
+// Калибровка: датчик лежит горизонтально, неподвижно.
+// Измеряем начальный bias по всем трём осям.
+// ─────────────────────────────────────────────
+void calibrate(sqlite3* db) {
+    cout << "Калибровка (держите датчик горизонтально, неподвижно)..." << endl;
 
-int main() {
+    double sum_ax = 0, sum_ay = 0, sum_az = 0;
-    signal(SIGTERM, shutdown);
+    int n = 0;
-    signal(SIGINT, shutdown);
+    double last_ts = 0;
-
+    bool seen_data = false;
-    sqlite3* db;
+    auto start = chrono::steady_clock::now();
-    sqlite3_open("../inertial_data.db", &db);
+    auto first_data_time = chrono::steady_clock::time_point();
-    initDB(db);
-
-    State state;
-    state.x = VectorXd::Zero(15);
-    state.P = MatrixXd::Identity(15, 15) * 0.1;
-
-    vector<Vector3d> prev_scan, curr_scan;
-    size_t lidar_idx = 0;
-    size_t imu_idx = 0;
-
-    cout << "Расчёты запущены..." << endl;
 
     while (running) {
-        auto imu = readIMU(db);
+        auto imu = readIMU(db, last_ts);
-        auto lidar = readLidar(db);
+        for (auto& d : imu) {
+            if (!seen_data) {
+                seen_data = true;
+                first_data_time = chrono::steady_clock::now();
+                cout << "Данные IMU получены, собираю калибровку..." << endl;
+            }
+            sum_ax += d.acc.x();
+            sum_ay += d.acc.y();
+            sum_az += d.acc.z();
+            n++;
+            last_ts = d.t;
+        }
 
-        // Обрабатываем только новые данные
+        if (seen_data) {
-        for (size_t i = max((size_t)1, imu_idx); i < imu.size(); i++) {
+            auto elapsed = chrono::steady_clock::now() - first_data_time;
-            double dt = imu[i].t - imu[i-1].t;
+            if (elapsed >= chrono::seconds(2) && n >= 30) break;
+        } else {
+            auto waited = chrono::steady_clock::now() - start;
+            if (waited >= chrono::seconds(60)) {
+                cout << "IMU не отвечает. Использую стандартный bias." << endl;
+                return;
+            }
+        }
+        this_thread::sleep_for(chrono::milliseconds(100));
+    }
+
+    if (n > 0) {
+        ax_bias = sum_ax / n;
+        ay_bias = sum_ay / n;
+        az_bias = sum_az / n;
+        cout << "Калибровка завершена. Bias: ax=" << ax_bias
+             << "g, ay=" << ay_bias << "g, az=" << az_bias
+             << "g (по " << n << " сэмплам)" << endl;
+        cout << "Bias будет уточняться автоматически во время остановок." << endl;
+    }
+}
+
+// ─────────────────────────────────────────────
+// main
+// ─────────────────────────────────────────────
+int main() {
+    signal(SIGTERM, shutdown);
+    signal(SIGINT,  shutdown);
+
+    sqlite3* db;
+    char exe_path[1024];
+    ssize_t len = readlink("/proc/self/exe", exe_path, sizeof(exe_path)-1);
+    exe_path[len] = '\0';
+    string db_path = string(dirname(exe_path)) + "/../inertial_data.db";
+    sqlite3_open(db_path.c_str(), &db);
+    initDB(db);
+
+    cout << "Расчёты запущены..." << endl;
+    calibrate(db);
+
+    double last_imu_ts   = 0;
+    double last_lidar_ts = 0;
+    {
+        sqlite3_stmt* stmt;
+        sqlite3_prepare_v2(db, "SELECT MAX(timestamp) FROM imu_data",
+                           -1, &stmt, nullptr);
+        if (sqlite3_step(stmt) == SQLITE_ROW)
+            last_imu_ts = sqlite3_column_double(stmt, 0);
+        sqlite3_finalize(stmt);
+
+        sqlite3_prepare_v2(db, "SELECT MAX(timestamp) FROM lidar_data",
+                           -1, &stmt, nullptr);
+        if (sqlite3_step(stmt) == SQLITE_ROW)
+            last_lidar_ts = sqlite3_column_double(stmt, 0);
+        sqlite3_finalize(stmt);
+    }
+
+    vector<Vector3d> prev_scan, curr_scan;
+    vector<LidarData> pending_lidar;
+
+    while (running) {
+        auto imu       = readIMU(db, last_imu_ts);
+        auto new_lidar = readLidar(db, last_lidar_ts);
+        if (!new_lidar.empty()) last_lidar_ts = new_lidar.back().t;
+        pending_lidar.insert(pending_lidar.end(),
+                             new_lidar.begin(), new_lidar.end());
+
+        for (size_t i = 0; i < imu.size(); i++) {
+            double prev_ts = (i == 0) ? last_imu_ts : imu[i-1].t;
+            double dt = imu[i].t - prev_ts;
             if (dt <= 0 || dt > 0.1) continue;
 
-            predict(state, imu[i], dt);
+            predict(imu[i], dt);
 
-            while (lidar_idx < lidar.size() &&
+            while (!pending_lidar.empty() &&
-                   lidar[lidar_idx].t <= imu[i].t) {
+                   pending_lidar.front().t <= imu[i].t) {
-                curr_scan.push_back(lidarToWorld(state, lidar[lidar_idx]));
+                curr_scan.push_back(lidarToWorld(pending_lidar.front()));
-                lidar_idx++;
+                pending_lidar.erase(pending_lidar.begin());
             }
 
             if (curr_scan.size() > 200) {
@@ -294,26 +420,21 @@ int main() {
                     vector<Vector3d> dst = downsample(prev_scan, 5);
                     Matrix3d R; Vector3d t;
                     ICP_fast(src, dst, R, t);
-                    state.x.segment<3>(0) += t;
+                    pos += t;
                 }
-                for (auto& p : curr_scan)
+                for (auto& p : curr_scan) writeLidarPoint(db, p);
-                    writeLidarPoint(db, p);
                 prev_scan = curr_scan;
                 curr_scan.clear();
             }
 
-            Vector3d pos = state.x.segment<3>(0);
+            writeTraj(db, imu[i].t);
-            Vector3d ang = state.x.segment<3>(6);
-            writeTraj(db, imu[i].t, pos, ang);
         }
 
-        imu_idx = imu.size();
+        if (!imu.empty()) last_imu_ts = imu.back().t;
-
-        // Ждём 200 мс перед следующим циклом
         this_thread::sleep_for(chrono::milliseconds(200));
     }
 
     sqlite3_close(db);
     cout << "Расчёты остановлены" << endl;
     return 0;
-}
+}

processing/final_server_auto.py

@@ -0,0 +1,368 @@
+import sqlite3
+import json
+import os
+from http.server import HTTPServer, BaseHTTPRequestHandler
+
+DB_PATH = os.path.expanduser('~/inertial_control/inertial_data.db')
+PORT = 8050
+TRAJ_LIMIT = 5000      # показываем последние 5000 точек траектории (~60 сек движения)
+LIDAR_LIMIT = 2000     # последние точки лидара
+
+HTML = """
+<!DOCTYPE html>
+<html>
+<head>
+    <title>Inertial Tracker — Реальное время</title>
+    <script src="plotly-latest.min.js"></script>
+    <style>
+        * { box-sizing: border-box; }
+        body { margin:0; background:#111827; font-family: 'Courier New', monospace; }
+
+        #topbar {
+            position: fixed; top: 0; left: 0; right: 0; height: 48px;
+            background: #1f2937; border-bottom: 1px solid #374151;
+            display: flex; align-items: center; padding: 0 16px; gap: 24px;
+            z-index: 200;
+        }
+        #topbar .title { color: #f9fafb; font-size: 15px; font-weight: bold; letter-spacing: 1px; }
+        #topbar .badge {
+            padding: 3px 10px; border-radius: 12px; font-size: 12px; font-weight: bold;
+        }
+        .badge-moving  { background: #065f46; color: #6ee7b7; }
+        .badge-still   { background: #1e3a5f; color: #93c5fd; }
+        .badge-nodata  { background: #3f2a2a; color: #fca5a5; }
+        #topbar .dist  { color: #9ca3af; font-size: 13px; }
+        #topbar .dist span { color: #e5e7eb; }
+
+        #coords {
+            position: fixed; top: 60px; right: 12px;
+            background: #1f2937; color: #6ee7b7;
+            padding: 12px 16px; font-size: 17px; z-index: 100;
+            border-radius: 10px; border: 1px solid #374151;
+            line-height: 1.8;
+        }
+        #legend {
+            position: fixed; top: 60px; left: 12px;
+            background: #1f2937; color: #d1d5db;
+            padding: 10px 14px; font-size: 13px; z-index: 100;
+            border-radius: 10px; border: 1px solid #374151;
+            line-height: 2.0;
+        }
+        #hint {
+            position: fixed; bottom: 42px; left: 12px;
+            background: #1f2937; color: #6b7280;
+            padding: 7px 12px; font-size: 11px; z-index: 100;
+            border-radius: 8px; border: 1px solid #374151;
+            max-width: 260px; line-height: 1.5;
+        }
+        #status {
+            position: fixed; bottom: 10px; left: 12px;
+            background: #111827; color: #6b7280;
+            padding: 4px 10px; font-size: 11px; z-index: 100;
+            border-radius: 6px;
+        }
+        #plot { width: 100%; height: 100vh; padding-top: 48px; }
+    </style>
+</head>
+<body>
+    <div id="topbar">
+        <div class="title">INERTIAL TRACKER</div>
+        <div id="motion-badge" class="badge badge-nodata">Нет данных</div>
+        <div class="dist">Пройдено: <span id="dist-val">0.00</span> м</div>
+        <div class="dist">Время: <span id="time-val">0:00</span></div>
+    </div>
+
+    <div id="coords">X: —<br>Y: —<br>Z: —</div>
+
+    <div id="legend">
+        <span style="color:#ffffff; font-size:16px;">◆</span> Начальная точка (0, 0, 0)<br>
+        <span style="color:#f87171; font-size:14px;">━━</span> Траектория движения<br>
+        <span style="color:#60a5fa; font-size:10px;">●●●</span> Карта окружения (лидар)<br>
+        <span style="color:#34d399; font-size:16px;">●</span> Текущее положение
+    </div>
+
+    <div id="hint">
+        Оси X, Y, Z — перемещение в метрах.<br>
+        Масштаб одинаковый по всем осям.<br>
+        Вращайте сцену мышью.
+    </div>
+
+    <div id="status">Подключение...</div>
+    <div id="plot"></div>
+
+    <script>
+        let updateCount = 0;
+        let lastRange = null;
+        let totalDist = 0;
+        let lastPos = null;
+        let startTime = null;
+        let sessionTimer = null;
+
+        // Вычисляет равные диапазоны для всех трёх осей
+        // (одинаковый масштаб = квадратный куб, без растяжки по Z)
+        function squareRanges(allX, allY, allZ) {
+            function minMax(arr) {
+                if (!arr || arr.length === 0) return [0, 0];
+                let mn = arr[0], mx = arr[0];
+                for (let v of arr) { if (v < mn) mn = v; if (v > mx) mx = v; }
+                return [mn, mx];
+            }
+            const rx = minMax(allX), ry = minMax(allY), rz = minMax(allZ);
+            const spans = [rx[1]-rx[0], ry[1]-ry[0], rz[1]-rz[0]];
+            const maxSpan = Math.max(...spans, 0.5); // минимум 0.5 м
+            const half = maxSpan / 2 * 1.2; // запас 20%
+
+            function mid(r) { return (r[0] + r[1]) / 2; }
+            return [
+                [mid(rx) - half, mid(rx) + half],
+                [mid(ry) - half, mid(ry) + half],
+                [mid(rz) - half, mid(rz) + half]
+            ];
+        }
+
+        function rangeChanged(newR, oldR) {
+            if (!oldR) return true;
+            for (let i = 0; i < newR.length; i++) {
+                if (Math.abs(newR[i][0] - oldR[i][0]) > 0.25) return true;
+                if (Math.abs(newR[i][1] - oldR[i][1]) > 0.25) return true;
+            }
+            return false;
+        }
+
+        function fmtTime(sec) {
+            const m = Math.floor(sec / 60), s = Math.floor(sec % 60);
+            return m + ':' + String(s).padStart(2, '0');
+        }
+
+        // Инициализация графика
+        Plotly.newPlot('plot', [
+            {
+                name: 'Начало',
+                x: [0], y: [0], z: [0],
+                mode: 'markers',
+                type: 'scatter3d',
+                marker: { size: 10, color: 'white', symbol: 'diamond',
+                          line: { color: '#374151', width: 2 } }
+            },
+            {
+                name: 'Траектория',
+                x: [], y: [], z: [],
+                mode: 'lines',
+                type: 'scatter3d',
+                line: { color: '#f87171', width: 3 }
+            },
+            {
+                name: 'Лидар',
+                x: [], y: [], z: [],
+                mode: 'markers',
+                type: 'scatter3d',
+                marker: { size: 2, color: '#60a5fa', opacity: 0.5 }
+            },
+            {
+                name: 'Позиция',
+                x: [0], y: [0], z: [0],
+                mode: 'markers',
+                type: 'scatter3d',
+                marker: { size: 12, color: '#34d399', symbol: 'circle',
+                          line: { color: '#6ee7b7', width: 2 } }
+            }
+        ], {
+            scene: {
+                xaxis: { title: 'X (м)', color: '#9ca3af', gridcolor: '#374151', zerolinecolor: '#6b7280' },
+                yaxis: { title: 'Y (м)', color: '#9ca3af', gridcolor: '#374151', zerolinecolor: '#6b7280' },
+                zaxis: { title: 'Z (м)', color: '#9ca3af', gridcolor: '#374151', zerolinecolor: '#6b7280' },
+                bgcolor: '#111827',
+                aspectmode: 'cube',
+                camera: { eye: { x: 1.4, y: 1.4, z: 1.0 } }
+            },
+            paper_bgcolor: '#111827',
+            plot_bgcolor: '#111827',
+            showlegend: false,
+            margin: { l: 0, r: 0, b: 0, t: 0 }
+        });
+
+        async function update() {
+            try {
+                const r = await fetch('/api/data?t=' + Date.now());
+                if (!r.ok) throw new Error('HTTP ' + r.status);
+                const d = await r.json();
+                updateCount++;
+
+                const pos = d.pos;
+                const hasData = d.traj.x.length > 0;
+
+                // Координаты
+                document.getElementById('coords').innerHTML =
+                    `X: ${pos.x.toFixed(3)} м<br>Y: ${pos.y.toFixed(3)} м<br>Z: ${pos.z.toFixed(3)} м`;
+
+                // Таймер сессии
+                if (hasData && !startTime) {
+                    startTime = Date.now();
+                    sessionTimer = setInterval(() => {
+                        document.getElementById('time-val').textContent =
+                            fmtTime((Date.now() - startTime) / 1000);
+                    }, 1000);
+                }
+
+                // Пройденное расстояние
+                if (lastPos && hasData) {
+                    const dx = pos.x - lastPos.x,
+                          dy = pos.y - lastPos.y,
+                          dz = pos.z - lastPos.z;
+                    const step = Math.sqrt(dx*dx + dy*dy + dz*dz);
+                    if (step < 0.5) totalDist += step; // фильтр прыжков
+                }
+                lastPos = pos;
+                document.getElementById('dist-val').textContent = totalDist.toFixed(2);
+
+                // Статус движения
+                const badge = document.getElementById('motion-badge');
+                if (!hasData) {
+                    badge.textContent = 'Нет данных';
+                    badge.className = 'badge badge-nodata';
+                } else {
+                    const spd = lastPos ? Math.sqrt(
+                        (pos.x-lastPos.x)**2 + (pos.y-lastPos.y)**2 + (pos.z-lastPos.z)**2
+                    ) : 0;
+                    if (totalDist > 0.02 && spd > 0.001) {
+                        badge.textContent = '▶ В движении';
+                        badge.className = 'badge badge-moving';
+                    } else {
+                        badge.textContent = '■ В покое';
+                        badge.className = 'badge badge-still';
+                    }
+                }
+
+                // Обновляем трассы
+                Plotly.restyle('plot', { x:[d.traj.x], y:[d.traj.y], z:[d.traj.z] }, 1);
+                Plotly.restyle('plot', { x:[d.lidar.x], y:[d.lidar.y], z:[d.lidar.z] }, 2);
+                Plotly.restyle('plot', { x:[[pos.x]], y:[[pos.y]], z:[[pos.z]] }, 3);
+
+                // Автоподгонка с одинаковым масштабом по всем осям
+                const allX = [0, pos.x].concat(d.traj.x);
+                const allY = [0, pos.y].concat(d.traj.y);
+                const allZ = [0, pos.z].concat(d.traj.z);
+                const newRange = squareRanges(allX, allY, allZ);
+                if (rangeChanged(newRange, lastRange)) {
+                    lastRange = newRange;
+                    Plotly.relayout('plot', {
+                        'scene.xaxis.range': newRange[0],
+                        'scene.yaxis.range': newRange[1],
+                        'scene.zaxis.range': newRange[2]
+                    });
+                }
+
+                document.getElementById('status').textContent =
+                    `Обновлений: ${updateCount} | Траектория: ${d.traj.x.length} тч | Лидар: ${d.lidar.x.length} тч | ${new Date().toLocaleTimeString()}`;
+
+            } catch(e) {
+                document.getElementById('status').textContent = 'Ошибка: ' + e.message;
+            }
+        }
+
+        update();
+        setInterval(update, 200);
+    </script>
+</body>
+</html>
+"""
+
+
+def fetch_data():
+    """Читает свежие данные из БД в read-only режиме (не мешает писателям)."""
+    conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True)
+    try:
+        # Текущая позиция
+        cur = conn.execute(
+            "SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT 1"
+        )
+        row = cur.fetchone()
+        pos = {'x': row[0], 'y': row[1], 'z': row[2]} if row else {'x': 0.0, 'y': 0.0, 'z': 0.0}
+
+        # Последние N точек траектории (разворачиваем в хронологический порядок)
+        cur = conn.execute(
+            f"SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT {TRAJ_LIMIT}"
+        )
+        traj_rows = cur.fetchall()[::-1]
+        traj = {
+            'x': [r[0] for r in traj_rows],
+            'y': [r[1] for r in traj_rows],
+            'z': [r[2] for r in traj_rows],
+        }
+
+        # Последние N точек лидара
+        cur = conn.execute(
+            f"SELECT x, y, z FROM lidar_points ORDER BY rowid DESC LIMIT {LIDAR_LIMIT}"
+        )
+        lidar_rows = cur.fetchall()
+        lidar = {
+            'x': [r[0] for r in lidar_rows],
+            'y': [r[1] for r in lidar_rows],
+            'z': [r[2] for r in lidar_rows],
+        }
+    finally:
+        conn.close()
+    return {'pos': pos, 'traj': traj, 'lidar': lidar}
+
+
+class Handler(BaseHTTPRequestHandler):
+    def log_message(self, format, *args):
+        pass
+
+    def do_GET(self):
+        if self.path == '/':
+            self.send_response(200)
+            self.send_header('Content-type', 'text/html; charset=utf-8')
+            self.end_headers()
+            self.wfile.write(HTML.encode('utf-8'))
+
+        elif self.path == '/plotly-latest.min.js':
+            self.send_response(200)
+            self.send_header('Content-type', 'application/javascript')
+            self.end_headers()
+            with open('plotly-latest.min.js', 'rb') as f:
+                self.wfile.write(f.read())
+
+        elif self.path.startswith('/api/data'):
+            try:
+                data = fetch_data()
+                payload = json.dumps(data).encode('utf-8')
+                self.send_response(200)
+                self.send_header('Content-type', 'application/json')
+                self.send_header('Cache-Control', 'no-store')
+                self.end_headers()
+                self.wfile.write(payload)
+            except Exception as e:
+                self.send_response(500)
+                self.end_headers()
+                self.wfile.write(str(e).encode('utf-8'))
+
+        elif self.path.startswith('/api/pos'):
+            try:
+                conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True)
+                cur = conn.execute(
+                    "SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT 1"
+                )
+                row = cur.fetchone()
+                conn.close()
+                pos = {'x': row[0], 'y': row[1], 'z': row[2]} if row else {'x': 0, 'y': 0, 'z': 0}
+                self.send_response(200)
+                self.send_header('Content-type', 'application/json')
+                self.end_headers()
+                self.wfile.write(json.dumps(pos).encode('utf-8'))
+            except Exception as e:
+                self.send_response(500)
+                self.end_headers()
+                self.wfile.write(str(e).encode('utf-8'))
+
+        else:
+            self.send_response(404)
+            self.end_headers()
+
+
+if __name__ == '__main__':
+    print("=" * 50)
+    print("INERTIAL TRACKER — реальное время")
+    print(f"Открыть: http://192.168.0.106:{PORT}")
+    print("=" * 50)
+    HTTPServer(('0.0.0.0', PORT), Handler).serve_forever()