Skip to content
TheChatArchiveThe Chat Archive
gpt-4o-2024-08-06· OpenAIAi2 WildChat-4.8M (AI2)

В предложенной реализации фильтра Махони чему соответствую компоненты вектора q

  1. User

    В предложенной реализации фильтра Махони чему соответствую компоненты вектора q

  2. Assistant

    В фильтре Махони используются кватернионы для представления ориентации объекта в трёхмерном пространстве. Кватернион обычно обозначается как ( q = (q_0, q_1, q_2, q_3) ), где:

    • ( q_0 ) – скалярная часть кватерниона.
    • ( q_1, q_2, q_3 ) – векторная часть кватерниона, соответствующая осям x, y и z.

    Кватернионы используются для описания поворотов и ориентации, поскольку они избегают проблем, связанных с потерей точности и особенностями, как это может происходить при использовании других математических описаний, таких как Эйлеровы углы. В контексте фильтра Махони кватернионы позволяют плавно и стабильно отслеживать ориентацию объекта, к примеру, в таких приложениях, как инерциальная навигация или стабилизация платформы.

  3. User

    Фильтр Махони для ориентации объекта по данным с гироскопа и акселерометра на питон

  4. Assistant

    Фильтр Махони — это алгоритм, который используется для расчета ориентации объекта с использованием данных от гироскопа и акселерометра. Он использует кватернионы для устойчивого представления ориентации. В этой реализации, фильтр будет корректировать данные гироскопа с учетом ускорения, измеренного акселерометром, чтобы минимизировать дрейф гироскопа.

    Вот пример простой реализации фильтра Махони на Python:

    import numpy as np
    
    class MahonyAHRS:
        def __init__(self, sample_freq=100.0, kp=2.0, ki=0.0):
            self.sample_freq = sample_freq  # Частота дискретизации
            self.kp = kp  # Коэффициент пропорциональности
            self.ki = ki  # Коэффициент интеграции
            self.q = np.array([1.0, 0.0, 0.0, 0.0])  # Начальная ориентация кватерниона
            self.integral_fb = np.array([0.0, 0.0, 0.0])  # Интегральная ошибка
    
        def update(self, gx, gy, gz, ax, ay, az):
            # Нормализуем ускорение
            norm = np.sqrt(ax * ax + ay * ay + az * az)
            if norm == 0:
                return  # Ускорение равно 0, невозможно нормализовать
            ax /= norm
            ay /= norm
            az /= norm
    
            # Вычисляем промежуточные величины
            q1, q2, q3, q4 = self.q
            v2q1, v2q2, v2q3 = 2 * q1, 2 * q2, 2 * q3
            v4q1, v4q2, v4q3, v4q4 = 4 * q1, 4 * q2, 4 * q3, 4 * q4
            v8q2, v8q3 = 8 * q2, 8 * q3
    
            # Вычисляем вектор ошибки
            f1 = v2q2 * q4 - v2q1 * q3 - ax
            f2 = v2q1 * q2 + v2q3 * q4 - ay
            f3 = 1.0 - v2q2 * q2 - v2q3 * q3 - az
    
            # Вычисляем градиентный спуск
            grad = np.array([
                v4q1 * f1 + v2q3 * f2,
                v4q2 * f1 + v4q3 * f2 + v8q2 * f3,
                v4q1 * f2 - v2q4 * f1 + v8q3 * f3,
                -v2q1 * f1 + v4q4 * f2
            ])
    
            # Нормализуем градиент
            norm = np.linalg.norm(grad)
            grad /= norm
    
            # Интегральная часть управления
            self.integral_fb += self.ki * grad / self.sample_freq
    
            # Корректируем данные гироскопа
            gx += self.kp * grad[0] + self.integral_fb[0]
            gy += self.kp * grad[1] + self.integral_fb[1]
            gz += self.kp * grad[2] + self.integral_fb[2]
    
            # Интеграл от скорости накапливается в кватернионе
            q_dot = 0.5 * np.array([
                -q2 * gx - q3 * gy - q4 * gz,
                 q1 * gx + q3 * gz - q4 * gy,
                 q1 * gy - q2 * gz + q4 * gx,
                 q1 * gz + q2 * gy - q3 * gx
            ])
    
            # Интегрируем для получения нового состояния кватерниона
            self.q += q_dot / self.sample_freq
            self.q /= np.linalg.norm(self.q)  # Нормализуем кватернион
    
    # Пример использования
    ahrs = MahonyAHRS(sample_freq=100.0)
    for _ in range(100):  # Пример использования в цикле
        ahrs.update(gx=0.01, gy=0.02, gz=0.03, ax=0.0, ay=0.0, az=1.0)
        print(ahrs.q)
    

    Этот код реализует фильтр Махони для оценки ориентации на основе данных синхронизированных инерциальных датчиков. Переменные gx, gy, gz представляют собой угловые скорости, измеренные гироскопом, а ax, ay, az — ускорения, измеренные акселерометром. Ориентация возвращается в виде кватерниона self.q.

  5. User

    По данным с акселерометра и гироскопа необходимо точно оценить ускорение с учетом разложения вектора сила тяжести по оясм с учетом данных гироскопа. Нужна программа на питоне

  6. Assistant

    Для точной оценки ускорения, с учетом компенсации влияния гравитации, на основе данных с акселерометра и гироскопа, необходимо использовать фильтр для оценки ориентации объекта. В данном контексте можно использовать фильтр Махони или фильтр Комплементарной, чтобы сначала оценить ориентацию, а затем использовать ее для компенсации гравитации.

    Вот пример программы на Python, которая использует фильтр Махони для оценки ориентации и компенсирует гравитацию:

    import numpy as np
    
    class MahonyAHRS:
        def __init__(self, sample_freq=100.0, kp=2.0, ki=0.0):
            self.sample_freq = sample_freq  # Частота дискретизации
            self.kp = kp
            self.ki = ki
            self.q = np.array([1.0, 0.0, 0.0, 0.0])
            self.integral_fb = np.array([0.0, 0.0, 0.0])
    
        def update(self, gx, gy, gz, ax, ay, az, dt):
            # Normalize acceleration
            norm = np.sqrt(ax * ax + ay * ay + az * az)
            if norm == 0:
                return
            ax /= norm
            ay /= norm
            az /= norm
    
            q1, q2, q3, q4 = self.q
    
            # Gradient descent algorithm corrective step
            f1 = 2.0 * (q2 * q4 - q1 * q3) - ax
            f2 = 2.0 * (q1 * q2 + q3 * q4) - ay
            f3 = 2.0 * (0.5 - q2 * q2 - q3 * q3) - az
            j11or24 = 2.0 * q3
            j12or23 = 2.0 * q4
            j13or22 = 2.0 * q1
            j14or21 = 2.0 * q2
            j32 = 2.0 * j14or21
            j33 = 2.0 * j11or24
    
            # Gradient calculation
            grad_f = np.array([
                j14or21 * f2 - j11or24 * f3,
                j12or23 * f3 - j33 * f1,
                j32 * f1 - j13or22 * f2
            ])
    
            norm = np.linalg.norm(grad_f)
            grad_f /= norm
    
            # Integrate rate of change of quaternion
            self.integral_fb += self.ki * grad_f * dt
            gx += self.kp * grad_f[0] + self.integral_fb[0]
            gy += self.kp * grad_f[1] + self.integral_fb[1]
            gz += self.kp * grad_f[2] + self.integral_fb[2]
    
            # Integrate to yield quaternion
            q_dot = 0.5 * np.array([
                -q2 * gx - q3 * gy - q4 * gz,
                 q1 * gx + q3 * gz - q4 * gy,
                 q1 * gy - q2 * gz + q4 * gx,
                 q1 * gz + q2 * gy - q3 * gx
            ])
    
            self.q += q_dot * dt
            self.q /= np.linalg.norm(self.q)
    
        def get_gravity(self):
            q1, q2, q3, q4 = self.q
            g_x = 2 * (q2 * q4 - q1 * q3)
            g_y = 2 * (q1 * q2 + q3 * q4)
            g_z = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4
            return np.array([g_x, g_y, g_z])
    
        def get_linear_acceleration(self, ax, ay, az):
            gravity = self.get_gravity()
            return np.array([ax, ay, az]) - gravity
    
    # Пример использования
    ahrs = MahonyAHRS(sample_freq=100.0)
    
    # Допустим, есть поток данных от датчиков:
    for _ in range(100):  # Этот цикл имитирует чтение данных
        gx, gy, gz = 0.01, 0.02, 0.03  # Данные гироскопа (в радианах/секунду)
        ax, ay, az = 0.0, 0.0, 1.0      # Данные акселерометра (в g)
        dt = 1.0 / 100.0               # Период дискретизации
    
        ahrs.update(gx, gy, gz, ax, ay, az, dt)
        linear_acceleration = ahrs.get_linear_acceleration(ax, ay, az)
        print("Linear Acceleration:", linear_acceleration)
    

    В этом примере:

    1. Мы сначала обновляем состояние фильтра, используя данные акселерометра и гироскопа.
    2. Затем мы рассчитываем жесткий вектор гравитации из текущего кватерниона ориентации.
    3. Наконец, мы вычитаем рассчитанную гравитацию из измеренного ускорения для оценки линейного ускорения, исключая влияние гравитации.

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

  7. User

    В таком случае накапливается ускорение и данные не точны

  8. Assistant

    Если в процессе работы системы наблюдается накопление ошибок в оценке ускорения, такое явление обычно связано с дрейфом интеграции ошибок гироскопа и шумами в данных сенсоров. Для улучшения точности и уменьшения накопления ошибок можно рассмотреть несколько подходов:

    1. Калибровка сенсоров: Убедитесь, что акселерометры и гироскопы правильно откалиброваны. Неточные данные с сенсоров могут привести к накапливающимся ошибкам.

    2. Фильтрация данных: Используйте фильтры, такие как комплементарный, Калмана или усовершенствованные версии алгоритма Махони, которые умеют сглаживать данные и уменьшать влияние шума.

    3. Регулярная коррекция: Добавьте дополнительные методы коррекции (например, используя данные от других сенсоров, таких как GPS или магнитометр), чтобы периодически корректировать оценку ориентации и ускорения.

    4. Улучшение модели: Увеличьте сложность модели, например, добавьте дополнительные состояния в фильтр, которые будут учитывать дрейф гироскопа или случайные ошибки.

    Давайте рассмотрим, как можно применить более сложный подход, используя фильтр Калмана для улучшенной оценки линейного ускорения. Это достаточно обширная тема, но здесь приведён адаптированный пример:

    import numpy as np
    from scipy.spatial.transform import Rotation as R
    
    class KalmanFilter:
        def __init__(self, dt, process_noise_var, measurement_noise_var):
            self.dt = dt
            
            # Состояния: [угловая скорость, ускорение]
            self.state = np.zeros(6)
            
            # Ковариационная матрица
            self.covariance = np.eye(6) * 0.1
            
            # Матрица перехода состояния
            self.F = np.eye(6)
            
            # Матрица измерений
            self.H = np.zeros((3, 6))
            self.H[:, 3:] = np.eye(3)
            
            # Процесс и измерение ковариаций
            self.Q = np.eye(6) * process_noise_var
            self.R = np.eye(3) * measurement_noise_var
    
        def predict(self, gyro):
            gyro_scaled = np.array(gyro) * self.dt
            rotation = R.from_rotvec(gyro_scaled).as_matrix()
            
            # Обновляем матрицу перехода состояния для ускорения
            acc_rotation_matrix = np.eye(6)
            acc_rotation_matrix[3:, 3:] = rotation
            self.F = acc_rotation_matrix
    
            # Предсказание нового состояния
            self.state = self.F @ self.state
            self.covariance = self.F @ self.covariance @ self.F.T + self.Q
    
        def update(self, measured_acc):
            y = measured_acc - self.H @ self.state
            S = self.H @ self.covariance @ self.H.T + self.R
            K = self.covariance @ self.H.T @ np.linalg.inv(S)
            self.state = self.state + K @ y
            self.covariance = (np.eye(6) - K @ self.H) @ self.covariance
    
        def get_linear_acceleration(self):
            return self.state[3:]
    
    
    # Пример использования KalmanFilter:
    sample_freq = 100.0
    kf = KalmanFilter(dt=1.0/sample_freq, process_noise_var=0.001, measurement_noise_var=0.01)
    
    # Основной поток данных
    for _ in range(100):  
        gx, gy, gz = 0.01, 0.02, 0.03  # Данные гироскопа, радиан/секунда
        ax, ay, az = 0.0, 0.0, 1.0     # Данные акселерометра, g
    
        # Обновляем оценку данных
        kf.predict([gx, gy, gz])
        kf.update([ax, ay, az])
        
        # Получаем линейное ускорение
        linear_acceleration = kf.get_linear_acceleration()
        print("Linear Acceleration:", linear_acceleration)
    

    Этот пример использует упрощённый фильтр Калмана для оценки линейного ускорения с учетом ориентации, основанной на данных гироскопа. Фильтр Калмана помогает улучшить точность оценки, объединяя прогнозируемую динамику системы и реальное измеренное состояние.

    Для продвинутого моделирования следует более внимательно выбирать параметры фильтра и ковариационные матрицы, а также учитывать возможные дополнительные источники данных для получения оптимальных результатов.

  9. User

    Данные выдаются с mpu6050. Обнови программу с учетом этого

  10. Assistant

    Чтобы интегрировать данные с датчика MPU6050, необходимо использовать библиотеку, которая поддерживает сбор данных с этого сенсора. Одна из популярных библиотек для работы с MPU6050 в Python — это mpu6050-raspberrypi, которая позволяет читать данные с акселерометра и гироскопа через интерфейс I2C. Вот пример, как можно использовать эту библиотеку совместно с фильтром Калмана для оценки линейного ускорения:

    1. Установите библиотеку mpu6050-raspberrypi, если вы еще этого не сделали:
    pip install mpu6050-raspberrypi
    
    1. Теперь создадим программу для работы с MPU6050 и фильтром Калмана:
    import numpy as np
    from mpu6050 import mpu6050
    from scipy.spatial.transform import Rotation as R
    import time
    
    class KalmanFilter:
        def __init__(self, dt, process_noise_var, measurement_noise_var):
            self.dt = dt
            self.state = np.zeros(6)  # [угловая скорость, ускорение]
            self.covariance = np.eye(6) * 0.1
            self.F = np.eye(6)
            self.H = np.zeros((3, 6))
            self.H[:, 3:] = np.eye(3)
            self.Q = np.eye(6) * process_noise_var
            self.R = np.eye(3) * measurement_noise_var
    
        def predict(self, gyro):
            gyro_scaled = np.array(gyro) * self.dt
            rotation = R.from_rotvec(gyro_scaled).as_matrix()
            acc_rotation_matrix = np.eye(6)
            acc_rotation_matrix[3:, 3:] = rotation
            self.F = acc_rotation_matrix
            self.state = self.F @ self.state
            self.covariance = self.F @ self.covariance @ self.F.T + self.Q
    
        def update(self, measured_acc):
            y = measured_acc - self.H @ self.state
            S = self.H @ self.covariance @ self.H.T + self.R
            K = self.covariance @ self.H.T @ np.linalg.inv(S)
            self.state = self.state + K @ y
            self.covariance = (np.eye(6) - K @ self.H) @ self.covariance
    
        def get_linear_acceleration(self):
            return self.state[3:]
    
    # Установка параметров
    sample_freq = 100.0
    kf = KalmanFilter(dt=1.0/sample_freq, process_noise_var=0.001, measurement_noise_var=0.01)
    
    # Инициализация датчика
    sensor = mpu6050(0x68)
    
    try:
        while True:
            accel_data = sensor.get_accel_data()
            gyro_data = sensor.get_gyro_data()
    
            ax, ay, az = accel_data['x'], accel_data['y'], accel_data['z']
            gx, gy, gz = np.radians([gyro_data['x'], gyro_data['y'], gyro_data['z']])  # Перевод в радианы
    
            kf.predict([gx, gy, gz])
            kf.update([ax, ay, az])
    
            linear_acceleration = kf.get_linear_acceleration()
            print("Linear Acceleration:", linear_acceleration)
    
            time.sleep(1.0 / sample_freq)
    
    except KeyboardInterrupt:
        print("Measurement stopped by user")
    

    Основные моменты:

    • Интерфейс I2C: Чтобы использовать датчик MPU6050, ваш микрокомпьютер (например, Raspberry Pi) должен поддерживать I2C и иметь соответствующие подключения.

    • Настройка библиотеки: Пример использует mpu6050-raspberrypi для доступа к данным акселерометра и гироскопа. Убедитесь, что адрес I2C указан правильно. По умолчанию адрес для MPU6050 — 0x68.

    • Образец кода: Он читает данные с датчика, передает их в фильтр Калмана и периодически выводит оценивание линейного ускорения с учетом компенсированной гравитации.

    Перед использованием убедитесь, что датчик корректно подключен и настроена соответствующая конфигурация I2C. Кроме того, фильтр может нуждаться в тонкой настройке коэффициентов, чтобы обеспечить оптимальную производительность в зависимости от конкретного применения и условий эксплуатации.

  11. User

    Есть программа, реализующая оценку ускорения с датчика MPU6050 через фильтр Махони. Однако ускорение накапливается. Если пошевелить датчик, то она покажет ускорение. При остановке датчика покажет ноль, а затем если опять пошевелить датчик, она покажет ускорение, но с учетом последнего ускорение от предыдущего шевеления. Как это исправить? Код программы: import numpy as np from scipy import signal from mpu6050 import mpu6050 import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation import time

    def low_pass_filter(new_data, prev_filtered_data, alpha=0.95): return alpha * new_data + (1 - alpha) * prev_filtered_data

    class MahonyFilter: def init(self, kp=2.0, ki=0.0): self.q = np.array([1.0, 0.0, 0.0, 0.0]) # Инициализация кватерниона self.kp = kp self.ki = ki self.integral_error = np.zeros(3)

    def update(self, gyro, accel, dt):
        accel_norm = np.linalg.norm(accel)
        if accel_norm == 0:
            return
        accel = accel / accel_norm
    
        v = np.array([2*(self.q[1]*self.q[3] - self.q[0]*self.q[2]),
                      2*(self.q[0]*self.q[1] + self.q[2]*self.q[3]),
                      2*(0.5 - self.q[1]*self.q[1] - self.q[2]*self.q[2])])
        error = np.cross(v, accel)
        self.integral_error += self.ki * error * dt
        gyro = gyro + self.kp * error + self.integral_error
    
        q_dot = 0.5 * np.array([
            0.0,             -gyro[0], -gyro[1], -gyro[2],
            gyro[0],  0.0,              gyro[2], -gyro[1],
            gyro[1], -gyro[2],  0.0,             gyro[0],
            gyro[2],  gyro[1], -gyro[0],  0.0
        ]).reshape((4, 4)).dot(self.q)
    
        self.q += q_dot * dt
        self.q /= np.linalg.norm(self.q)
    
    def get_quaternion(self):
        return self.q
    

    def calibrate_sensors(sensor, samples=100): gyro_offsets = [] accel_offsets = []

    for _ in range(samples):
        data = sensor.get_all_data()
        
        gyro_offsets.append(np.radians([data[1]['x'] * 250 / 16384, data[1]['y'] * 250 / 16384, data[1]['z'] * 250 / 16384]))
        accel_offsets.append([data[0]['x'] / 16384, data[0]['y'] / 16384, data[0]['z'] / 16384])
        time.sleep(0.01)
    
    gyro_offset = np.mean(gyro_offsets, axis=0)
    
    accel_offset = np.mean(accel_offsets, axis=0) - np.array([0, 0, 9.81])
    return gyro_offset, accel_offset
    

    def detect_stationary(gyro, accel, gyro_threshold=np.radians(0.15), accel_threshold=0.03): gyro_magnitude = np.linalg.norm(gyro) accel_magnitude = np.linalg.norm(accel) - 9.81 # Исключаем эффект гравитации # ~ print(accel_magnitude) return gyro_magnitude < gyro_threshold and abs(accel_magnitude) < accel_threshold

    def main(): sensor = mpu6050(0x68)

    # Калибровка
    gyro_offset, accel_offset = calibrate_sensors(sensor)
    
    # Инициализация
    velocity = np.zeros(3)
    mahony = MahonyFilter()
    
    prev_filtered_gyro = np.zeros(3)
    prev_filtered_accel = np.zeros(3)
    
    dt = 0.01
    fig, ax = plt.subplots()
    ax.set_xlim(0, 100)
    
    line, = ax.plot(np.zeros(100), label="Velocity")
    
    velocity_data = []
    accel_data = []
    
    
    
    def update_plot(frame):
        nonlocal prev_filtered_gyro, prev_filtered_accel, velocity
    
        data = sensor.get_all_data()
        gyro_raw = np.array(np.radians([data[1]['x'] * 250 / 16384, data[1]['y'] * 250 / 16384, data[1]['z'] * 250 / 16384])) - gyro_offset
        accel_raw = np.array([data[0]['x'] / 16384, data[0]['y'] / 16384, data[0]['z'] / 16384]) - accel_offset
        
        
        # ~ print('Raw accel')
        # ~ print(accel_raw)
        print('Raw gyro')
        print(gyro_raw)
    
        # Фильтрация данных
        filtered_gyro = low_pass_filter(gyro_raw, prev_filtered_gyro)
        filtered_accel = low_pass_filter(accel_raw, prev_filtered_accel)
        # ~ filtered_gyro = gyro_raw
        # ~ filtered_accel = accel_raw
        
        print('filtered_accel: ', filtered_accel)
    
        if detect_stationary(filtered_gyro, filtered_accel):
            velocity = np.zeros(3)  # Обнуляем скорость, если устройство неподвижно
            net_accel = np.zeros(3)
            # ~ filtered_gyro = np.array([0, 0, 0])
            # ~ filtered_accel = np.array([0, 0, 0])
        else:
    
            # Обновление фильтра Махони
            mahony.update(filtered_gyro, filtered_accel, dt)
    
            # Получение ориентации и компенсация гравитации
            q = mahony.get_quaternion()
            # ~ g_estimate = 2 * np.array([
                    # ~ q[1] * q[3] - q[0] * q[2],
                    # ~ q[0] * q[1] + q[2] * q[3],
                    # ~ 0.5 - q[1]*q[1] - q[2]*q[2]
                # ~ ]) * 9.81
                
            w = q[0]
            x = q[1]
            y = q[2]
            z = q[3]
            
            g_estimate = np.array([
                        [1 - 2*y**2 - 2*z**2, 2*x*y - 2*z*w, 2*x*z + 2*y*w],
                        [2*x*y + 2*z*w, 1 - 2*x**2 - 2*z**2, 2*y*z - 2*x*w],
                        [2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x**2 - 2*y**2]
                        ]) @ np.array([0, 0, 9.81])
            
            net_accel = filtered_accel - g_estimate
    
            # Обновление скорости
            velocity += net_accel * dt
            
        
    
        velocity_data.append(np.linalg.norm(velocity))
        accel_data.append(np.linalg.norm(net_accel))
        
        print('net_accel')
        print(net_accel)
        print()
        
        if len(velocity_data) > 100:
            velocity_data.pop(0)
        
        if len(net_accel) > 100:
            accel_data.pop(0)
    
        
        line.set_ydata(accel_data)
        line.set_xdata(np.arange(len(accel_data)))
        
        # ~ line.set_ydata(velocity_data)
        # ~ line.set_xdata(np.arange(len(velocity_data)))
    
        prev_filtered_gyro = filtered_gyro
        prev_filtered_accel = filtered_accel
        print()
        print('prev values:')
        print(prev_filtered_accel)
        print(prev_filtered_gyro)
        print()
        
        ax.set_ylim(min(accel_data)-3, max(accel_data)+5)
        ax.set_xlim(0, len(accel_data)+5)
    
        return line,
    
    ani = FuncAnimation(fig, update_plot, blit=True, interval=dt*1000)
    plt.legend()
    plt.show()
    

    if name == "main": main()

  12. Assistant

    Замечено, что в вашем коде происходит накопление ошибок в скорости, что может быть связано с несколькими вопросами:

    1. Неточная калибровка и дрифт: Даже после калибровки гироскопов и акселерометров могут возникать небольшие ошибки, которые складываются с течением времени.

    2. Выход за пределы при определении состояния покоя: Убедитесь, что вы правильно обнуляете скорость, когда устройство находится в покое.

    3. Накопление небольших ошибок в фильтре: Малейшая систематическая ошибка в расчетах, если ее своевременно не корректировать, также будет склонна быстро накапливаться.

    Давайте рассмотрим улучшения, которые помогут уменьшить накопление ошибок в этом контексте:

    • Обновление обнуления скорости: Обнуляйте скорость только в случае уверенного определения состояния покоя, что предотвращает накопление погрешностей.

    • Улучшите фильтрацию данных: Убедитесь, что фильтрация данных выходного сигнала достаточно агрессивна, чтобы удалить дрифт.

    Вот обновленный код с рекомендациями по улучшению:

    import numpy as np
    from scipy import signal
    from mpu6050 import mpu6050
    import matplotlib.pyplot as plt
    from matplotlib.animation import FuncAnimation
    import time
    
    def low_pass_filter(new_data, prev_filtered_data, alpha=0.95):
        return alpha * new_data + (1 - alpha) * prev_filtered_data
    
    class MahonyFilter:
        def __init__(self, kp=2.0, ki=0.0):
            self.q = np.array([1.0, 0.0, 0.0, 0.0])  # инициализация кватерниона
            self.kp = kp
            self.ki = ki
            self.integral_error = np.zeros(3)
    
        def update(self, gyro, accel, dt):
            accel_norm = np.linalg.norm(accel)
            if accel_norm == 0:
                return
            accel = accel / accel_norm
    
            v = np.array([
                2*(self.q[1]*self.q[3] - self.q[0]*self.q[2]),
                2*(self.q[0]*self.q[1] + self.q[2]*self.q[3]),
                2*(0.5 - self.q[1]*self.q[1] - self.q[2]*self.q[2])
            ])
            error = np.cross(v, accel)
            self.integral_error += self.ki * error * dt
            gyro = gyro + self.kp * error + self.integral_error
    
            q_dot = 0.5 * np.array([
                -self.q[1] * gyro[0] - self.q[2] * gyro[1] - self.q[3] * gyro[2],
                 self.q[0] * gyro[0] + self.q[2] * gyro[2] - self.q[3] * gyro[1],
                 self.q[0] * gyro[1] - self.q[1] * gyro[2] + self.q[3] * gyro[0],
                 self.q[0] * gyro[2] + self.q[1] * gyro[1] - self.q[2] * gyro[0]
            ])
    
            self.q += q_dot * dt
            self.q /= np.linalg.norm(self.q)
    
        def get_gravity_vector(self):
            q = self.q
            g = np.array([
                2 * (q[1]*q[3] - q[0]*q[2]),
                2 * (q[0]*q[1] + q[2]*q[3]),
                q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]
            ])
            return g
    
    def calibrate_sensors(sensor, samples=100):
        gyro_offsets = []
        accel_offsets = []
    
        for _ in range(samples):
            data = sensor.get_all_data()
            gyro_offsets.append([
                np.radians(data[1]['x'] / 131.0),
                np.radians(data[1]['y'] / 131.0),
                np.radians(data[1]['z'] / 131.0)
            ])
            accel_offsets.append([
                data[0]['x'] / 16384,
                data[0]['y'] / 16384,
                data[0]['z'] / 16384
            ])
            time.sleep(0.01)
    
        gyro_offset = np.mean(gyro_offsets, axis=0)
        accel_offset = np.mean(accel_offsets, axis=0)
        accel_offset[2] -= 1  # компенсировать гравитацию
        return gyro_offset, accel_offset
    
    def detect_stationary(gyro, accel, gyro_threshold=np.radians(0.15), accel_threshold=0.03):
        gyro_magnitude = np.linalg.norm(gyro)
        accel_magnitude = np.linalg.norm(accel) - 1.0  # Исключаем эффект гравитации
        return gyro_magnitude < gyro_threshold and abs(accel_magnitude) < accel_threshold
    
    def main():
        sensor = mpu6050(0x68)
    
        # Калибровка
        gyro_offset, accel_offset = calibrate_sensors(sensor)
    
        # Инициализация переменных
        velocity = np.zeros(3)
        mahony = MahonyFilter()
    
        prev_filtered_gyro = np.zeros(3)
        prev_filtered_accel = np.zeros(3)
    
        dt = 0.01
        fig, ax = plt.subplots()
        ax.set_xlim(0, 100)
        ax.set_ylim(-0.2, 0.2)
        line, = ax.plot(np.zeros(100), label="Velocity")
    
        velocity_data = []
    
        def update_plot(frame):
            nonlocal prev_filtered_gyro, prev_filtered_accel, velocity
    
            data = sensor.get_all_data()
            gyro_raw = np.array([
                np.radians(data[1]['x'] / 131.0) - gyro_offset[0],
                np.radians(data[1]['y'] / 131.0) - gyro_offset[1],
                np.radians(data[1]['z'] / 131.0) - gyro_offset[2]
            ])
            accel_raw = np.array([
                data[0]['x'] / 16384,
                data[0]['y'] / 16384,
                data[0]['z'] / 16384
            ]) - accel_offset
    
            filtered_gyro = low_pass_filter(gyro_raw, prev_filtered_gyro, alpha=0.8)
            filtered_accel = low_pass_filter(accel_raw, prev_filtered_accel, alpha=0.8)
    
            if detect_stationary(filtered_gyro, filtered_accel):
                velocity = np.zeros(3)  # Обнуляем скорость, если устройство неподвижно
            else:
                # Обновление фильтра Махони
                mahony.update(filtered_gyro, filtered_accel, dt)
    
                # Получение ориентации и компенсация гравитации
                g_estimate = mahony.get_gravity_vector()
                net_accel = filtered_accel - g_estimate
    
                # Обновление скорости
                velocity += net_accel * dt
    
            velocity_data.append(np.linalg.norm(velocity))
    
            if len(velocity_data) > 100:
                velocity_data.pop(0)
    
            line.set_ydata(velocity_data)
            line.set_xdata(np.arange(len(velocity_data)))
    
            prev_filtered_gyro = filtered_gyro
            prev_filtered_accel = filtered_accel
    
            return line,
    
        ani = FuncAnimation(fig, update_plot, blit=True, interval=dt*1000)
        plt.legend()
        plt.show()
    
    if __name__ == "__main__":
        main()
    

    Объяснения улучшений:

    • Реализация get_gravity_vector: Более точно рассчитывает вектор гравитации из текущей ориентации, определенной фильтром Махони.
    • Обнуление скорости при покое: Только когда уверены, что устройство действительно в состоянии покоя, будет сбрасываться скорость.
    • Улучшенная фильтрация: Ускорения и угловые скорости проходят через несколько сглаживающих фильтров, чтобы уменьшить шумы.

    Этот код направлен на сокращение накопления ошибок, гарантируя, что система обнуляет накопленные ошибки, когда определяет, что устройство неподвижно. Вы можете добавить другие стратегии фильтрации и коррекции ошибок в зависимости от специфики применения и данных.

  13. User

    Мне нужно рисовать не скорость, а ускорение

  14. Assistant

    Если вам нужно отобразить линейное ускорение, а не скорость, в вашей системе, вы можете сделать это, просто изменив код, чтобы сохранять и отображать линейное ускорение после компенсации гравитации. Это даст вам визуализацию изменений ускорения при движении датчика.

    Вот обновленный код, который отображает линейное ускорение вместо скорости:

    import numpy as np
    from scipy import signal
    from mpu6050 import mpu6050
    import matplotlib.pyplot as plt
    from matplotlib.animation import FuncAnimation
    import time
    
    def low_pass_filter(new_data, prev_filtered_data, alpha=0.95):
        return alpha * new_data + (1 - alpha) * prev_filtered_data
    
    class MahonyFilter:
        def __init__(self, kp=2.0, ki=0.0):
            self.q = np.array([1.0, 0.0, 0.0, 0.0])  # Инициализация кватерниона
            self.kp = kp
            self.ki = ki
            self.integral_error = np.zeros(3)
    
        def update(self, gyro, accel, dt):
            accel_norm = np.linalg.norm(accel)
            if accel_norm == 0:
                return
            accel = accel / accel_norm
    
            v = np.array([
                2*(self.q[1]*self.q[3] - self.q[0]*self.q[2]),
                2*(self.q[0]*self.q[1] + self.q[2]*self.q[3]),
                2*(0.5 - self.q[1]*self.q[1] - self.q[2]*self.q[2])
            ])
            error = np.cross(v, accel)
            self.integral_error += self.ki * error * dt
            gyro = gyro + self.kp * error + self.integral_error
    
            q_dot = 0.5 * np.array([
                -self.q[1] * gyro[0] - self.q[2] * gyro[1] - self.q[3] * gyro[2],
                 self.q[0] * gyro[0] + self.q[2] * gyro[2] - self.q[3] * gyro[1],
                 self.q[0] * gyro[1] - self.q[1] * gyro[2] + self.q[3] * gyro[0],
                 self.q[0] * gyro[2] + self.q[1] * gyro[1] - self.q[2] * gyro[0]
            ])
    
            self.q += q_dot * dt
            self.q /= np.linalg.norm(self.q)
    
        def get_gravity_vector(self):
            q = self.q
            g = np.array([
                2 * (q[1]*q[3] - q[0]*q[2]),
                2 * (q[0]*q[1] + q[2]*q[3]),
                q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]
            ])
            return g
    
    def calibrate_sensors(sensor, samples=100):
        gyro_offsets = []
        accel_offsets = []
    
        for _ in range(samples):
            data = sensor.get_all_data()
            gyro_offsets.append([
                np.radians(data[1]['x'] / 131.0),
                np.radians(data[1]['y'] / 131.0),
                np.radians(data[1]['z'] / 131.0)
            ])
            accel_offsets.append([
                data[0]['x'] / 16384,
                data[0]['y'] / 16384,
                data[0]['z'] / 16384
            ])
            time.sleep(0.01)
    
        gyro_offset = np.mean(gyro_offsets, axis=0)
        accel_offset = np.mean(accel_offsets, axis=0)
        accel_offset[2] -= 1  # компенсировать гравитацию
        return gyro_offset, accel_offset
    
    def detect_stationary(gyro, accel, gyro_threshold=np.radians(0.15), accel_threshold=0.03):
        gyro_magnitude = np.linalg.norm(gyro)
        accel_magnitude = np.linalg.norm(accel) - 1.0  # Исключаем эффект гравитации
        return gyro_magnitude < gyro_threshold and abs(accel_magnitude) < accel_threshold
    
    def main():
        sensor = mpu6050(0x68)
    
        # Калибровка
        gyro_offset, accel_offset = calibrate_sensors(sensor)
    
        # Инициализация
        mahony = MahonyFilter()
    
        prev_filtered_gyro = np.zeros(3)
        prev_filtered_accel = np.zeros(3)
    
        dt = 0.01
        fig, ax = plt.subplots()
        ax.set_xlim(0, 100)
        ax.set_ylim(-2, 2)
        line, = ax.plot(np.zeros(100), label="Linear Acceleration (m/s²)")
    
        accel_data = []
    
        def update_plot(frame):
            nonlocal prev_filtered_gyro, prev_filtered_accel
    
            data = sensor.get_all_data()
            gyro_raw = np.array([
                np.radians(data[1]['x'] / 131.0) - gyro_offset[0],
                np.radians(data[1]['y'] / 131.0) - gyro_offset[1],
                np.radians(data[1]['z'] / 131.0) - gyro_offset[2]
            ])
            accel_raw = np.array([
                data[0]['x'] / 16384,
                data[0]['y'] / 16384,
                data[0]['z'] / 16384
            ]) - accel_offset
    
            filtered_gyro = low_pass_filter(gyro_raw, prev_filtered_gyro, alpha=0.8)
            filtered_accel = low_pass_filter(accel_raw, prev_filtered_accel, alpha=0.8)
    
            # Обновление фильтра Махони
            mahony.update(filtered_gyro, filtered_accel, dt)
    
            # Получение ориентации и компенсация гравитации
            g_estimate = mahony.get_gravity_vector()
            net_accel = filtered_accel - g_estimate
    
            # Сохранение линейного ускорения
            accel_data.append(np.linalg.norm(net_accel))
    
            if len(accel_data) > 100:
                accel_data.pop(0)
    
            line.set_ydata(accel_data)
            line.set_xdata(np.arange(len(accel_data)))
    
            prev_filtered_gyro = filtered_gyro
            prev_filtered_accel = filtered_accel
    
            return line,
    
        ani = FuncAnimation(fig, update_plot, blit=True, interval=dt*1000)
        plt.legend()
        plt.show()
    
    if __name__ == "__main__":
        main()
    

    Объяснение:

    • Мы отрисовываем линейное ускорение (accel_data), оставляя отфильтрованные данные акселерометра от долгосрочного вклада гравитации (предоставляемого кватернионным фильтром Махони).
    • Графика будет представлять изменение линейного ускорения в течение времени, что дает более точное понимание, как устройство движется в пространстве.
    • Убедитесь, что датчик надежно калиброван перед использованием, это существенно влияет на качество компенсированных оценок ускорения.
  15. User

    Почему при компенсации ускорения свободного падения вычитается единица, а не 9.8?

  16. Assistant

    В программах и расчетах, связанных с акселерометрами, величина 1 часто используется для представления ускорения свободного падения (гравитационного ускорения), и эта величина измеряется в g — размерной единице ускорения, равной 9.81 м/с².

    По умолчанию, многие акселерометры возвращают данные в виде величины, нормированной на единицу g. Это значит, что если акселерометр неподвижен и его чувствительная ось направлена вдоль гравитационного поля, сенсор покажет значение, близкое к ±1, что соответствует ±9.81 м/с² в более привычных для нас единицах измерения (метры в секунду в квадрате).

    Таким образом, если вы вычитаете единицу в ваших расчетах, это соответствует корректировке ускорения свободного падения в тех данных, которые представлены в g. Если ваши данные акселерометра уже нормализованы на 1g, то вычитание 1 обнуляет гравитационное ускорение. Если же они представлены в м/с², необходимо вычесть 9.81.

    В вашем предыдущем коде, данные акселерометра делились на 16384 (что соответствует количеству единиц в гравитации для 16-битного акселерометра), чтобы нормализовать показания в g. Это также связано с тем, что вычитание единицы выглядело бы более подходящим в этой ситуации. Если ваши данные сразу в м/с², то вы должны вычитать 9.81 для правильной компенсации.

  17. User

    Чему равно среднее значение ускорения при движении человека?

  18. Assistant

    Среднее ускорение человека при движении сильно зависит от вида активности и скорости выполнения. Разные виды движений, такие как ходьба, бег, прыжки или езда на велосипеде, имеют разные профили ускорения. Вот некоторые приблизительные значения:

    1. Ходьба: При ходьбе ускорения обычно находятся в пределах от 0.5 до 1.5 м/с². В моменты, когда ноги касаются земли или поднимаются, могут наблюдаться небольшие пики ускорения.

    2. Бег: Ускорения при беге могут варьироваться от 1.5 до 5 м/с² или даже выше, в зависимости от интенсивности и скорости бега.

    3. Прыжки: В момент отталкивания и приземления ускорение может кратковременно достигать значительно больших значений, вплоть до 10 м/с² и более.

    4. Езда на велосипеде: Это зависит от скорости и усилий, но часто ускорения на велосипеде ниже, чем при беге, и могут находиться в пределах 0.5-2 м/с², за исключением моментов начала движения или резкого торможения.

    Эти значения — лишь ориентиры и могут существенно варьироваться в зависимости от личных особенностей физической активности, условий движения и использования различных устройств для измерения.

  19. User

    MPU6050 sensor.get_all_data() в каких единицах измерения выдает значения

  20. Assistant

    Сенсор MPU6050 выдает сырые данные, которые представляют собой 16-битные значения из регистров акселерометра и гироскопа. Эти значения необходимо преобразовать в физические величины с соответствующими единицами через деление на коэффициенты чувствительности.

    Акселерометр

    • Сырой выход: 16-битное целочисленное значение от -32768 до 32767.

    • Единицы физической величины: Обычно измеряется в "g" (ускорение свободного падения), где 1g = 9.81 м/с².

    • Чувствительность: Наиболее распространенные уровни настраиваемой чувствительности для акселерометра в MPU6050 — ±2g, ±4g, ±8g, и ±16g. Например, если акселерометр установлен на ±2g:

      • Для диапазона ±2g: 1g соответствует 16384 (т.е. 32768 / 2) единицам.
      • Чтобы преобразовать в "g", значения делятся на 16384, если настроено на ±2g.

    Гироскоп

    • Сырой выход: 16-битное целочисленное значение от -32768 до 32767.

    • Единицы физической величины: Обычно измеряется в градусах в секунду (°/s).

    • Чувствительность: Наиболее распространенные настроенные уровни чувствительности для гироскопа — ±250, ±500, ±1000, ±2000 °/s:

      • Для диапазона ±250 °/s: 1 °/s соответствует примерно 131 единице (т.е. 32768 / 250).
      • Чтобы преобразовать в °/s, значения делятся на эти коэффициенты.

    Пример преобразования

    Когда вы получите данные с MPU6050 через sensor.get_all_data(), вы, по сути, получите сырые значения, которые нужно преобразовать, чтобы привести их к физически значимым показателям.

    Пример:

    data = sensor.get_all_data()
    accel_raw = data[0]  # Сырые данные акселерометра
    gyro_raw = data[1]   # Сырые данные гироскопа
    
    # Предполагаем, что акселерометр настроен на ±2g и гироскоп на ±250 °/s
    
    # Преобразование акселерометра в g
    accel_g = {
        'x': accel_raw['x'] / 16384.0,
        'y': accel_raw['y'] / 16384.0,
        'z': accel_raw['z'] / 16384.0
    }
    
    # Преобразование гироскопа в °/s
    gyro_dps = {
        'x': gyro_raw['x'] / 131.0,
        'y': gyro_raw['y'] / 131.0,
        'z': gyro_raw['z'] / 131.0
    }
    

    Полученные преобразованные значения можно использовать для анализа или дальнейших вычислений, таких как оценка ориентации или измерение линейного ускорения.

  21. User

    Как изменить подпись к графику в приведенной мной программе?

  22. Assistant

    Чтобы изменить подпись к графику в программе, вы можете изменить параметры для функции визуализации matplotlib. В вашем коде вы используете matplotlib для отображения данных. Вот места, где можно изменить подписи:

    1. Изменение подписи к графику: Если вы хотите изменить текстовое описание, отображаемое для линии на графике, вам нужно изменить label параметр в функции plot.

    2. Изменение заголовка графика: Чтобы задать заголовок для всего графика, используйте функцию plt.title.

    3. Изменение подписи к осям: Вы можете изменить описание для обоих осей с помощью plt.xlabel и plt.ylabel.

    Пример обновленного кода с изменением подписи графика:

    import numpy as np
    from scipy import signal
    from mpu6050 import mpu6050
    import matplotlib.pyplot as plt
    from matplotlib.animation import FuncAnimation
    import time
    
    # ... ваш существующий код ...
    
    def main():
        sensor = mpu6050(0x68)
    
        # Инициализация и калибровка
        gyro_offset, accel_offset = calibrate_sensors(sensor)
        mahony = MahonyFilter()
    
        prev_filtered_gyro = np.zeros(3)
        prev_filtered_accel = np.zeros(3)
    
        dt = 0.01
        fig, ax = plt.subplots()
    
        # Установите пределы осей
        ax.set_xlim(0, 100)
        ax.set_ylim(-2, 2)
    
        # Установите подписи
        ax.set_title("Линейное ускорение с MPU6050")  # Заголовок графика
        ax.set_xlabel("Время (кадры)")
        ax.set_ylabel("Ускорение (m/s²)")
    
        line, = ax.plot(np.zeros(100), label="Линейное Ускорение")  # Измените подпись линии
    
        accel_data = []
    
        def update_plot(frame):
            nonlocal prev_filtered_gyro, prev_filtered_accel
    
            data = sensor.get_all_data()
            gyro_raw = np.array([
                np.radians(data[1]['x'] / 131.0) - gyro_offset[0],
                np.radians(data[1]['y'] / 131.0) - gyro_offset[1],
                np.radians(data[1]['z'] / 131.0) - gyro_offset[2]
            ])
            accel_raw = np.array([
                data[0]['x'] / 16384,
                data[0]['y'] / 16384,
                data[0]['z'] / 16384
            ]) - accel_offset
    
            filtered_gyro = low_pass_filter(gyro_raw, prev_filtered_gyro, alpha=0.8)
            filtered_accel = low_pass_filter(accel_raw, prev_filtered_accel, alpha=0.8)
    
            mahony.update(filtered_gyro, filtered_accel, dt)
            g_estimate = mahony.get_gravity_vector()
            net_accel = filtered_accel - g_estimate
    
            accel_data.append(np.linalg.norm(net_accel))
    
            if len(accel_data) > 100:
                accel_data.pop(0)
    
            line.set_ydata(accel_data)
            line.set_xdata(np.arange(len(accel_data)))
    
            prev_filtered_gyro = filtered_gyro
            prev_filtered_accel = filtered_accel
    
            return line,
    
        ani = FuncAnimation(fig, update_plot, blit=True, interval=dt*1000)
        plt.legend()
        plt.show()
    
    if __name__ == "__main__":
        main()
    

    Обратите внимание на изменения в функциях ax.set_title, ax.set_xlabel, и ax.set_ylabel, а также на настройку label в функции plot, чтобы изменить подпись линии. Эти изменения позволят вам задавать собственные заголовки и описание осей для вашей визуализации данных.

  23. User

    А если нужно добавить текст на график снизу так чтобы на каждый кадр он менялся, выводя текущее ускорение

  24. Assistant

    Чтобы добавить текст, который будет обновляться в каждом кадре анимации и отображать текущее ускорение, вы можете использовать объект text из matplotlib. Этот объект можно обновлять в функции update_plot, как и линию графика. В данном случае текст будет отображать текущее значение линейного ускорения в каждом кадре.

    Вот пример, как это можно сделать:

    import numpy as np
    from mpu6050 import mpu6050
    import matplotlib.pyplot as plt
    from matplotlib.animation import FuncAnimation
    import time
    
    def low_pass_filter(new_data, prev_filtered_data, alpha=0.95):
        return alpha * new_data + (1 - alpha) * prev_filtered_data
    
    class MahonyFilter:
        def __init__(self, kp=2.0, ki=0.0):
            self.q = np.array([1.0, 0.0, 0.0, 0.0])
            self.kp = kp
            self.ki = ki
            self.integral_error = np.zeros(3)
    
        def update(self, gyro, accel, dt):
            accel_norm = np.linalg.norm(accel)
            if accel_norm == 0:
                return
            accel = accel / accel_norm
    
            v = np.array([
                2*(self.q[1]*self.q[3] - self.q[0]*self.q[2]),
                2*(self.q[0]*self.q[1] + self.q[2]*self.q[3]),
                2*(0.5 - self.q[1]*self.q[1] - self.q[2]*self.q[2])
            ])
            error = np.cross(v, accel)
            self.integral_error += self.ki * error * dt
            gyro = gyro + self.kp * error + self.integral_error
    
            q_dot = 0.5 * np.array([
                -self.q[1] * gyro[0] - self.q[2] * gyro[1] - self.q[3] * gyro[2],
                 self.q[0] * gyro[0] + self.q[2] * gyro[2] - self.q[3] * gyro[1],
                 self.q[0] * gyro[1] - self.q[1] * gyro[2] + self.q[3] * gyro[0],
                 self.q[0] * gyro[2] + self.q[1] * gyro[1] - self.q[2] * gyro[0]
            ])
    
            self.q += q_dot * dt
            self.q /= np.linalg.norm(self.q)
    
        def get_gravity_vector(self):
            q = self.q
            g = np.array([
                2 * (q[1]*q[3] - q[0]*q[2]),
                2 * (q[0]*q[1] + q[2]*q[3]),
                q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]
            ])
            return g
    
    def calibrate_sensors(sensor, samples=100):
        gyro_offsets = []
        accel_offsets = []
    
        for _ in range(samples):
            data = sensor.get_all_data()
            gyro_offsets.append([
                np.radians(data[1]['x'] / 131.0),
                np.radians(data[1]['y'] / 131.0),
                np.radians(data[1]['z'] / 131.0)
            ])
            accel_offsets.append([
                data[0]['x'] / 16384,
                data[0]['y'] / 16384,
                data[0]['z'] / 16384
            ])
            time.sleep(0.01)
    
        gyro_offset = np.mean(gyro_offsets, axis=0)
        accel_offset = np.mean(accel_offsets, axis=0)
        accel_offset[2] -= 1  # компенсировать гравитацию
        return gyro_offset, accel_offset
    
    def main():
        sensor = mpu6050(0x68)
    
        # Калибровка
        gyro_offset, accel_offset = calibrate_sensors(sensor)
    
        # Инициализация
        mahony = MahonyFilter()
    
        prev_filtered_gyro = np.zeros(3)
        prev_filtered_accel = np.zeros(3)
    
        dt = 0.01
        fig, ax = plt.subplots()
    
        # Установите пределы осей
        ax.set_xlim(0, 100)
        ax.set_ylim(-2, 2)
    
        # Установите подписи
        ax.set_title("Линейное ускорение с MPU6050")
        ax.set_xlabel("Время (кадры)")
        ax.set_ylabel("Ускорение (m/s²)")
    
        line, = ax.plot(np.zeros(100), label="Линейное Ускорение")
        accel_text = ax.text(0.5, -2.5, "", transform=ax.transAxes, fontsize=12, ha='center')  # добавление текста ниже оси
    
        accel_data = []
    
        def update_plot(frame):
            nonlocal prev_filtered_gyro, prev_filtered_accel
    
            data = sensor.get_all_data()
            gyro_raw = np.array([
                np.radians(data[1]['x'] / 131.0) - gyro_offset[0],
                np.radians(data[1]['y'] / 131.0) - gyro_offset[1],
                np.radians(data[1]['z'] / 131.0) - gyro_offset[2]
            ])
            accel_raw = np.array([
                data[0]['x'] / 16384,
                data[0]['y'] / 16384,
                data[0]['z'] / 16384
            ]) - accel_offset
    
            filtered_gyro = low_pass_filter(gyro_raw, prev_filtered_gyro, alpha=0.8)
            filtered_accel = low_pass_filter(accel_raw, prev_filtered_accel, alpha=0.8)
    
            mahony.update(filtered_gyro, filtered_accel, dt)
            g_estimate = mahony.get_gravity_vector()
            net_accel = filtered_accel - g_estimate
    
            current_acceleration = np.linalg.norm(net_accel)
            accel_data.append(current_acceleration)
    
            if len(accel_data) > 100:
                accel_data.pop(0)
    
            line.set_ydata(accel_data)
            line.set_xdata(np.arange(len(accel_data)))
    
            accel_text.set_text(f"Текущее ускорение: {current_acceleration:.2f} m/s²")  # обновление текста
    
            prev_filtered_gyro = filtered_gyro
            prev_filtered_accel = filtered_accel
    
            return line, accel_text
    
        ani = FuncAnimation(fig, update_plot, blit=True, interval=dt*1000)
        plt.legend()
        plt.show()
    
    if __name__ == "__main__":
        main()
    

    Примечания по коду:

    • ax.text используется для создания текста под графиком. Параметр transform=ax.transAxes гарантирует, что текст отрисовывается в координатах относительно области осей, а не данных. ha='center' центрирует текст по горизонтали.
    • Текст обновляется в каждом кадре посредством функции set_text, где current_acceleration — это величина текущего линейного ускорения.
  25. User

    Как написать в строке м/с^2

  26. Assistant

    Чтобы отобразить единицы измерения ( \text{м/с}^2 ) (метры в секунду в квадрате) в текстовой строке в Python, вы можете использовать Unicode для правильного представления степени. В частности, символ ² имеет код \u00B2.

    Вот как можно отобразить текст с этими единицами, включая квадрат:

     "м/с²"
    

    Этот код корректно отобразит "м/с²". Однако, если вам нужно это сделать в строковом отображении в matplotlib или где-либо еще, этот же подход будет работать. Например:

    plt.ylabel("Ускорение (м/с²)")
    

    Символ ² представляет собой один знак Unicode и корректно отобразится в большинстве современных систем и текстовых рендереров, включая графические интерфейсы в matplotlib.