Update lidar reader (baud 57600), filter_lid with quality, gyro bias calibration

Reviewed-on: #2

imu_module/imu_reader.py

@@ -1,68 +1,153 @@
-import smbus2 #библиотека Python для работы с I2C шиной
+"""
+IMU Reader для MPU-9250.
+Улучшения относительно прежней версии:
+  - реальные timestamp (time.monotonic), а не счётчик
+  - блочное чтение 14 байт за один I2C-запрос
+  - пачечная запись в SQLite (commit раз в 5 семплов = ~50 мс)
+  - WAL-режим для параллельного чтения
+  - устойчивость к временным ошибкам I2C
+  - точное удержание 100 Гц с компенсацией дрейфа цикла
+"""
+import smbus2
 import time
-import csv
+import sqlite3
-import struct
+import signal
+import os
 
-# Константы
+# === Конфигурация ===
 MPU_ADDRESS = 0x68
-I2C_BUS = 1
+I2C_BUS = 2
+DB_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'inertial_data.db')
 
 # Регистры MPU-9250
-PWR_MGMT_1 = 0x6B      # управление питанием
+PWR_MGMT_1 = 0x6B
-ACCEL_XOUT_H = 0x3B    # начало данных акселерометра
+ACCEL_XOUT_H = 0x3B
-GYRO_XOUT_H = 0x43     # начало данных гироскопа
+
+# Масштабные коэффициенты
+ACCEL_SCALE = 16384.0  # LSB/g для диапазона ±2g
+GYRO_SCALE = 131.0     # LSB/(°/с) для диапазона ±250°/с
+
+# Частота и буферизация
+SAMPLE_PERIOD = 0.01   # 10 мс = 100 Гц
+COMMIT_EVERY_N = 5     # коммит раз в 5 семплов (~50 мс)
+
+running = True
+
+
+def shutdown(signum, frame):
+    global running
+    print("Завершение IMU модуля...", flush=True)
+    running = False
+
+
+signal.signal(signal.SIGTERM, shutdown)
+signal.signal(signal.SIGINT, shutdown)
+
+
+def init_db():
+    conn = sqlite3.connect(DB_PATH, timeout=10)
+    conn.execute('PRAGMA journal_mode=WAL')
+    conn.execute('PRAGMA busy_timeout=5000')
+    conn.execute('PRAGMA synchronous=NORMAL')
+    conn.execute('''CREATE TABLE IF NOT EXISTS imu_data (
+        timestamp REAL,
+        ax REAL, ay REAL, az REAL,
+        gx REAL, gy REAL, gz REAL
+    )''')
+    conn.execute('DELETE FROM imu_data')
+    conn.commit()
+    return conn
 
-# Масштабные коэффициенты (диапазон ±2g и ±250°/с)
-ACCEL_SCALE = 16384.0  # LSB/g
-GYRO_SCALE = 131.0     # LSB/(°/с)
 
 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):
+
-    """Читать 16-битное значение из двух регистров"""
+def to_signed(high, low):
-    high = bus.read_byte_data(MPU_ADDRESS, reg)
+    v = (high << 8) | low
-    low = bus.read_byte_data(MPU_ADDRESS, reg + 1)
+    return v - 65536 if v > 32767 else v
-    value = (high << 8) | low
+
-    # Перевод в знаковое число
-    if value > 32767:
-        value -= 65536
-    return value
 
 def read_imu(bus):
-    """Читать и вернуть данные акселерометра и гироскопа"""
+    # Одним запросом читаем 14 байт: ax/ay/az(6) + TEMP(2) + gx/gy/gz(6)
-    ax = read_raw(bus, ACCEL_XOUT_H) / ACCEL_SCALE
+    data = bus.read_i2c_block_data(MPU_ADDRESS, ACCEL_XOUT_H, 14)
-    ay = read_raw(bus, ACCEL_XOUT_H + 2) / ACCEL_SCALE
+    ax = to_signed(data[0], data[1]) / ACCEL_SCALE
-    az = read_raw(bus, ACCEL_XOUT_H + 4) / ACCEL_SCALE
+    ay = to_signed(data[2], data[3]) / ACCEL_SCALE
-
+    az = to_signed(data[4], data[5]) / ACCEL_SCALE
-    gx = read_raw(bus, GYRO_XOUT_H) / GYRO_SCALE
+    # data[6], data[7] — температура, не используем
-    gy = read_raw(bus, GYRO_XOUT_H + 2) / GYRO_SCALE
+    gx = to_signed(data[8], data[9]) / GYRO_SCALE
-    gz = read_raw(bus, GYRO_XOUT_H + 4) / 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_csv(filename, duration_seconds):
+
-    """Записать данные в CSV файл"""
+def main():
+    global running
+    conn = init_db()
     bus = smbus2.SMBus(I2C_BUS)
     init_mpu(bus)
 
-    with open(filename, 'w', newline='') as f:
+    print(f"IMU запущен. Целевая частота: {1/SAMPLE_PERIOD:.0f} Гц", flush=True)
-        writer = csv.writer(f)
-        writer.writerow(['timestamp', 'ax', 'ay', 'az', 'gx', 'gy', 'gz'])
 
-        t = 0.0
+    start_time = time.monotonic()
-        while t < duration_seconds:
+    next_tick = start_time + SAMPLE_PERIOD
-            ax, ay, az, gx, gy, gz = read_imu(bus)
+    batch = []
-            writer.writerow([round(t, 3), 
-                           round(ax, 4), round(ay, 4), round(az, 4),
-                           round(gx, 4), round(gy, 4), round(gz, 4)])
-            t += 0.01
-            time.sleep(0.01)
 
-    bus.close()
+    try:
+        while running:
+            # Спим до следующего тика — без дрейфа цикла
+            now = time.monotonic()
+            sleep_time = next_tick - now
+            if sleep_time > 0:
+                time.sleep(sleep_time)
 
-# Запуск — записываем 5 секунд
+            # Читаем датчик с защитой от случайных ошибок I2C
-save_to_csv('imu.csv', 5)
+            try:
-print("Готово!")
+                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.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 (?, ?, ?, ?, ?, ?, ?)',
+                batch
+            )
+            conn.commit()
+        bus.close()
+        conn.close()
+        print("IMU модуль остановлен", flush=True)
+
+
+if __name__ == '__main__':
+    main()

lidar_module/lidar_reader.py

@@ -1,87 +1,112 @@
 import serial
-import csv
 import time
+import sqlite3
 import struct
+import signal
+import os
 
 # Константы
-SERIAL_PORT = '/dev/ttyS0'  # UART порт на MangoPi
+SERIAL_PORT = '/dev/ttyUSB0'
-BAUD_RATE = 230400          # скорость передачи LDS02RR
+BAUD_RATE = 57600
-DURATION = 5                # секунд записи
+DB_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'inertial_data.db')
 
 # Заголовок пакета LDS02RR
 HEADER = 0xFA
-PACKET_SIZE = 22            # размер пакета в байтах
+PACKET_SIZE = 22
+
+# Глобальные переменные для завершения
+running = True
+conn = None
+ser = None
+
+def shutdown(signum, frame):
+    """Обработчик сигнала завершения"""
+    global running
+    print("Завершение лидар модуля...")
+    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 lidar_data (
+        timestamp REAL,
+        angle REAL,
+        distance_mm REAL,
+        quality INTEGER
+    )''')
+    conn.execute('DELETE FROM lidar_data')
+    conn.commit()
+    return conn
 
 def parse_packet(data):
     """Парсить один пакет данных лидара"""
-    if len(data) < PACKET_SIZE:
+    if len(data) < PACKET_SIZE or data[0] != HEADER:
         return None
-    
-    if data[0] != HEADER:
-        return None
-
-    # Индекс угла (0xA0 = 0°, 0xF9 = 359°)
     index = data[1] - 0xA0
     if index < 0 or index > 89:
         return None
-
-    # Скорость вращения
-    speed = struct.unpack_from('<H', data, 2)[0] / 64.0
-
     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] >> 8
+        b1 = data[offset + 1]
-
+        b2 = data[offset + 2]
-        distance = distance_raw / 4.0  # в мм
+        b3 = data[offset + 3]
-        angle = index * 4 + i          # угол в градусах
+        invalid = (b1 >> 7) & 1
-
+        distance = b0 | ((b1 & 0x3F) << 8)
-        points.append((angle, distance, quality))
+        quality = b2 | (b3 << 8)
-
+        angle = index * 4 + i
+        if not invalid and distance > 0:
+            points.append((angle, distance, quality))
     return points
 
-def save_to_csv(filename, duration_seconds):
+def save_to_db():
-    """Читать данные лидара и записывать в CSV"""
+    """Основной цикл записи данных в БД"""
+    global conn, ser, running
+    conn = init_db()
+
     try:
-        ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
+        ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=0.1)
-        print(f"Подключено к {SERIAL_PORT}")
+        print("Подключено к лидару")
     except Exception as e:
         print(f"Ошибка подключения: {e}")
+        conn.close()
         return
 
-    with open(filename, 'w', newline='') as f:
+    print("Запись данных лидара...")
-        writer = csv.writer(f)
+    buffer = bytearray()
-        writer.writerow(['timestamp', 'angle', 'distance_mm', 'quality'])
+    start_time = time.time()
 
-        start_time = time.time()
+    try:
-        buffer = bytearray()
+        while running:
-
-        while time.time() - start_time < duration_seconds:
             data = ser.read(PACKET_SIZE)
             if not data:
                 continue
-
             buffer.extend(data)
-
-            # Ищем заголовок пакета
             while len(buffer) >= PACKET_SIZE:
                 if buffer[0] == HEADER:
                     packet = bytes(buffer[:PACKET_SIZE])
                     points = parse_packet(packet)
-
                     if points:
-                        t = round(time.time() - start_time, 3)
+                        t = round(time.time(), 3)
                         for angle, distance, quality in points:
-                            writer.writerow([t, angle, 
+                            conn.execute(
-                                           round(distance, 1), 
+                                'INSERT INTO lidar_data VALUES (?, ?, ?, ?)',
-                                           quality])
+                                (t, angle, round(distance, 1), quality)
+                            )
+                        conn.commit()
                     buffer = buffer[PACKET_SIZE:]
                 else:
                     buffer.pop(0)
+    finally:
+        ser.close()
+        conn.close()
+        print("Лидар модуль остановлен")
 
-    ser.close()
+save_to_db()
-    print("Готово!")
-
-# Запуск
-save_to_csv('lidar.csv', DURATION)

processing/calculations

@@ -0,0 +1,6 @@
+#!/bin/bash
+cd /home/grant/inertial_control/processing
+./calc_acc &
+./calc_gyro &
+./calc_lid &
+./calc_main

processing/calculations.cpp

@@ -0,0 +1,440 @@
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <algorithm>
+#include <chrono>
+#include <thread>
+#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; }
+
+struct IMUData   { double t; Vector3d acc; Vector3d gyro; };
+struct LidarData { double t; double angle; double distance; };
+
+// Состояние системы
+Vector3d pos    = Vector3d::Zero();  // позиция (м)
+Vector3d vel    = Vector3d::Zero();  // скорость (м/с)
+Vector3d angles = Vector3d::Zero();  // углы roll, pitch, yaw (рад)
+
+// Bias акселерометра (в единицах g).
+// При калибровке измеряется начальное значение,
+// затем автоматически уточняется в каждую остановку (ZUPT).
+double ax_bias = 0.0;
+double ay_bias = 0.0;
+double az_bias = 1.0;  // ~1g — гравитация по Z когда датчик горизонтальный
+
+// Параметры ZUPT
+const double ZUPT_ACC_THR  = 0.08;  // отклонение нормы от 1g
+const double ZUPT_GYRO_THR = 0.05;  // рад/с (~3 °/с)
+const int    ZUPT_WINDOW   = 10;    // сэмплов подряд (~120 мс)
+
+// Коэффициент комплементарного фильтра
+// 0.98 = 98% гироскоп (быстро, но дрейфует)
+// 0.02 = 2%  акселерометр (медленно, но абсолютное значение)
+const double CF_ALPHA = 0.98;
+
+// ─────────────────────────────────────────────
+// Чтение БД
+// ─────────────────────────────────────────────
+vector<IMUData> readIMU(sqlite3* db, double after_ts) {
+    vector<IMUData> data;
+    sqlite3_stmt* stmt;
+    sqlite3_prepare_v2(db,
+        "SELECT timestamp, ax, ay, az, gx, gy, gz "
+        "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.acc.x()  = sqlite3_column_double(stmt, 1);
+        d.acc.y()  = sqlite3_column_double(stmt, 2);
+        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, double after_ts) {
+    vector<LidarData> data;
+    sqlite3_stmt* stmt;
+    sqlite3_prepare_v2(db,
+        "SELECT timestamp, angle, distance_mm "
+        "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.angle    = sqlite3_column_double(stmt, 1);
+        d.distance = sqlite3_column_double(stmt, 2);
+        data.push_back(d);
+    }
+    sqlite3_finalize(stmt);
+    return data;
+}
+
+// ─────────────────────────────────────────────
+// Запись в БД
+// ─────────────────────────────────────────────
+void initDB(sqlite3* db) {
+    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 (x REAL, y REAL, z REAL);"
+        "DELETE FROM lidar_points;",
+        nullptr, nullptr, nullptr);
+}
+
+void writeTraj(sqlite3* db, double t) {
+    sqlite3_stmt* stmt;
+    sqlite3_prepare_v2(db,
+        "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, angles.x());
+    sqlite3_bind_double(stmt, 6, angles.y());
+    sqlite3_bind_double(stmt, 7, angles.z());
+    sqlite3_step(stmt);
+    sqlite3_finalize(stmt);
+}
+
+void writeLidarPoint(sqlite3* db, Vector3d p) {
+    sqlite3_stmt* stmt;
+    sqlite3_prepare_v2(db, "INSERT INTO lidar_points VALUES (?,?,?)",
+                       -1, &stmt, nullptr);
+    sqlite3_bind_double(stmt, 1, p.x());
+    sqlite3_bind_double(stmt, 2, p.y());
+    sqlite3_bind_double(stmt, 3, p.z());
+    sqlite3_step(stmt);
+    sqlite3_finalize(stmt);
+}
+
+// ─────────────────────────────────────────────
+// Матрица поворота (roll, pitch, yaw)
+// ─────────────────────────────────────────────
+Matrix3d rotationMatrix(double roll, double pitch, double yaw) {
+    AngleAxisd rx(roll,  Vector3d::UnitX());
+    AngleAxisd ry(pitch, Vector3d::UnitY());
+    AngleAxisd rz(yaw,   Vector3d::UnitZ());
+    return (rz * ry * rx).toRotationMatrix();
+}
+
+Vector3d lidarToWorld(const LidarData& l) {
+    double angle = l.angle * M_PI / 180.0;
+    double dist  = l.distance / 1000.0;
+    Vector3d local(dist * cos(angle), dist * sin(angle), 0);
+    return pos + rotationMatrix(angles[0], angles[1], angles[2]) * local;
+}
+
+// ─────────────────────────────────────────────
+// 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;
+
+    // ── Комплементарный фильтр ориентации ──────────────
+    // Углы наклона из акселерометра (абсолютные, но шумные)
+    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()));
+
+    // Комбинируем: быстрая часть (гироскоп) + медленная коррекция (акселерометр)
+    // φ = α·(φ + ω·T) + (1-α)·φ_акселерометр
+    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 — только гироскоп (нет абсолютного источника)
+
+    // ── Линейное ускорение в мировой системе ───────────
+    // Шаг 1: вычитаем bias датчика и переводим в м/с²
+    Vector3d acc_sensor(
+        (imu.acc.x() - ax_bias) * 9.81,
+        (imu.acc.y() - ay_bias) * 9.81,
+        imu.acc.z() * 9.81   // Z: bias не вычитаем, гравитация убирается через матрицу поворота
+    );
+
+    // Шаг 2: поворачиваем в мировую систему координат
+    Matrix3d R = rotationMatrix(angles.x(), angles.y(), angles.z());
+    Vector3d acc_world = R * acc_sensor;
+
+    // Шаг 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;
+    }
+}
+
+// ─────────────────────────────────────────────
+// ICP — сопоставление сканов лидара
+// ─────────────────────────────────────────────
+struct KDNode {
+    Vector3d point; KDNode* left; KDNode* right; int axis;
+    KDNode(Vector3d p, int ax) : point(p), left(nullptr), right(nullptr), axis(ax) {}
+};
+
+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]; });
+    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->right = buildKDTree(right, depth + 1);
+    return node;
+}
+
+void nearestSearch(KDNode* node, const Vector3d& target,
+                   Vector3d& best, double& best_dist) {
+    if (!node) return;
+    double d = (node->point - target).squaredNorm();
+    if (d < best_dist) { best_dist = d; best = node->point; }
+    KDNode* near_ = target[node->axis] < node->point[node->axis] ? node->left  : node->right;
+    KDNode* far_  = target[node->axis] < node->point[node->axis] ? node->right : node->left;
+    nearestSearch(near_, target, best, best_dist);
+    double diff = target[node->axis] - node->point[node->axis];
+    if (diff * diff < best_dist) nearestSearch(far_, target, best, best_dist);
+}
+
+Matrix3d computeRotation(const vector<Vector3d>& src, const vector<Vector3d>& dst) {
+    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();
+    Matrix3d H = Matrix3d::Zero();
+    for (size_t i = 0; i < src.size(); i++)
+        H += (src[i] - c1) * (dst[i] - c2).transpose();
+    JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
+    return svd.matrixV() * svd.matrixU().transpose();
+}
+
+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();
+    return c2 - R * c1;
+}
+
+vector<Vector3d> downsample(const vector<Vector3d>& pts, int step = 5) {
+    vector<Vector3d> out;
+    for (size_t i = 0; i < pts.size(); i += step) out.push_back(pts[i]);
+    return out;
+}
+
+void ICP_fast(vector<Vector3d>& src, vector<Vector3d>& dst,
+              Matrix3d& R, Vector3d& t) {
+    R = Matrix3d::Identity(); t = Vector3d::Zero();
+    vector<Vector3d> dst_copy = dst;
+    KDNode* tree = buildKDTree(dst_copy);
+    for (int iter = 0; iter < 10; iter++) {
+        vector<Vector3d> matched;
+        for (auto& p : src) {
+            Vector3d best; double best_dist = 1e9;
+            nearestSearch(tree, p, best, best_dist);
+            if (best_dist < 0.5) matched.push_back(best);
+        }
+        if (matched.size() < 10) break;
+        Matrix3d dR = computeRotation(src, matched);
+        Vector3d dt = computeTranslation(src, matched, dR);
+        for (auto& p : src) p = dR * p + dt;
+        R = dR * R;
+        t = dR * t + dt;
+    }
+}
+
+// ─────────────────────────────────────────────
+// Калибровка: датчик лежит горизонтально, неподвижно.
+// Измеряем начальный bias по всем трём осям.
+// ─────────────────────────────────────────────
+void calibrate(sqlite3* db) {
+    cout << "Калибровка (держите датчик горизонтально, неподвижно)..." << endl;
+
+    double sum_ax = 0, sum_ay = 0, sum_az = 0;
+    int n = 0;
+    double last_ts = 0;
+    bool seen_data = false;
+    auto start = chrono::steady_clock::now();
+    auto first_data_time = chrono::steady_clock::time_point();
+
+    while (running) {
+        auto imu = readIMU(db, last_ts);
+        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) {
+            auto elapsed = chrono::steady_clock::now() - first_data_time;
+            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(imu[i], dt);
+
+            while (!pending_lidar.empty() &&
+                   pending_lidar.front().t <= imu[i].t) {
+                curr_scan.push_back(lidarToWorld(pending_lidar.front()));
+                pending_lidar.erase(pending_lidar.begin());
+            }
+
+            if (curr_scan.size() > 200) {
+                if (!prev_scan.empty()) {
+                    vector<Vector3d> src = downsample(curr_scan, 5);
+                    vector<Vector3d> dst = downsample(prev_scan, 5);
+                    Matrix3d R; Vector3d t;
+                    ICP_fast(src, dst, R, t);
+                    pos += t;
+                }
+                for (auto& p : curr_scan) writeLidarPoint(db, p);
+                prev_scan = curr_scan;
+                curr_scan.clear();
+            }
+
+            writeTraj(db, imu[i].t);
+        }
+
+        if (!imu.empty()) last_imu_ts = imu.back().t;
+        this_thread::sleep_for(chrono::milliseconds(200));
+    }
+
+    sqlite3_close(db);
+    cout << "Расчёты остановлены" << endl;
+    return 0;
+}

processing/filter_lid.cpp

@@ -0,0 +1,149 @@
+#define _USE_MATH_DEFINES
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <unistd.h>
+#include "sqlite3.h"
+
+using namespace std;
+
+// ------------------- НАСТРОЙКИ ФИЛЬТРАЦИИ -------------------
+static const double MIN_DIST_MM = 50.0;      // минимальное расстояние (мм)
+static const double MAX_DIST_MM = 16000.0;   // максимальное расстояние (мм)
+static const int    MIN_QUALITY = 500;       // минимальное качество сигнала
+// -----------------------------------------------------------
+
+struct LidarPoint {
+    double timestamp;
+    double angle;        // градусы
+    double distance_mm;
+    int    quality;
+};
+
+// Чтение последнего обработанного timestamp из lidar_points
+double readLastTimestamp(sqlite3* db) {
+    double last_ts = 0.0;
+    sqlite3_stmt* stmt = nullptr;
+    if (sqlite3_prepare_v2(db, "SELECT MAX(timestamp) FROM lidar_points", -1, &stmt, nullptr) == SQLITE_OK) {
+        if (sqlite3_step(stmt) == SQLITE_ROW && sqlite3_column_type(stmt, 0) != SQLITE_NULL)
+            last_ts = sqlite3_column_double(stmt, 0);
+    }
+    sqlite3_finalize(stmt);
+    return last_ts;
+}
+
+// Чтение новых сырых данных лидара из БД после last_timestamp
+vector<LidarPoint> readLidar(sqlite3* db, double last_timestamp) {
+    vector<LidarPoint> data;
+    sqlite3_stmt* stmt = nullptr;
+    const char* sql = "SELECT timestamp, angle, distance_mm, quality FROM lidar_data WHERE timestamp > ? ORDER BY timestamp ASC";
+    if (sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr) != SQLITE_OK) {
+        cerr << "Ошибка подготовки запроса: " << sqlite3_errmsg(db) << endl;
+        return data;
+    }
+    sqlite3_bind_double(stmt, 1, last_timestamp);
+    while (sqlite3_step(stmt) == SQLITE_ROW) {
+        LidarPoint p;
+        p.timestamp = sqlite3_column_double(stmt, 0);
+        p.angle = sqlite3_column_double(stmt, 1);
+        p.distance_mm = sqlite3_column_double(stmt, 2);
+        p.quality = sqlite3_column_int(stmt, 3);
+        data.push_back(p);
+    }
+    sqlite3_finalize(stmt);
+    return data;
+}
+
+int main() {
+    // 1. Открываем базу данных
+    sqlite3* db = nullptr;
+    if (sqlite3_open("/home/grant/inertial_control/inertial_data.db", &db) != SQLITE_OK) {
+        cerr << "Не удалось открыть БД: " << sqlite3_errmsg(db) << endl;
+        return 1;
+    }
+
+    sqlite3_busy_timeout(db, 5000);
+
+    // 2. Включаем WAL и синхронизацию (для производительности)
+    char* errMsg = nullptr;
+    sqlite3_exec(db, "PRAGMA journal_mode=WAL;", nullptr, nullptr, &errMsg);
+    sqlite3_exec(db, "PRAGMA synchronous=NORMAL;", nullptr, nullptr, &errMsg);
+
+    // 3. Создаём таблицу lidar_points один раз при старте
+    sqlite3_exec(db,
+        "CREATE TABLE IF NOT EXISTS lidar_points ("
+        "timestamp REAL, angle REAL, distance_mm REAL, "
+        "x REAL, y REAL, quality INTEGER)",
+        nullptr, nullptr, &errMsg);
+
+    // 4. Подготавливаем INSERT
+    sqlite3_stmt* insStmt;
+    const char* insertSql = "INSERT INTO lidar_points (timestamp, angle, distance_mm, x, y, quality) VALUES (?,?,?,?,?,?)";
+    if (sqlite3_prepare_v2(db, insertSql, -1, &insStmt, nullptr) != SQLITE_OK) {
+        cerr << "Ошибка подготовки INSERT: " << sqlite3_errmsg(db) << endl;
+        sqlite3_close(db);
+        return 1;
+    }
+
+    // 5. Бесконечный цикл — читаем только новые данные каждые 3 секунды
+    while (true) {
+        double last_ts = readLastTimestamp(db);
+        vector<LidarPoint> raw = readLidar(db, last_ts);
+
+        if (raw.empty()) {
+            sleep(3);
+            continue;
+        }
+
+        cout << "Новых точек из lidar_data: " << raw.size() << endl;
+
+        int written = 0;
+        int filtered = 0;
+
+        sqlite3_exec(db, "BEGIN TRANSACTION;", nullptr, nullptr, &errMsg);
+
+        for (const auto& p : raw) {
+            // Фильтрация по расстоянию и качеству
+            if (p.distance_mm < MIN_DIST_MM) { filtered++; continue; }
+            if (p.distance_mm > MAX_DIST_MM) { filtered++; continue; }
+            if (p.quality < MIN_QUALITY) { filtered++; continue; }
+
+            // Пересчёт в декартовы координаты (метры)
+            double angle_rad = p.angle * M_PI / 180.0;
+            double dist_m = p.distance_mm / 1000.0;
+            double x = dist_m * cos(angle_rad);
+            double y = dist_m * sin(angle_rad);
+
+            // Привязываем значения
+            sqlite3_reset(insStmt);
+            sqlite3_bind_double(insStmt, 1, p.timestamp);
+            sqlite3_bind_double(insStmt, 2, p.angle);
+            sqlite3_bind_double(insStmt, 3, p.distance_mm);
+            sqlite3_bind_double(insStmt, 4, x);
+            sqlite3_bind_double(insStmt, 5, y);
+            sqlite3_bind_int(insStmt, 6, p.quality);
+
+            // Выполняем вставку
+            int stepRes = sqlite3_step(insStmt);
+            if (stepRes == SQLITE_DONE) {
+                written++;
+            }
+            else {
+                cerr << "Ошибка вставки: " << sqlite3_errmsg(db) << endl;
+            }
+        }
+
+        sqlite3_exec(db, "COMMIT;", nullptr, nullptr, &errMsg);
+
+        cout << "=====================================" << endl;
+        cout << "Фильтрация: отброшено " << filtered << " точек" << endl;
+        cout << "Записано в lidar_points: " << written << " точек" << endl;
+        cout << "=====================================" << endl;
+
+        sleep(3);
+    }
+
+    sqlite3_finalize(insStmt);
+    sqlite3_close(db);
+    return 0;
+}

processing/final_server_auto.py

@@ -0,0 +1,453 @@
+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
+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:#0f0f0f; font-family: 'Courier New', monospace; }
+
+        #topbar {
+            position: fixed; top: 0; left: 0; right: 0; height: 48px;
+            background: #181818; border-bottom: 1px solid #2a2a2a;
+            display: flex; align-items: center; padding: 0 16px; gap: 24px;
+            z-index: 200;
+        }
+        #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: #181818; color: #6ee7b7;
+            padding: 12px 16px; font-size: 17px; z-index: 100;
+            border-radius: 10px; border: 1px solid #2a2a2a;
+            line-height: 1.8;
+        }
+        #legend {
+            position: fixed; top: 60px; left: 12px;
+            background: #181818; color: #d1d5db;
+            padding: 10px 14px; font-size: 13px; z-index: 100;
+            border-radius: 10px; border: 1px solid #2a2a2a;
+            line-height: 2.0;
+        }
+        #status {
+            position: fixed; bottom: 6px; left: 12px;
+            background: #0f0f0f; color: #4b5563;
+            padding: 4px 10px; font-size: 11px; z-index: 100;
+            border-radius: 6px;
+        }
+
+        #main-wrap {
+            position: fixed; top: 48px; left: 0; right: 0; bottom: 0;
+            display: flex; flex-direction: column;
+        }
+        #plot3d { flex: 0 0 62%; width: 100%; }
+        #plots2d {
+            flex: 1 1 0; display: flex; flex-direction: row;
+            border-top: 1px solid #2a2a2a;
+        }
+        .plot2d { flex: 1 1 0; min-width: 0; }
+        .plot2d:not(:last-child) { border-right: 1px solid #2a2a2a; }
+    </style>
+</head>
+<body>
+    <div id="topbar">
+        <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:#ffffff;">●</span> Положение робота<br>
+        <span style="color:#fbbf24;">━━</span> Направление (жёлтый луч)
+    </div>
+
+    <div id="status">Подключение...</div>
+
+    <div id="main-wrap">
+        <div id="plot3d"></div>
+        <div id="plots2d">
+            <div id="plot-xy" class="plot2d"></div>
+            <div id="plot-yz" class="plot2d"></div>
+            <div id="plot-zx" class="plot2d"></div>
+        </div>
+    </div>
+
+    <script>
+        let updateCount = 0;
+        let lastRange   = null;
+        let totalDist   = 0;
+        let lastPos     = null;
+        let startTime   = null;
+        let sessionTimer = null;
+
+        const BG = '#0f0f0f';
+        const GRID = '#1e1e1e';
+        const ZERO = '#2a2a2a';
+        const AXIS_COLOR = '#555';
+        const ARROW_LEN = 0.5;   // длина жёлтого луча (м)
+
+        // ── Матрица полного поворота ─────────────────────────────────────────
+        function rotFull(roll, pitch, yaw) {
+            const cr=Math.cos(roll), sr=Math.sin(roll);
+            const cp=Math.cos(pitch), sp=Math.sin(pitch);
+            const cy=Math.cos(yaw),  sy=Math.sin(yaw);
+            return [
+                [cy*cp,  cy*sp*sr-sy*cr,  cy*sp*cr+sy*sr],
+                [sy*cp,  sy*sp*sr+cy*cr,  sy*sp*cr-cy*sr],
+                [-sp,    cp*sr,            cp*cr]
+            ];
+        }
+
+        // Применение матрицы поворота к вектору
+        function applyRot(R, v) {
+            return [
+                R[0][0]*v[0] + R[0][1]*v[1] + R[0][2]*v[2],
+                R[1][0]*v[0] + R[1][1]*v[1] + R[1][2]*v[2],
+                R[2][0]*v[0] + R[2][1]*v[1] + R[2][2]*v[2]
+            ];
+        }
+
+        // ── Вспомогательные функции ──────────────────────────────────────────
+        function minMax(arr) {
+            if (!arr || !arr.length) return [0,0];
+            let mn=arr[0], mx=arr[0];
+            for (const v of arr) { if(v<mn)mn=v; if(v>mx)mx=v; }
+            return [mn, mx];
+        }
+        function squareRanges(ax, ay, az) {
+            const rx=minMax(ax), ry=minMax(ay), rz=minMax(az);
+            const maxSpan=Math.max(rx[1]-rx[0],ry[1]-ry[0],rz[1]-rz[0],0.5);
+            const half=maxSpan/2*1.2;
+            const mid=r=>(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(nR, oR) {
+            if (!oR) return true;
+            for (let i=0;i<nR.length;i++) {
+                if(Math.abs(nR[i][0]-oR[i][0])>0.25||Math.abs(nR[i][1]-oR[i][1])>0.25) return true;
+            }
+            return false;
+        }
+        function square2D(ax, ay) {
+            const rx=minMax(ax), ry=minMax(ay);
+            const maxSpan=Math.max(rx[1]-rx[0],ry[1]-ry[0],0.5);
+            const half=maxSpan/2*1.2;
+            const mid=r=>(r[0]+r[1])/2;
+            return [[mid(rx)-half,mid(rx)+half],[mid(ry)-half,mid(ry)+half]];
+        }
+        function fmtTime(sec) {
+            const m=Math.floor(sec/60), s=Math.floor(sec%60);
+            return m+':'+String(s).padStart(2,'0');
+        }
+
+        const darkAxis3D = label => ({
+            title:label, color:AXIS_COLOR,
+            gridcolor:GRID, zerolinecolor:ZERO,
+            backgroundcolor: BG, showbackground: true
+        });
+        const darkAxis2D = label => ({
+            title:label, color:AXIS_COLOR,
+            gridcolor:GRID, zerolinecolor:ZERO,
+            tickfont:{color:AXIS_COLOR}
+        });
+        const darkLayout2D = (xL, yL) => ({
+            xaxis: darkAxis2D(xL), yaxis: darkAxis2D(yL),
+            paper_bgcolor: BG, plot_bgcolor: BG,
+            showlegend: false,
+            margin:{l:44,r:10,b:36,t:24},
+            font:{color:AXIS_COLOR, size:11}
+        });
+
+        // ── Инициализация 3D-графика ─────────────────────────────────────────
+        // Трейсы:
+        // 0 – начальная точка (белый ромб)
+        // 1 – траектория (красная линия)
+        // 2 – лидар (голубые точки)
+        // 3 – кружок – текущее положение (белый круг)
+        // 4 – жёлтый луч направления (линия)
+        Plotly.newPlot('plot3d', [
+            {
+                name:'Начало', x:[0],y:[0],z:[0],
+                mode:'markers', type:'scatter3d',
+                marker:{size:10,color:'white',symbol:'diamond',line:{color:'#2a2a2a',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:'white',symbol:'circle',line:{color:'#fbbf24',width:2}}
+            },
+            {
+                name:'Направление', x:[0,0], y:[0,0], z:[0,0],
+                mode:'lines', type:'scatter3d',
+                line:{color:'#fbbf24',width:4},
+                hoverinfo:'none'
+            }
+        ], {
+            scene: {
+                xaxis: darkAxis3D('X (м)'),
+                yaxis: darkAxis3D('Y (м)'),
+                zaxis: darkAxis3D('Z (м)'),
+                bgcolor: BG,
+                aspectmode: 'cube',
+                camera: {eye:{x:1.4,y:1.4,z:1.0}}
+            },
+            paper_bgcolor: BG,
+            plot_bgcolor:  BG,
+            showlegend: false,
+            margin:{l:0,r:0,b:0,t:0}
+        }, {responsive:true});
+
+        // ── 2D проекции ───────────────────────────────────────────────────────
+        function init2D(divId, xL, yL) {
+            Plotly.newPlot(divId, [
+                { x:[0],y:[0], mode:'markers', type:'scatter',
+                  marker:{size:8,color:'white',symbol:'diamond',line:{color:'#2a2a2a',width:1}} },
+                { x:[],y:[], mode:'lines', type:'scatter',
+                  line:{color:'#f87171',width:2} },
+                { x:[0],y:[0], mode:'markers', type:'scatter',
+                  marker:{size:10,color:'#34d399',line:{color:'#6ee7b7',width:2}} }
+            ], darkLayout2D(xL,yL), {responsive:true});
+        }
+        init2D('plot-xy','X (м)','Y (м)');
+        init2D('plot-yz','Y (м)','Z (м)');
+        init2D('plot-zx','Z (м)','X (м)');
+
+        // ── Главный цикл обновления ────────────────────────────────────────────
+        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 if (totalDist>0.02) {
+                    badge.textContent='▶ В движении'; badge.className='badge badge-moving';
+                } else {
+                    badge.textContent='■ В покое'; badge.className='badge badge-still';
+                }
+
+                // Обновляем траекторию и лидар
+                Plotly.restyle('plot3d',{x:[d.traj.x],y:[d.traj.y],z:[d.traj.z]},1);
+                Plotly.restyle('plot3d',{x:[d.lidar.x],y:[d.lidar.y],z:[d.lidar.z]},2);
+
+                // ── Кружок (текущее положение) ────────────────────────────────
+                Plotly.restyle('plot3d',{x:[pos.x], y:[pos.y], z:[pos.z]}, 3);
+
+                // ── Жёлтый луч направления ────────────────────────────────────
+                const roll  = d.orient.roll;
+                const pitch = d.orient.pitch;
+                const yaw   = d.orient.yaw;
+                const R = rotFull(roll, pitch, yaw);
+                const dir = applyRot(R, [ARROW_LEN, 0, 0]);   // вектор вперёд
+                const endX = pos.x + dir[0];
+                const endY = pos.y + dir[1];
+                const endZ = pos.z + dir[2];
+                Plotly.restyle('plot3d',{
+                    x:[[pos.x, endX]], y:[[pos.y, endY]], z:[[pos.z, endZ]]
+                }, 4);
+
+                // Автомасштаб 3D
+                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('plot3d',{
+                        'scene.xaxis.range':newRange[0],
+                        'scene.yaxis.range':newRange[1],
+                        'scene.zaxis.range':newRange[2]
+                    });
+                }
+
+                // 2D проекции
+                const tx=d.traj.x, ty=d.traj.y, tz=d.traj.z;
+
+                Plotly.restyle('plot-xy',{x:[tx],y:[ty]},1);
+                Plotly.restyle('plot-xy',{x:[[pos.x]],y:[[pos.y]]},2);
+                const rXY=square2D([0,pos.x].concat(tx),[0,pos.y].concat(ty));
+                Plotly.relayout('plot-xy',{'xaxis.range':rXY[0],'yaxis.range':rXY[1]});
+
+                Plotly.restyle('plot-yz',{x:[ty],y:[tz]},1);
+                Plotly.restyle('plot-yz',{x:[[pos.y]],y:[[pos.z]]},2);
+                const rYZ=square2D([0,pos.y].concat(ty),[0,pos.z].concat(tz));
+                Plotly.relayout('plot-yz',{'xaxis.range':rYZ[0],'yaxis.range':rYZ[1]});
+
+                Plotly.restyle('plot-zx',{x:[tz],y:[tx]},1);
+                Plotly.restyle('plot-zx',{x:[[pos.z]],y:[[pos.x]]},2);
+                const rZX=square2D([0,pos.z].concat(tz),[0,pos.x].concat(tx));
+                Plotly.relayout('plot-zx',{'xaxis.range':rZX[0],'yaxis.range':rZX[1]});
+
+                document.getElementById('status').textContent =
+                    `Обновлений: ${updateCount} | Traj: ${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():
+    conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True)
+    try:
+        cur = conn.execute(
+            "SELECT x, y, z, roll, pitch, yaw FROM trajectory ORDER BY timestamp DESC LIMIT 1"
+        )
+        row = cur.fetchone()
+        if row:
+            pos    = {'x': row[0], 'y': row[1], 'z': row[2]}
+            orient = {'roll': row[3] or 0.0, 'pitch': row[4] or 0.0, 'yaw': row[5] or 0.0}
+        else:
+            pos    = {'x': 0.0, 'y': 0.0, 'z': 0.0}
+            orient = {'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}
+
+        cur = conn.execute(
+            f"SELECT x, y, z FROM trajectory ORDER BY timestamp ASC 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],
+        }
+
+        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, 'orient': orient, '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()

processing/simple_filter_acc.cpp

@@ -0,0 +1,159 @@
+#include <iostream>
+#include <vector>
+#include <unistd.h>
+#include "sqlite3.h"
+
+using namespace std;
+
+struct ImuSample {
+    double timestamp;
+    double ax, ay, az;   // акселерометр (g)
+    double gx, gy, gz;   // гироскоп (°/с)
+};
+
+// Чтение новых строк из таблицы imu_data после last_timestamp
+vector<ImuSample> readAllImuData(sqlite3* db, double last_timestamp) {
+    vector<ImuSample> samples;
+    const char* sql = "SELECT timestamp, ax, ay, az, gx, gy, gz FROM imu_data WHERE timestamp > ? ORDER BY timestamp ASC";
+
+    sqlite3_stmt* stmt = nullptr;
+    if (sqlite3_prepare_v2(db, sql, -1, & stmt, nullptr) != SQLITE_OK) {
+        std::cerr << "Ошибка подготовки запроса: " << sqlite3_errmsg(db) << std::endl;
+        return samples;
+    }
+    sqlite3_bind_double(stmt, 1, last_timestamp);
+
+    while (sqlite3_step(stmt) == SQLITE_ROW) {
+        ImuSample s;
+        s.timestamp = sqlite3_column_double(stmt, 0);
+        s.ax = sqlite3_column_double(stmt, 1);
+        s.ay = sqlite3_column_double(stmt, 2);
+        s.az = sqlite3_column_double(stmt, 3);
+        s.gx = sqlite3_column_double(stmt, 4);
+        s.gy = sqlite3_column_double(stmt, 5);
+        s.gz = sqlite3_column_double(stmt, 6);
+        samples.push_back(s);
+    }
+
+    sqlite3_finalize(stmt);
+    return samples;
+}
+
+// Чтение последнего обработанного timestamp из filtered_acc_data
+double readLastTimestamp(sqlite3* db) {
+    double last_ts = 0.0;
+    sqlite3_stmt* stmt = nullptr;
+    if (sqlite3_prepare_v2(db, "SELECT MAX(timestamp) FROM filtered_acc_data", -1, &stmt, nullptr) == SQLITE_OK) {
+        if (sqlite3_step(stmt) == SQLITE_ROW && sqlite3_column_type(stmt, 0) != SQLITE_NULL)
+            last_ts = sqlite3_column_double(stmt, 0);
+    }
+    sqlite3_finalize(stmt);
+    return last_ts;
+}
+
+// Запись одной строки (timestamp, ax, ay, az) в таблицу filtered_acc_data
+void writeFilteredAccData(sqlite3* db, double timestamp, double ax, double ay, double az) {
+    // 1. Создаём таблицу, если она не существует
+    const char* create_sql =
+        "CREATE TABLE IF NOT EXISTS filtered_acc_data ("
+        "timestamp REAL, "
+        "ax REAL, "
+        "ay REAL, "
+        "az REAL)";
+    char* err_msg = nullptr;
+    if (sqlite3_exec(db, create_sql, nullptr, nullptr, &err_msg) != SQLITE_OK) {
+        std::cerr << "Ошибка создания таблицы: " << err_msg << std::endl;
+        sqlite3_free(err_msg);
+        return;
+    }
+
+    // 2. Подготавливаем INSERT
+    const char* insert_sql = "INSERT INTO filtered_acc_data (timestamp, ax, ay, az) VALUES (?, ?, ?, ?)";
+    sqlite3_stmt* stmt = nullptr;
+    if (sqlite3_prepare_v2(db, insert_sql, -1, &stmt, nullptr) != SQLITE_OK) {
+        std::cerr << "Ошибка подготовки запроса: " << sqlite3_errmsg(db) << std::endl;
+        return;
+    }
+
+    // 3. Привязываем значения
+    sqlite3_bind_double(stmt, 1, timestamp);
+    sqlite3_bind_double(stmt, 2, ax);
+    sqlite3_bind_double(stmt, 3, ay);
+    sqlite3_bind_double(stmt, 4, az);
+
+    // 4. Выполняем
+    if (sqlite3_step(stmt) != SQLITE_DONE) {
+        std::cerr << "Ошибка вставки данных: " << sqlite3_errmsg(db) << std::endl;
+    }
+
+    // 5. Очищаем
+    sqlite3_finalize(stmt);
+}
+
+
+int main() {
+    sqlite3* db = nullptr;
+
+    if (sqlite3_open("/home/grant/inertial_control/inertial_data.db", &db) != SQLITE_OK) {
+        cerr << "Не удалось открыть БД" << endl;
+        return 1;
+    }
+
+    sqlite3_busy_timeout(db, 5000);
+
+    char* errMsg = nullptr;
+    if (sqlite3_exec(db, "PRAGMA journal_mode=WAL;", nullptr, nullptr, &errMsg) != SQLITE_OK) {
+        cerr << "Ошибка установки WAL режима: " << errMsg << std::endl;
+        sqlite3_free(errMsg);
+    }
+    else {
+        cout << "WAL режим успешно включен" << endl;
+    }
+
+    // расчет bias
+    double bias_x = 0.0, bias_y = 0.0, bias_z = 0.0;
+    const int WIN_SIZE = 4;
+
+    // Бесконечный цикл — читаем только новые данные каждые 3 секунды
+    while (true) {
+        double last_ts = readLastTimestamp(db);
+        auto all = readAllImuData(db, last_ts);
+
+        if (all.empty()) {
+            sleep(3);
+            continue;
+        }
+
+        cout << "Новых строк: " << all.size() << "\n";
+
+        // заполнения первичных значений
+        double acc_xf = 0.0, acc_yf = 0.0, acc_zf = 0.0;
+        for (size_t i = 0; i < WIN_SIZE && i < all.size(); ++i) {
+            acc_xf += all[i].ax;
+            acc_yf += all[i].ay;
+            acc_zf += all[i].az;
+        }
+
+        // основной цикл сбора - расчета - записи
+        sqlite3_exec(db, "BEGIN;", nullptr, nullptr, nullptr);
+        for (size_t i = WIN_SIZE; i < all.size(); ++i) {
+            writeFilteredAccData(db, all[i].timestamp,
+                acc_xf / WIN_SIZE - bias_x,
+                acc_yf / WIN_SIZE - bias_y,
+                acc_zf / WIN_SIZE - bias_z);
+            acc_xf -= all[i - WIN_SIZE].ax;
+            acc_xf += all[i].ax;
+            acc_yf -= all[i - WIN_SIZE].ay;
+            acc_yf += all[i].ay;
+            acc_zf -= all[i - WIN_SIZE].az;
+            acc_zf += all[i].az;
+        }
+        sqlite3_exec(db, "COMMIT;", nullptr, nullptr, nullptr);
+
+        cout << "записано в filtered_acc_data\n";
+        sleep(3);
+    }
+
+    sqlite3_close(db);
+    return 0;
+}

processing/simple_filter_gyro.cpp

@@ -0,0 +1,180 @@
+#include <iostream>
+#include <vector>
+#include <unistd.h>
+#include "sqlite3.h"
+
+using namespace std;
+
+struct ImuSample {
+    double timestamp;
+    double ax, ay, az;   // акселерометр (g)
+    double gx, gy, gz;   // гироскоп (°/с)
+};
+
+// Чтение новых строк из таблицы imu_data после last_timestamp
+vector<ImuSample> readAllImuData(sqlite3* db, double last_timestamp) {
+    vector<ImuSample> samples;
+    const char* sql = "SELECT timestamp, ax, ay, az, gx, gy, gz FROM imu_data WHERE timestamp > ? ORDER BY timestamp ASC";
+
+    sqlite3_stmt* stmt = nullptr;
+    if (sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr) != SQLITE_OK) {
+        cerr << "Ошибка подготовки запроса: " << sqlite3_errmsg(db) << endl;
+        return samples;
+    }
+    sqlite3_bind_double(stmt, 1, last_timestamp);
+
+    while (sqlite3_step(stmt) == SQLITE_ROW) {
+        ImuSample s;
+        s.timestamp = sqlite3_column_double(stmt, 0);
+        s.ax = sqlite3_column_double(stmt, 1);
+        s.ay = sqlite3_column_double(stmt, 2);
+        s.az = sqlite3_column_double(stmt, 3);
+        s.gx = sqlite3_column_double(stmt, 4);
+        s.gy = sqlite3_column_double(stmt, 5);
+        s.gz = sqlite3_column_double(stmt, 6);
+        samples.push_back(s);
+    }
+
+    sqlite3_finalize(stmt);
+    return samples;
+}
+
+// Чтение последнего обработанного timestamp из filtered_gyro_data
+double readLastTimestamp(sqlite3* db) {
+    double last_ts = 0.0;
+    sqlite3_stmt* stmt = nullptr;
+    if (sqlite3_prepare_v2(db, "SELECT MAX(timestamp) FROM filtered_gyro_data", -1, &stmt, nullptr) == SQLITE_OK) {
+        if (sqlite3_step(stmt) == SQLITE_ROW && sqlite3_column_type(stmt, 0) != SQLITE_NULL)
+            last_ts = sqlite3_column_double(stmt, 0);
+    }
+    sqlite3_finalize(stmt);
+    return last_ts;
+}
+
+// Запись отфильтрованных угловых скоростей в таблицу filtered_gyro_data
+void writeFilteredGyroData(sqlite3* db, double timestamp, double gx, double gy, double gz) {
+    // 1. Создаём таблицу, если она не существует
+    const char* create_sql =
+        "CREATE TABLE IF NOT EXISTS filtered_gyro_data ("
+        "timestamp REAL, "
+        "gx REAL, "
+        "gy REAL, "
+        "gz REAL)";
+    char* err_msg = nullptr;
+    if (sqlite3_exec(db, create_sql, nullptr, nullptr, &err_msg) != SQLITE_OK) {
+        cerr << "Ошибка создания таблицы filtered_gyro_data: " << err_msg << endl;
+        sqlite3_free(err_msg);
+        return;
+    }
+
+    // 2. Подготавливаем INSERT
+    const char* insert_sql = "INSERT INTO filtered_gyro_data (timestamp, gx, gy, gz) VALUES (?, ?, ?, ?)";
+    sqlite3_stmt* stmt = nullptr;
+    if (sqlite3_prepare_v2(db, insert_sql, -1, &stmt, nullptr) != SQLITE_OK) {
+        cerr << "Ошибка подготовки INSERT для gyro: " << sqlite3_errmsg(db) << endl;
+        return;
+    }
+
+    // 3. Привязываем значения
+    sqlite3_bind_double(stmt, 1, timestamp);
+    sqlite3_bind_double(stmt, 2, gx);
+    sqlite3_bind_double(stmt, 3, gy);
+    sqlite3_bind_double(stmt, 4, gz);
+
+    // 4. Выполняем
+    if (sqlite3_step(stmt) != SQLITE_DONE) {
+        cerr << "Ошибка вставки gyro: " << sqlite3_errmsg(db) << endl;
+    }
+
+    // 5. Очищаем
+    sqlite3_finalize(stmt);
+}
+
+int main() {
+    sqlite3* db = nullptr;
+    if (sqlite3_open("/home/grant/inertial_control/inertial_data.db", &db) != SQLITE_OK) {
+        cerr << "Не удалось открыть БД" << endl;
+        return 1;
+    }
+
+    sqlite3_busy_timeout(db, 5000);
+
+    char* errMsg = nullptr;
+    if (sqlite3_exec(db, "PRAGMA journal_mode=WAL;", nullptr, nullptr, &errMsg) != SQLITE_OK) {
+        cerr << "Ошибка установки WAL режима: " << errMsg << endl;
+        sqlite3_free(errMsg);
+    }
+    else {
+        cout << "WAL режим успешно включен" << endl;
+    }
+
+    // 1. Вычисление bias гироскопа по первым 200 выборкам
+    double gyro_bias_x = 0.0, gyro_bias_y = 0.0, gyro_bias_z = 0.0;
+    const int WIN_SIZE = 4;
+    const int BIAS_SAMPLES = 200;
+    {
+        cout << "Ожидание данных для вычисления bias гироскопа..." << endl;
+        vector<ImuSample> init_data;
+        while (init_data.size() < BIAS_SAMPLES) {
+            init_data = readAllImuData(db, 0.0);
+            if ((int)init_data.size() < BIAS_SAMPLES) sleep(1);
+        }
+        int n = BIAS_SAMPLES;
+        for (int i = 0; i < n; i++) {
+            gyro_bias_x += init_data[i].gx;
+            gyro_bias_y += init_data[i].gy;
+            gyro_bias_z += init_data[i].gz;
+        }
+        gyro_bias_x /= n;
+        gyro_bias_y /= n;
+        gyro_bias_z /= n;
+        cout << "Bias гироскопа: gx=" << gyro_bias_x
+             << " gy=" << gyro_bias_y
+             << " gz=" << gyro_bias_z << endl;
+    }
+
+    // Бесконечный цикл — читаем только новые данные каждые 3 секунды
+    while (true) {
+        double last_ts = readLastTimestamp(db);
+        auto all = readAllImuData(db, last_ts);
+
+        if (all.empty()) {
+            sleep(3);
+            continue;
+        }
+
+        cout << "Новых строк: " << all.size() << "\n";
+
+        // 2. Инициализация скользящих сумм для следующего окна
+        double gyro_xf = 0.0, gyro_yf = 0.0, gyro_zf = 0.0;
+        for (size_t i = 0; i < WIN_SIZE && i < all.size(); ++i) {
+            gyro_xf += all[i].gx;
+            gyro_yf += all[i].gy;
+            gyro_zf += all[i].gz;
+        }
+
+        // 3. Основной цикл: вычисляем среднее в окне, вычитаем bias и записываем
+        sqlite3_exec(db, "BEGIN;", nullptr, nullptr, nullptr);
+        for (size_t i = WIN_SIZE; i < all.size(); ++i) {
+            // Записываем отфильтрованные данные
+            writeFilteredGyroData(db, all[i].timestamp,
+                gyro_xf / WIN_SIZE - gyro_bias_x,
+                gyro_yf / WIN_SIZE - gyro_bias_y,
+                gyro_zf / WIN_SIZE - gyro_bias_z);
+
+            gyro_xf -= all[i - WIN_SIZE].gx;
+            gyro_xf += all[i].gx;
+            gyro_yf -= all[i - WIN_SIZE].gy;
+            gyro_yf += all[i].gy;
+            gyro_zf -= all[i - WIN_SIZE].gz;
+            gyro_zf += all[i].gz;
+        }
+        sqlite3_exec(db, "COMMIT;", nullptr, nullptr, nullptr);
+
+        cout << "записано в filtered_gyro_data." << endl;
+        sleep(3);
+    }
+
+    sqlite3_close(db);
+    return 0;
+}

processing/the_main_block.cpp

@@ -0,0 +1,337 @@
+#define _USE_MATH_DEFINES
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <unistd.h>
+#include "sqlite3.h"
+
+using namespace std;
+
+static const double G_MS2 = 9.81;   // g -> м/с²
+static const double DEG2RAD = M_PI / 180.0;
+
+
+class Kalman1D {
+private:
+    double x;      // положение
+    double v;      // скорость
+    double P[2][2]; // ковариационная матрица 2x2
+    double Q;       // интенсивность шума процесса (ускорение)
+    double R;       // дисперсия шума измерения
+    double dt;      // временной шаг
+
+public:
+    Kalman1D(double q_accel, double r_measure, double init_x = 0.0, double init_v = 0.0, double dt0 = 0.01)
+        : Q(q_accel), R(r_measure), dt(dt0), x(init_x), v(init_v)
+    {
+        P[0][0] = 1.0;
+        P[0][1] = 0.0;
+        P[1][0] = 0.0;
+        P[1][1] = 100.0;
+    }
+
+    void setDt(double new_dt) { dt = new_dt; }
+
+    void predict() {
+        double new_x = x + v * dt;
+        double new_v = v;
+
+        double P00 = P[0][0] + dt * (P[1][0] + P[0][1]) + dt * dt * P[1][1];
+        double P01 = P[0][1] + dt * P[1][1];
+        double P10 = P[1][0] + dt * P[1][1];
+        double P11 = P[1][1];
+
+        double Qv = Q * dt;
+        double Qp = Q * dt * dt * dt / 3.0;
+        double Qpv = Q * dt * dt / 2.0;
+
+        P00 += Qp;
+        P01 += Qpv;
+        P10 += Qpv;
+        P11 += Qv;
+
+        P[0][0] = P00; P[0][1] = P01;
+        P[1][0] = P10; P[1][1] = P11;
+
+        x = new_x;
+        v = new_v;
+    }
+
+    double update(double measurement) {
+        double y = measurement - x;
+        double S = P[0][0] + R;
+        double K_pos = P[0][0] / S;
+        double K_vel = P[1][0] / S;
+
+        x += K_pos * y;
+        v += K_vel * y;
+
+        double P00_new = (1.0 - K_pos) * P[0][0];
+        double P01_new = (1.0 - K_pos) * P[0][1];
+        double P11_new = P[1][1] - K_vel * P[1][0];
+
+        P[0][0] = P00_new; P[0][1] = P01_new;
+        P[1][0] = P01_new; P[1][1] = P11_new;
+
+        return x;
+    }
+
+    double getState() const { return x; }
+    double getVelocity() const { return v; }
+};
+
+
+// ─────────────────────────────────────────────
+//  Структуры
+// ─────────────────────────────────────────────
+struct AccSample {
+    double timestamp;
+    double ax, ay, az;
+};
+
+struct GyroSample {
+    double timestamp;
+    double gx, gy, gz;
+};
+
+// ─────────────────────────────────────────────
+//  Чтение из БД
+// ─────────────────────────────────────────────
+
+// Читаем последний timestamp и последнее состояние навигации из trajectory
+double readLastTrajectoryState(sqlite3* db,
+    double& px, double& py, double& pz,
+    double& vx, double& vy, double& vz,
+    double& roll, double& pitch, double& yaw)
+{
+    double last_ts = 0.0;
+    sqlite3_stmt* stmt = nullptr;
+    if (sqlite3_prepare_v2(db,
+        "SELECT timestamp, x, y, z, vx, vy, vz, roll, pitch, yaw "
+        "FROM trajectory ORDER BY timestamp DESC LIMIT 1",
+        -1, &stmt, nullptr) == SQLITE_OK)
+    {
+        if (sqlite3_step(stmt) == SQLITE_ROW) {
+            last_ts = sqlite3_column_double(stmt, 0);
+            px    = sqlite3_column_double(stmt, 1);
+            py    = sqlite3_column_double(stmt, 2);
+            pz    = sqlite3_column_double(stmt, 3);
+            vx    = sqlite3_column_double(stmt, 4);
+            vy    = sqlite3_column_double(stmt, 5);
+            vz    = sqlite3_column_double(stmt, 6);
+            roll  = sqlite3_column_double(stmt, 7);
+            pitch = sqlite3_column_double(stmt, 8);
+            yaw   = sqlite3_column_double(stmt, 9);
+        }
+    }
+    sqlite3_finalize(stmt);
+    return last_ts;
+}
+
+vector<AccSample> readAcc(sqlite3* db, double last_timestamp) {
+    vector<AccSample> data;
+    sqlite3_stmt* stmt = nullptr;
+    sqlite3_prepare_v2(db,
+        "SELECT timestamp, ax, ay, az "
+        "FROM filtered_acc_data WHERE timestamp > ? ORDER BY timestamp ASC",
+        -1, &stmt, nullptr);
+    sqlite3_bind_double(stmt, 1, last_timestamp);
+    while (sqlite3_step(stmt) == SQLITE_ROW) {
+        AccSample s;
+        s.timestamp = sqlite3_column_double(stmt, 0);
+        s.ax = sqlite3_column_double(stmt, 1);
+        s.ay = sqlite3_column_double(stmt, 2);
+        s.az = sqlite3_column_double(stmt, 3);
+        data.push_back(s);
+    }
+    sqlite3_finalize(stmt);
+    return data;
+}
+
+vector<GyroSample> readGyro(sqlite3* db, double last_timestamp) {
+    vector<GyroSample> data;
+    sqlite3_stmt* stmt = nullptr;
+    sqlite3_prepare_v2(db,
+        "SELECT timestamp, gx, gy, gz "
+        "FROM filtered_gyro_data WHERE timestamp > ? ORDER BY timestamp ASC",
+        -1, &stmt, nullptr);
+    sqlite3_bind_double(stmt, 1, last_timestamp);
+    while (sqlite3_step(stmt) == SQLITE_ROW) {
+        GyroSample s;
+        s.timestamp = sqlite3_column_double(stmt, 0);
+        s.gx = sqlite3_column_double(stmt, 1);
+        s.gy = sqlite3_column_double(stmt, 2);
+        s.gz = sqlite3_column_double(stmt, 3);
+        data.push_back(s);
+    }
+    sqlite3_finalize(stmt);
+    return data;
+}
+
+// ─────────────────────────────────────────────
+//  Запись в БД
+// ─────────────────────────────────────────────
+void createTable(sqlite3* db) {
+    char* err = nullptr;
+    sqlite3_exec(db,
+        "CREATE TABLE IF NOT EXISTS trajectory ("
+        "timestamp REAL, "
+        "x REAL, y REAL, z REAL, "
+        "vx REAL, vy REAL, vz REAL, "
+        "roll REAL, pitch REAL, yaw REAL)",
+        nullptr, nullptr, &err);
+}
+
+void writePoint(sqlite3_stmt* stmt,
+    double ts,
+    double x, double y, double z,
+    double vx, double vy, double vz,
+    double roll, double pitch, double yaw)
+{
+    sqlite3_reset(stmt);
+    sqlite3_bind_double(stmt, 1, ts);
+    sqlite3_bind_double(stmt, 2, x);
+    sqlite3_bind_double(stmt, 3, y);
+    sqlite3_bind_double(stmt, 4, z);
+    sqlite3_bind_double(stmt, 5, vx);
+    sqlite3_bind_double(stmt, 6, vy);
+    sqlite3_bind_double(stmt, 7, vz);
+    sqlite3_bind_double(stmt, 8, roll);
+    sqlite3_bind_double(stmt, 9, pitch);
+    sqlite3_bind_double(stmt, 10, yaw);
+    sqlite3_step(stmt);
+}
+
+// ─────────────────────────────────────────────
+//  main
+// ─────────────────────────────────────────────
+int main()
+{
+    sqlite3* db = nullptr;
+    if (sqlite3_open("/home/grant/inertial_control/inertial_data.db", &db) != SQLITE_OK) {
+        cerr << "Не удалось открыть БД" << endl;
+        return 1;
+    }
+
+    sqlite3_busy_timeout(db, 5000);
+    sqlite3_exec(db, "PRAGMA journal_mode=WAL;", nullptr, nullptr, nullptr);
+
+    createTable(db);
+
+    // Начальные условия — читаем из последней точки trajectory
+    double px = 0.0, py = 0.0, pz = 0.0;
+    double vx = 0.0, vy = 0.0, vz = 0.0;
+    double roll = 0.0, pitch = 0.0, yaw = 0.0;
+    double last_ts = readLastTrajectoryState(db, px, py, pz, vx, vy, vz, roll, pitch, yaw);
+
+    // Инициализация фильтров Калмана один раз при старте
+    Kalman1D kalmanAx(0.01, 0.1, 0.0, 0.0);
+    Kalman1D kalmanAy(0.01, 0.1, 0.0, 0.0);
+    Kalman1D kalmanAz(0.01, 0.1, 0.0, 0.0);
+    Kalman1D kalmanGx(0.001, 0.01, 0.0, 0.0);
+    Kalman1D kalmanGy(0.001, 0.01, 0.0, 0.0);
+    Kalman1D kalmanGz(0.001, 0.01, 0.0, 0.0);
+
+    sqlite3_stmt* ins = nullptr;
+    sqlite3_prepare_v2(db,
+        "INSERT INTO trajectory "
+        "(timestamp,x,y,z,vx,vy,vz,roll,pitch,yaw) "
+        "VALUES (?,?,?,?,?,?,?,?,?,?)",
+        -1, &ins, nullptr);
+
+    // Бесконечный цикл — читаем только новые данные каждые 3 секунды
+    while (true) {
+        last_ts = readLastTrajectoryState(db, px, py, pz, vx, vy, vz, roll, pitch, yaw);
+
+        auto accData = readAcc(db, last_ts);
+        auto gyroData = readGyro(db, last_ts);
+
+        if (accData.empty() || gyroData.empty()) {
+            sleep(3);
+            continue;
+        }
+
+        size_t N = min(accData.size(), gyroData.size());
+        cout << "Новых: acc=" << accData.size() << " gyro=" << gyroData.size() << endl;
+
+        sqlite3_exec(db, "BEGIN TRANSACTION;", nullptr, nullptr, nullptr);
+        size_t written = 0;
+
+        for (size_t i = 1; i < N; i++) {
+            double dt = 0.01;
+            if (dt <= 0.0 || dt > 0.5) dt = 0.01;
+
+            kalmanAx.setDt(dt);
+            kalmanAy.setDt(dt);
+            kalmanAz.setDt(dt);
+            kalmanGx.setDt(dt);
+            kalmanGy.setDt(dt);
+            kalmanGz.setDt(dt);
+
+            // Предсказание и обновление для акселерометра
+            kalmanAx.predict(); kalmanAx.update(accData[i].ax);
+            kalmanAy.predict(); kalmanAy.update(accData[i].ay);
+            kalmanAz.predict(); kalmanAz.update(accData[i].az);
+
+            // Предсказание и обновление для гироскопа
+            kalmanGx.predict(); kalmanGx.update(gyroData[i].gx);
+            kalmanGy.predict(); kalmanGy.update(gyroData[i].gy);
+            kalmanGz.predict(); kalmanGz.update(gyroData[i].gz);
+
+            // Получаем отфильтрованные значения
+            double ax_f = kalmanAx.getState();
+            double ay_f = kalmanAy.getState();
+            double az_f = kalmanAz.getState();
+            double gx_f = kalmanGx.getState();
+            double gy_f = kalmanGy.getState();
+            double gz_f = kalmanGz.getState();
+
+            // Перевод в м/с²
+            double ax_ms2 = ax_f * G_MS2;
+            double ay_ms2 = ay_f * G_MS2;
+            double az_ms2 = az_f * G_MS2;
+
+            // Интегрирование углов (используем отфильтрованные угловые скорости)
+            roll += gx_f * dt;
+            pitch += gy_f * dt;
+            yaw += gz_f * dt;
+
+            double Gx = G_MS2 * sin(pitch * DEG2RAD);
+            double Gy = G_MS2 * cos(pitch * DEG2RAD) * sin(roll * DEG2RAD) + fmod(sin(yaw * DEG2RAD) * G_MS2, sqrt(2));
+            double Gz = G_MS2 * cos(pitch * DEG2RAD) * cos(roll * DEG2RAD) - fmod(sin(yaw * DEG2RAD) * G_MS2, sqrt(2));
+
+            ax_ms2 -= Gx;
+            ay_ms2 += Gy;
+            az_ms2 -= Gz;
+
+            // Интегрирование скорости: v += a * dt
+            vx += ax_ms2 * dt;
+            vy += ay_ms2 * dt;
+            vz += az_ms2 * dt;
+
+            // Интегрирование позиции: p += v * dt
+            px += vx * dt;
+            py += vy * dt;
+            pz += vz * dt;
+
+            // Запись
+            writePoint(ins,
+                accData[i].timestamp,
+                px, py, pz,
+                vx, vy, vz,
+                roll, pitch, yaw);
+            written++;
+        }
+
+        sqlite3_exec(db, "COMMIT;", nullptr, nullptr, nullptr);
+
+        cout << "Готово. Записано " << written << " точек в trajectory." << endl;
+        cout << "Позиция в метрах, углы в градусах." << endl;
+
+        sleep(3);
+    }
+
+    sqlite3_finalize(ins);
+    sqlite3_close(db);
+    return 0;
+}

processing/visualization.py

@@ -0,0 +1,96 @@
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+from mpl_toolkits.mplot3d import Axes3D
+import sqlite3
+import os
+import numpy as np
+
+DB_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'inertial_data.db')
+
+TRAJECTORY_LIMIT = 5000
+LIDAR_LIMIT = 2000
+
+
+def read_trajectory():
+    # Новое соединение на каждый кадр — иначе видим устаревший снимок БД.
+    with sqlite3.connect(DB_PATH) as conn:
+        cursor = conn.execute(
+            "SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT ?",
+            (TRAJECTORY_LIMIT,)
+        )
+        rows = cursor.fetchall()
+    if not rows:
+        return np.zeros((0, 3))
+    return np.array(rows[::-1])  # от старых к новым
+
+
+def read_lidar_points():
+    with sqlite3.connect(DB_PATH) as conn:
+        cursor = conn.execute(
+            "SELECT x, y, z FROM lidar_points ORDER BY rowid DESC LIMIT ?",
+            (LIDAR_LIMIT,)
+        )
+        rows = cursor.fetchall()
+    if not rows:
+        return np.zeros((0, 3))
+    return np.array(rows)
+
+
+def main():
+    fig = plt.figure(figsize=(10, 8))
+    ax = fig.add_subplot(111, projection='3d')
+
+    def on_close(event):
+        print("Окно визуализации закрыто.")
+
+    fig.canvas.mpl_connect('close_event', on_close)
+
+    def update(frame):
+        ax.cla()
+        ax.set_title("Инерциальное управление")
+        ax.set_xlabel("X, м")
+        ax.set_ylabel("Y, м")
+        ax.set_zlabel("Z, м")
+
+        trajectory = read_trajectory()
+        lidar_points = read_lidar_points()
+
+        # Точка старта — для ориентира всегда показываем
+        ax.scatter([0], [0], [0], c='black', s=80, marker='x',
+                   label='Старт (0, 0, 0)')
+
+        if len(lidar_points) > 0:
+            ax.scatter(
+                lidar_points[:, 0],
+                lidar_points[:, 1],
+                lidar_points[:, 2],
+                c='blue', s=1, alpha=0.5, label='Лидар'
+            )
+
+        if len(trajectory) > 1:
+            ax.plot(
+                trajectory[:, 0],
+                trajectory[:, 1],
+                trajectory[:, 2],
+                c='red', linewidth=2, label='Траектория'
+            )
+            # Текущая позиция датчика — последняя точка траектории
+            ax.scatter(
+                [trajectory[-1, 0]],
+                [trajectory[-1, 1]],
+                [trajectory[-1, 2]],
+                c='green', s=100, label='Позиция (сейчас)'
+            )
+
+        ax.legend(loc='upper right')
+
+    ani = animation.FuncAnimation(fig, update, interval=200, cache_frame_data=False)
+
+    print("Визуализация запущена. Закройте окно для выхода.")
+    print("Чёрный крестик — точка старта (0, 0, 0).")
+    print("Зелёная точка — текущая позиция датчика.")
+    print("Красная линия — путь от старта до текущей позиции.")
+    plt.show()
+
+
+main()

run.sh

@@ -1,22 +1,26 @@
 #!/bin/bash
-echo "Запуск системы..."
 
-# Запуск модуля акселерометра
+# Переходим в папку проекта
-echo "Запуск IMU модуля..."
+cd "$(dirname "$0")"
+
+echo "Запуск проекта..."
+
+# Запускаем модули сбора данных в фоне
 python3 imu_module/imu_reader.py &
+IMU_PID=$!
 
-# Запуск модуля лидара
-echo "Запуск лидар модуля..."
 python3 lidar_module/lidar_reader.py &
+LIDAR_PID=$!
 
-# Ждём пока данные запишутся
+# Запускаем расчёты в фоне
+processing/calculations &
+CALC_PID=$!
+
+# Запускаем визуализацию (она управляет завершением)
+python3 processing/visualization.py
+
+# Когда визуализация закрыта — останавливаем всё
+echo "Остановка проекта..."
+kill $IMU_PID $LIDAR_PID $CALC_PID 2>/dev/null
 wait
-
+echo "Проект остановлен"
-# Компиляция и запуск расчётов
-echo "Запуск расчётов..."
-g++ processing/calculations.cpp -o processing/calculations
-./processing/calculations
-
-# Запуск визуализации
-echo "Запуск визуализации..."
-python3 processing/visualization.py