Фильтр Калмана/фильтр Маджвика

@Fury67 · Регистрация: 12.11.2017

Студворк — интернет-сервис помощи студентам

Доброго времени суток!

Если кто-нибудь когда-нибудь реализовывал на python обработку угловых скоростей (гироскоп трех осевой) и линейных ускорений(акселерометр трех осевой) в виде углов тангажа/крена/рыскания, используя фильтр Калмана/Маджвика, можете поделиться кодом?
А то что-то не получается собственная реализация. Пока что более менее получилось только получить углы из ускорений без всяких фильтраций.

@Veseliy Hakker · 21.09.2023, 23:32

Сообщение от Fury67

Если кто-нибудь когда-нибудь реализовывал на python обработку угловых скоростей (гироскоп трех осевой) и линейных ускорений(акселерометр трех осевой) в виде углов тангажа/крена/рыскания, используя фильтр Калмана/Маджвика, можете поделиться кодом?
А то что-то не получается собственная реализация. Пока что более менее получилось только получить углы из ускорений без всяких фильтраций.

Если накидаешь ссылок и формул то напишу тебе все что надо и даже больше. По одному описанию решать задачу сложно.

@Fury67 · 02.10.2023, 12:47 **[ТС]**

В общем. Уже недели две борюсь с вычислением углом в пространстве на основе показаний акселерометра и гироскопа.

Есть реализованная функция возвращающая данные с датчиков get_data() в таком виде np.array([accel_x, accel_y, accel_z]), np.array([gyro_x, gyro_y, gyro_z]).

Была сделана попытка реализации комплиментарного фильтра:

Python
1
2
3
4
5
6
7
8
9
10
11
12
13
def complementary_filter(acceleration, gyroscope, dt, pitch, roll, alpha=0.02):
    acceleration /= np.linalg.norm(acceleration)
 
    pitch_acc = math.atan2(acceleration[0], math.sqrt(acceleration[1]**2 + acceleration[2]**2))
    roll_acc = math.atan2(-acceleration[1], acceleration[2])
 
    pitch_gyro = pitch + gyroscope[0] * dt
    roll_gyro = roll + gyroscope[1] * dt
    
    pitch = alpha * pitch_gyro + (1 - alpha) * pitch_acc
    roll = alpha * roll_gyro + (1 - alpha) * roll_acc
 
    return pitch, roll

Она, к сожалению, очень чувствительна получается к движению датчика.

Далее была попытка создания фильтра калмана:

Python
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
# Матрицы для фильтра Калмана
Q_angle = 0.001
Q_bias = 0.003
R_measure = 0.03
 
Q_angle = 0.01
Q_bias = 0.003
R_measure = 0.03
 
# Создание объекта фильтра Калмана
kf = KalmanFilter(dim_x=2, dim_z=2)
 
# Определение начальных состояний и матрицы ковариации
kf.x = np.array([initial_pitch, initial_roll])
kf.P = np.eye(2)
 
# Определение матриц A и H (модель системы и измерений)
kf.F = np.eye(2)  # Модель системы - единичная матрица
kf.H = np.eye(2)  # Модель измерений - единичная матрица
 
# Определение матриц Q и R (шумов модели и измерений)
kf.Q = np.array([[Q_angle, 0], [0, Q_angle]])
kf.R = np.array([[R_measure, 0], [0, R_measure]])
 
 
 
# Получение данных 
acceleration, gyroscope = get_data() 
gyro_x, gyro_y, gyro_z = gyroscope 
accel_x, accel_y, accel_z = acceleration
 
# Предсказание 
kf.predict(u=np.array([gyro_x, gyro_y]))
 
# Измерение углов на основе данных акселерометра
pitch_acc = np.arctan2(accel_x, np.sqrt(accel_y**2 + accel_z**2))  # Правильно, но почему-то угол режется от -90 до 90
roll_acc = np.arctan2(-accel_y, accel_z) 
 
# Проверка скачка через +-180
pitch_acc = unwrap_angle(pitch_acc, prev_pitch)
roll_acc = unwrap_angle(roll_acc, prev_roll)
 
# Сохранение старых углов
prev_pitch = pitch_acc
prev_roll = roll_acc
 
z = np.array([pitch_acc, roll_acc])
 
# Обновление фильтра с измерениями
kf.update(z)
 
# Получение оценок углов
estimated_pitch, estimated_roll = kf.x

Данная реализация оказалась проблемной при углах близких к +-180, поскольку это один и тот же угол, но пересчет нового угла бежит через 0, что иногда выдает какой-то рандомный угол между -180, +180. Эту проблему вроде бы пофиксил через функцию unwrap_angle().

Python
1
2
3
4
5
6
7
def unwrap_angle(current_angle, prev_angle):
    delta = current_angle - prev_angle
    if delta > math.pi:
        current_angle -= 2 * math.pi
    elif delta < -math.pi:
        current_angle += 2 * math.pi
    return current_angle

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

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

Собственно говоря вопросы. Как вычислять углы ориентации, если у меня тело всегда будет двигаться (лодка/самолетик на РУ)? Как бороться с проблемой +-180 и проблемой 90 по тангажу?

@u235 · 02.10.2023, 14:05

Fury67, насколько я знаю, проблема +-180 решается использованием кватернионов, а не углов.

@Fury67 · 04.10.2023, 11:55 **[ТС]**

u235, понял, спасибо.

Решил начать все с начала. Получилось что-то такое.

Python
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
import numpy as np
import time
import math
 
def normalize(vector):
    norm = np.linalg.norm(vector)
    if norm == 0:
        return vector
    return vector / norm
 
def compute_roll_pitch(acceleration):
    # Нормализуем ускорение
    acceleration = normalize(acceleration)
    
    # Вычисляем угол тангажа (pitch) и угол крена (roll) 
    pitch = math.asin(-acceleration[0])
    roll = math.atan2(acceleration[1], acceleration[2])
 
    # Преобразуем углы из радиан в градусы
    pitch_degrees = math.degrees(pitch)
    roll_degrees = math.degrees(roll)
 
    return roll_degrees, pitch_degrees
 
class KalmanFilter:
    def __init__(self, initial_angles, gyro_noise, accel_noise):
        # Инициализация матриц и параметров фильтра
        self.angle_estimate = np.array(initial_angles)  # Начальная оценка углов
        self.angle_covariance = np.identity(3)  # Начальная ковариация углов
        self.gyro_noise = gyro_noise
        self.accel_noise = accel_noise
 
    def update(self, gyro_deltas, accel_angles):
        # Прогнозирование следующего состояния фильтра
        predicted_angle = self.angle_estimate + gyro_deltas
        predicted_covariance = self.angle_covariance + self.gyro_noise
 
        # Вычисление коррекционного коэффициента (Калман-коэффициент)
        kalman_gain = np.dot(predicted_covariance, np.linalg.inv(predicted_covariance + self.accel_noise))
 
        # Обновление оценки углов с учетом показаний акселерометра
        self.angle_estimate = predicted_angle + np.dot(kalman_gain, (accel_angles - predicted_angle))
        self.angle_covariance = np.dot((np.identity(3) - kalman_gain), predicted_covariance)
 
        return self.angle_estimate.tolist()
 
def filter_angles(accel_angles, gyro_deltas, gyro_noise, accel_noise):
    # Создание объекта фильтра Калмана
    kalman_filter = KalmanFilter(accel_angles, gyro_noise, accel_noise)
 
    # Фильтрация углов с использованием данных гироскопа и акселерометра
    filtered_angles = [accel_angles]
    for delta in gyro_deltas:
        filtered_angle = kalman_filter.update(delta, accel_angles)
        filtered_angles.append(filtered_angle)
 
    return filtered_angles[-1]
 
 
roll, pitch, yaw = 0, 0, 0
time_output = time.time() # Время вывода
start_time = time.time() 
 
total_roll = [] # История roll
total_pitch = [] # История pitch
total_data = [] # История данных с акселерометра и гироскопа
 
while True:
    delta_time = time.time() - start_time # Время между итерациями
    start_time = time.time()
    
    acceleration, gyroscope = get_data() # Получение данных с акселерометра и гироскопа
    
    delta_roll, delta_pitch, delta_yaw = delta_gyro_angle(gyroscope, delta_time) # Вычисление изнения углов по гироскопу
    roll_accel, pitch_accel = compute_roll_pitch(acceleration) # Вычисление углов по акселерометру
    yaw_accel = yaw # Заглушка, добавить магнитометр. 
       
    accel_angles = [roll_accel, pitch_accel, yaw_accel]  # Углы на основе акселерометра (крен, тангаж, рыскание)
    gyro_deltas = [delta_roll, delta_pitch, delta_yaw]  # Угловые изменения по гироскопу (дельта крена, дельта тангажа, дельта рыскания)
    gyro_noise = np.identity(3) * 0.00001  # Матрица шума гироскопа
    accel_noise = np.identity(3) * 0.001  # Матрица шума акселерометра
 
    roll, pitch, yew = filter_angles(accel_angles, gyro_deltas, gyro_noise, accel_noise)
 
    
    if time.time() - time_output > 0.01:
        time_output = time.time()
        padding = ' ' * 100
        print(f"{yaw}, {pitch}, {roll}{padding}\r", end="")   
 
    total_roll.append(roll) 
    total_pitch.append(pitch)
    total_data.append([acceleration, gyroscope])

Но у меня возникает проблема в том, что фильтр Калмана не воспринимает изменение углов по показаниям гироскопа. То есть угол yaw вообще никак не меняется от кручения датчика.

Также еще проблема в том, что иногда бывает какой-то выброс данных акселерометра (скачок ускорения раза в 1.5-2 при статике), у меня фильтр не может с этим справиться. Опять же думаю по той причине, что фильтр как-то игнорирует данные с гироскопа.

Если кто-то разбирается, дайте совет, пожалуйста...

P.S. еще раз продублирую, что имею. Есть датчик акселерометра и датчик гироскопа, оба трех осевые. Мне надо бы как-то считать углы крена/тангажа в динамике по показаниям акселерометра и гироскопа, а также угол рысканья по показаниям гироскопа (в перспективе для коррекции еще добавлю магнитометр).

@Fury67 · 24.10.2023, 15:22 **[ТС]**

Я решил попробовать реализовать комплиментарный фильтр. Но у меня проблема со скоростью вычислений.

Если я использую альфа 0,992, то скорость вычисления изменений будет очень мала. Расчетные углы на 1,5-2 секунды отстают от фактического положения. Как это можно исправить? Или же это конкретная проблема этого метода объединения результатов?

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

Python
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
import math
import serial
import time
import numpy as np
 
def calculate_angles_accel(accel):
    a_x, a_y, a_z = accel
    
    teta_x = math.atan2(a_y, a_z)
    teta_y = math.atan2(-a_x, (a_y ** 2 + a_z ** 2) ** .5)
    teta_z = 0
    
    return [teta_x, teta_y, teta_z]
 
def calculate_angles_magnetometer(magnet, teta_x, teta_y):
    m_x, m_y, m_z = magnet
    
    b_x = m_x * math.cos(teta_x) + m_y * math.sin(teta_x) * math.sin(teta_y) + m_z * math.sin(teta_x) * math.cos(teta_y)
    b_y = m_y * math.cos(teta_y) - m_z * math.sin(teta_y)
    b_z = -m_x * math.sin(teta_x) + m_y * math.cos(teta_x) * math.sin(teta_y) + m_z * math.cos(teta_x) * math.cos(teta_y)
    
    yaw_mag = math.atan2(-b_y, b_x)
    return yaw_mag
 
def calculate_angles_gyro(gyro, last_angles, delta_time):
    w_x, w_y, w_z = gyro
    teta_x, teta_y, teta_z = last_angles
    
    new_teta_x = teta_x + w_x * delta_time
    new_teta_y = teta_y + w_y * delta_time
    new_teta_z = teta_z + w_z * delta_time
    
    return [new_teta_x, new_teta_y, new_teta_z]
 
def complementary_filter(accel, gyro, magnet, last_angles, delta_time, alpha=0.992):
    angles_accel = calculate_angles_accel(accel)
    angles_gyro = calculate_angles_gyro(gyro, last_angles, delta_time)
    
#     alpha = 1 / 400 * max(gyro)
    
    teta_x = alpha * angles_gyro[0] + (1 - alpha) * angles_accel[0]
    teta_y = alpha * angles_gyro[1] + (1 - alpha) * angles_accel[1]
    
    yaw_mag = angles_accel[2]
    if len(magnet) == 3: # if magnetometer turned on
        yaw_mag = calculate_angles_magnetometer(magnet, teta_x, teta_y)
        
    teta_z = alpha * angles_gyro[2] + (1 - alpha) * yaw_mag
    
    return [teta_x, teta_y, teta_z]
 
def convert_radians_to_degrees(angles):
    for i in range(len(angles)):
        angles[i] = round(math.degrees(angles[i]), 2)
    return angles
 
# Closing a port if it is open
try:
    ser.close()  
except:
    pass
 
ser = serial.Serial('COM7', 1000000) 
 
roll, pitch, yaw = 0, 0, 0
flag_0 = True
 
time_output = time.time()
start_time = time.time()
 
 
count = 0
total_roll = [] # Roll history
total_pitch = [] # Pitch history
total_data = [] # History of data from accelerometer and gyroscope
 
total_accel_x, total_accel_y, total_accel_z = [], [], []
 
while True:
    delta_time = time.time() - start_time # Time between iterations
    start_time = time.time()
    
    acceleration, gyroscope, magnetometer = get_data() # Getting data from accelerometer and gyroscope
    
    if flag_0: # Setting the initial angles
        flag_0 = False
        roll, pitch, yaw = calculate_angles_accel(acceleration)
        yaw = calculate_angles_magnetometer(magnetometer, roll, pitch)
        continue
    
    roll, pitch, yaw = complementary_filter(acceleration, gyroscope, magnetometer, [roll, pitch, yaw], delta_time) # Calculation of angles by a complementary filter
    
    roll_deg, pitch_deg, yaw_deg = convert_radians_to_degrees([roll, pitch, yaw]) # Converting angles from radians to degrees
    
    if time.time() - time_output > 0.01:
        time_output = time.time()
        padding = ' ' * 100
        print(f"{roll_deg}, {pitch_deg}, {yaw_deg}{padding}\r", end="")  
        
        
    total_roll.append(roll_deg) 
    total_pitch.append(pitch_deg)
    total_data.append([acceleration, gyroscope])
    
    
#     count += 1
#     if count > 30000:
#         ser.close()
#         break

P.S. Есть еще проблемы с вычислением углов по магнитометру, слишком большой разброс значений...

Добавлено через 59 минут
Еще есть вопросик. Не нужны ли какие-то матрицы поворотов для вычисления углов крена и тангажа?

@olegnikolaevich · 29.10.2023, 00:23

здесь надо подумать

Новые блоги и статьи Все статьи Все блоги /
модель ЗдравоСохранения 8. Подготовка к разному выполнению заданий anaschu 08.04.2026 https:/ / github. com/ shumilovas/ med2. git main ветка * содержимое блока дэлэй из старой модели теперь внутри зайца новой модели 8ATzM_2aurI	Блокировка документа от изменений, если он открыт у другого пользователя Maks 08.04.2026 Алгоритм из решения ниже реализован на примере нетипового документа, разработанного в конфигурации КА2. Задача: запретить редактирование документа, если он открыт у другого пользователя. / / . . .	Система безопасности+живучести для сервера-слоя интернета (сети). Двойная привязка. Hrethgir 08.04.2026 Далее были размышления о системе безопасности. Сообщения с наклонным текстом - мои. А как нам будет можно проверить, что ссылка наша, а не подделана хулиганами, которая выбросит на другую ветку и. . .	Модель ЗдрввоСохранения 7: больше работников, больше ресурсов. anaschu 08.04.2026 работников и заданий может быть сколько угодно, но настроено всё так, что используется пока что только 20% kYBz3eJf3jQ
Дальние перспективы сервера - слоя сети с космологическим дизайном интефейса карты и логики. Hrethgir 07.04.2026 Дальнейшее ближайшее планирование вывело к размышлениям над дальними перспективами. И вот тут может быть даже будут нужны оценки специалистов, так как в дальних перспективах всё может очень сильно. . .	Горе от ума kumehtar 07.04.2026 Эта мне ментальная установка, что вот прямо сейчас, мол, мне для полного счастья не хватает (нужное вписать), и когда я этого достигну - тогда и полный кайф. Одна из самых сильных ловушек на пути. . . .	Использование значений реквизитов справочника в документе, с определенными условиями и правами Maks 07.04.2026 1. Контроль срока действия договора Алгоритм из решения ниже реализован на примере нетипового документа "ЗаявкаНаРаботу", разработанного в конфигурации КА2. Задача: уведомлять пользователя, если. . .	Доступность команды формы по условию Maks 07.04.2026 Алгоритм из решения ниже реализован на примере нетипового документа "СписаниеМатериалов", разработанного в конфигурации КА2. Задача: сделать доступной кнопку (команда формы "ЗавершитьСписание") при. . .

@Fury67 Заяц, просто Заяц. 666 / 280 / 156 Регистрация: 12.11.2017 Сообщений: 882

	Фильтр Калмана/фильтр Маджвика 20.09.2023, 14:09. Показов 2353. Ответов 6 Метки нет (Все метки) Доброго времени суток! Если кто-нибудь когда-нибудь реализовывал на python обработку угловых скоростей (гироскоп трех осевой) и линейных ускорений(акселерометр трех осевой) в виде углов тангажа/крена/рыскания, используя фильтр Калмана/Маджвика, можете поделиться кодом? А то что-то не получается собственная реализация. Пока что более менее получилось только получить углы из ускорений без всяких фильтраций. 0

@u235 5517 / 2870 / 571 Регистрация: 07.11.2019 Сообщений: 4,761
	02.10.2023, 14:05
	Fury67, насколько я знаю, проблема +-180 решается использованием кватернионов, а не углов. 0

@olegnikolaevich 10 / 9 / 2 Регистрация: 27.10.2023 Сообщений: 73
	29.10.2023, 00:23
	здесь надо подумать 0