Фильтрация (Kalman/Complementary) — Теория и практика стохастической реконструкции реальности

Фильтрация в робототехнике — это не подавление шума, а акт восстановления скрытой истины. В 2026 году это союз двух философий: байесовский вывод (что наиболее вероятно?) и частотный анализ (где полезный сигнал?). Комплементарный фильтр и фильтр Калмана представляют эти подходы в чистом виде, создавая мост между интуитивной простотой и математической оптимальностью.

Имеем:

• Скрытое состояние: \( x_t \in \mathbb{R}^n \) (истинное положение, скорость, ориентация)
• Измерения: \( z_t \in \mathbb{R}^m \) (показания сенсоров)
• Модель перехода: \( x_t = f(x_{t-1}, u_t, w_t) \)
• Модель наблюдения: \( z_t = h(x_t, v_t) \)

где \( w_t \sim \mathcal{N}(0, Q) \), \( v_t \sim \mathcal{N}(0, R) \) — шумы процесса и измерений.

Задача: Найти \( \hat{x}_t = \mathbb{E}[x_t | z_{1:t}] \) — оптимальную оценку состояния по всем наблюдениям.

Комплементарный фильтр работает, когда два сенсора имеют дополняющие частотные характеристики:

Канонический пример — IMU:

• Гироскоп: Высокая точность в высокочастотной области, но дрейфует на низких частотах (интегральная ошибка)
• Акселерометр/Магнитометр: Точность в низкочастотной области, но шумит на высоких частотах

где \( \alpha = e^{-\Delta t / \tau} \approx 1 - \frac{\Delta t}{\tau} \) для малых \(\Delta t\).

Физический смысл α:

• \( \alpha = 0.98 \): Доверяем гироскопу на 98%
• \( \alpha \to 1 \): Полное доверие гироскопу (чистая интеграция)
• \( \alpha \to 0 \): Полное доверие акселерометру

class MahonyFilter {
    float q[4] = {1, 0, 0, 0};  // Кватернион
    float integralFB[3] = {0};  // Интеграл ошибки
    float Ki, Kp;              // Коэффициенты
    
    void update(float gx, float gy, float gz, 
                float ax, float ay, float az,
                float mx, float my, float mz, float dt) {
        
        // Нормализация измерений
        float norm = sqrt(ax*ax + ay*ay + az*az);
        ax /= norm; ay /= norm; az /= norm;
        
        // Ошибка через векторное произведение
        float ex = ay*q[2] - az*q[1] - mx;
        float ey = az*q[0] - ax*q[2] - my;
        float ez = ax*q[1] - ay*q[0] - mz;
        
        // ПИ-коррекция
        integralFB[0] += Ki * ex * dt;
        integralFB[1] += Ki * ey * dt;
        integralFB[2] += Ki * ez * dt;
        
        // Корректируем гироскоп
        gx += Kp * ex + integralFB[0];
        gy += Kp * ey + integralFB[1];
        gz += Kp * ez + integralFB[2];
        
        // Интегрирование кватерниона
        qDot[0] = 0.5 * (-q[1]*gx - q[2]*gy - q[3]*gz);
        qDot[1] = 0.5 * ( q[0]*gx + q[2]*gz - q[3]*gy);
        qDot[2] = 0.5 * ( q[0]*gy - q[1]*gz + q[3]*gx);
        qDot[3] = 0.5 * ( q[0]*gz + q[1]*gy - q[2]*gx);
        
        // Интегрирование (метод Рунге-Кутта 4 порядка)
        // ... интегрирование q += qDot * dt
    }
};

Пример — детектор движения:

float dynamic_alpha(float ax, float ay, float az, float threshold) {
    float accel_magnitude = sqrt(ax*ax + ay*ay + az*az);
    if (abs(accel_magnitude - 9.81) > threshold) {
        // Сильное ускорение → меньше доверия акселерометру
        return 0.995;
    } else {
        // Статика → больше доверия акселерометру
        return 0.98;
    }
}

Для гауссовых распределений эта теорема превращается в элегантные рекуррентные уравнения.

Алгоритм (двухэтапный):

1. \[ \hat{x}_{t|t-1} = F_t \hat{x}_{t-1|t-1} + B_t u_t \]\[ P_{t|t-1} = F_t P_{t-1|t-1} F_t^T + Q_t \]
2. \[ \tilde{y}_t = z_t - H_t \hat{x}_{t|t-1} \quad \text{(инновации)} \]\[ S_t = H_t P_{t|t-1} H_t^T + R_t \quad \text{(ковариация инноваций)} \]\[ K_t = P_{t|t-1} H_t^T S_t^{-1} \quad \text{(оптимальный коэффициент Калмана)} \]\[ \hat{x}_{t|t} = \hat{x}_{t|t-1} + K_t \tilde{y}_t \]\[ P_{t|t} = (I - K_t H_t) P_{t|t-1} \]

• F (State Transition): Как состояние эволюционирует без управления
• Q (Process Noise): Насколько мы доверяем своей модели
• H (Observation Model): Как состояние проявляется в измерениях
• R (Measurement Noise): Насколько мы доверяем сенсорам
• P (Error Covariance): Наша неуверенность в оценке
• K (Kalman Gain): Баланс между моделью и измерением

Состояние: \( x = [p_x, p_y, v_x, v_y, a_x, a_y]^T \)

Реализация:

class KalmanFilter6D {
    Eigen::Matrix<float, 6, 6> F, P, Q;
    Eigen::Matrix<float, 2, 6> H;
    Eigen::Matrix<float, 6, 2> K;
    Eigen::Matrix<float, 2, 2> R;
    Eigen::Vector<float, 6> x;
    
    void predict(float dt) {
        F(0,2) = dt; F(1,3) = dt;  // Обновляем матрицу перехода
        x = F * x;
        P = F * P * F.transpose() + Q;
    }
    
    void update(float px, float py) {
        Eigen::Vector2f z(px, py);
        Eigen::Vector2f y = z - H * x;  // Инновации
        Eigen::Matrix2f S = H * P * H.transpose() + R;
        K = P * H.transpose() * S.inverse();
        x = x + K * y;
        P = (Eigen::Matrix6f::Identity() - K * H) * P;
    }
};

Идея: Локальная линеаризация через разложение Тейлора.

Пример — оценка ориентации через EKF:

class OrientationEKF:
    def jacobian_F(self, q, w, dt):
        # Якобиан для кватернионной динамики
        # q̇ = 0.5 * Ω(w) * q
        F = np.eye(4) + 0.5*dt*self.omega_matrix(w)
        return F
    
    def jacobian_H(self, q):
        # Якобиан для преобразования кватерниона в ускорение
        # a_expected = R(q) * [0,0,-g]^T
        H = np.zeros((3, 4))
        # ... вычисление производных
        return H

Преимущество над EKF: Не требует вычисления Якобианов, точнее для сильно нелинейных систем.

Алгоритм:

1. Выбрать сигма-точки вокруг текущей оценки
2. Прогнать каждую точку через нелинейную модель
3. Вычислить статистики преобразованного ансамбля

Идея: Использовать Монте-Карло выборку вместо полной ковариационной матрицы.

Применение: Оценка состояния робота с сотнями степеней свободы (гибкие манипуляторы).

Экспериментальная установка:

• Платформа: Crazyflie 2.1 (STM32F4, MPU9250)
• Задача: Оценка ориентации для стабилизации полёта
• Методы: Комплементарный, EKF, Mahony, Madgwick
• Эталон: Оптическая система Vicon (точность 0.1°)

Метрики сравнения:

1. \[ RMSE = \sqrt{\frac{1}{N} \sum_{i=1}^N (\theta_i^{true} - \hat{\theta}_i)^2} \]

2. Вычислительная нагрузка:

• Время выполнения на такт
• Использование памяти
• Энергопотребление

3. Робастность:

• К электромагнитным помехам
• К вибрациям
• К температурным изменениям

Результаты типичного эксперимента 2026:

Комбинация частотного и байесовского подходов:

class HybridFilter {
    ComplementaryFilter fast_filter;  // 1 кГц
    KalmanFilter slow_filter;         // 100 Гц
    
    void update(float dt, SensorData data) {
        // Быстрый комплементарный фильтр
        float fast_estimate = fast_filter.update(data, dt);
        
        // Медленный Калман для коррекции смещений
        if (time_for_slow_update()) {
            slow_filter.calibrate(fast_estimate);
            fast_filter.set_bias(slow_filter.get_bias());
        }
    }
};

Идея: Замена предсказания или коррекции нейросетью.

Архитектура KalmanNet:

class KalmanNet(nn.Module):
    def __init__(self):
        super().__init__()
        self.F_net = nn.LSTM(6, 32)  # Предсказание состояния
        self.H_net = nn.Linear(32, 4)  # Наблюдение
        self.K_net = nn.Linear(4, 6)  # Коэффициент Калмана
    
    def forward(self, x, z):
        # Нейросетевая замена матриц F, H, K
        x_pred = self.F_net(x)
        z_pred = self.H_net(x_pred)
        innovation = z - z_pred
        K = self.K_net(innovation)
        x_corrected = x_pred + K * innovation
        return x_corrected

где \(w_{ij}\) — веса консенсуса.

где \(|\alpha|^2\) — вероятность состояния 0, \(|\beta|^2\) — вероятность состояния 1.

1. Комплементарный фильтр: Ориентация дрона, балансирующий робот
2. α-β-γ фильтр: Слежение за положением по радару

1. EKF: Одометрия робота, навигация с GPS/IMU
2. UKF: Нелинейная ориентация, SLAM в небольших масштабах

1. Particle Filter: SLAM с нелинейностями и мультимодальностью
2. Нейросетевые фильтры: Системы с неизвестными моделями

if (система_линейна && ресурсы_ограничены) → KF
else if (слабые_нелинейности && есть_Якобианы) → EKF  
else if (сильные_нелинейности && нет_Якобианов) → UKF
else if (частота_высокая && точность_приемлемая) → Комплементарный
else → Particle Filter или нейросеть

Фильтры, которые учатся, каким сенсорам доверять в разных ситуациях.

Использование квантовой суперпозиции для параллельной обработки множества гипотез.

• Вестибулярная фильтрация по аналогии с внутренним ухом
• Тактильная фильтрация с адаптацией рецепторов

Оценка состояния робота в физическом мире и его аватара в виртуальном мире одновременно.

1. SLAM и навигация — применение фильтрации для построения карт
2. Сенсорная фьюжн — объединение данных множества сенсоров
3. Управление роботами — использование оценок состояния в контуре управления
4. Машинное обучение — нейросетевые подходы к фильтрации

Философский итог: Фильтрация в 2026 году — это не просто обработка сигналов, а фундаментальный акт познания. Комплементарный фильтр представляет собой инженерную мудрость — простые решения сложных проблем через понимание физики. Фильтр Калмана — это математическая элегантность — оптимальность через строгий вывод. Вместе они образуют диалектику робототехнического восприятия: интуиция против формализма, простота против оптимальности, частота против вероятности. Истинное мастерство — в умении выбрать правильный инструмент для конкретной задачи, понимая, что в конечном счёте мы не подавляем шум, а восстанавливаем реальность из её зашумлённых проекций.