Фильтр Калмана для гусеничного робота? доброго времени суток
достаточно долго курил литературу по этмоу вопросу (в основном буржуйские статьи), однако осталось много вопросов
(далее будет много формул, записаных в текстовом виде, кто не хочет себя мучать — в конце текста будет ссылка на скриншот маткадовского листа, там формулы попонятнее выглядят)
имеем:
гусеничная платформа, на которой установлены акселерометр и гироскоп
получил вот такой фильтр:
Вектор состояния X(6x1):xкоордината по оси Х yкоордината по оси Y φугол поворота робота Vлинейная скорость робота ωугловая скорость робота aтангенциальное ускороение робота
Матрица наблюдений Y(2x1):yaпоказания акселерометраygпоказания гироскопа
модель системы (t — временой шаг):x = x + cos(φ) * (V * t + a * t^2/2)y = y + sin(φ) * (V * t + a * t^2/2)φ = φ + ω * tV = V + a * tω = ωa = a
матрица F(6x6):10-sin(φ) * (a * t^2 / 2 + V * t)t*cos(φ)0t^2 * cos(φ) / 201cos(φ) * (a * t^2 / 2 + V * t)t*cos(φ)0t^2 * sin(φ) / 20010t000010t0000t000000t
Матрица H(2x6):000001000010
Матрица I(6x6) — единичная
Матрица N(2x2) — единичная
Матрица V(2x2) — единичная
Матрица R(2x2) — единичная
Матрица W(2x2) — единичная
Матрица Q(6x6): — подбирал опытным путем, пробовал расчитывать на основе случайной выборки на основе дисперсии (для диагональных элементов) и ковариации для всех остальных
Матрица P(6x6) — изначально равна матрице Q
[xT] — транспонированная матрица, (x[-1]) — обратная матрица
собственно сам фильтр:
K = P*[HT] ((HP[HT] + VR[VT])[-1])
P=(I — KH)P
x = x + K(y — h(x))
P=FP[FT] + WQ[WT]
x = f(x,y)
что я в итоге получил:график ускоренияграфик угла поворота
в целом и общем, для ускорения график более-менее похож на правду, однако для угла поворота присходит полная анархия.
было замечено, что очень большое влияние на поведение фильтра оказывает матрица Q,
поэтому у меня два вопроса:
1) насколько я близок в правильности составления самого фильтра (не судите строго, если где сел в лужу, а лучше укажие)
2) как считается матрица Q? было даже такое — подобрал матрицу Q — все работает отлчино, изменил поведение системы (предположим, изменил ускорение) — и фильтр перестал работать.все расчетные моменты на листе маткада:
Фильтр Калмана для гусеничного робота? доброго времени суток
достаточно долго курил литературу по этмоу вопросу (в основном буржуйские статьи), однако осталось много вопросов
(далее будет много формул, записаных в текстовом виде, кто не хочет себя мучать — в конце текста будет ссылка на скриншот маткадовского листа, там формулы попонятнее выглядят)
имеем:
гусеничная платформа, на которой установлены акселерометр и гироскоп
получил вот такой фильтр:
Вектор состояния X(6x1):xкоордината по оси Х yкоордината по оси Y φугол поворота робота Vлинейная скорость робота ωугловая скорость робота aтангенциальное ускороение робота
Матрица наблюдений Y(2x1):yaпоказания акселерометраygпоказания гироскопа
модель системы (t — временой шаг):x = x + cos(φ) * (V * t + a * t^2/2)y = y + sin(φ) * (V * t + a * t^2/2)φ = φ + ω * tV = V + a * tω = ωa = a
матрица F(6x6):10-sin(φ) * (a * t^2 / 2 + V * t)t*cos(φ)0t^2 * cos(φ) / 201cos(φ) * (a * t^2 / 2 + V * t)t*cos(φ)0t^2 * sin(φ) / 20010t000010t0000t000000t
Матрица H(2x6):000001000010
Матрица I(6x6) — единичная
Матрица N(2x2) — единичная
Матрица V(2x2) — единичная
Матрица R(2x2) — единичная
Матрица W(2x2) — единичная
Матрица Q(6x6): — подбирал опытным путем, пробовал расчитывать на основе случайной выборки на основе дисперсии (для диагональных элементов) и ковариации для всех остальных
Матрица P(6x6) — изначально равна матрице Q
[xT] — транспонированная матрица, (x[-1]) — обратная матрица
собственно сам фильтр:
K = P*[HT] ((HP[HT] + VR[VT])[-1])
P=(I — KH)P
x = x + K(y — h(x))
P=FP[FT] + WQ[WT]
x = f(x,y)
что я в итоге получил:график ускоренияграфик угла поворота
в целом и общем, для ускорения график более-менее похож на правду, однако для угла поворота присходит полная анархия.
было замечено, что очень большое влияние на поведение фильтра оказывает матрица Q,
поэтому у меня два вопроса:
1) насколько я близок в правильности составления самого фильтра (не судите строго, если где сел в лужу, а лучше укажие)
2) как считается матрица Q? было даже такое — подобрал матрицу Q — все работает отлчино, изменил поведение системы (предположим, изменил ускорение) — и фильтр перестал работать.все расчетные моменты на листе маткада:
достаточно долго курил литературу по этмоу вопросу (в основном буржуйские статьи), однако осталось много вопросов
(далее будет много формул, записаных в текстовом виде, кто не хочет себя мучать — в конце текста будет ссылка на скриншот маткадовского листа, там формулы попонятнее выглядят)
имеем:
гусеничная платформа, на которой установлены акселерометр и гироскоп
получил вот такой фильтр:
Вектор состояния X(6x1):xкоордината по оси Х yкоордината по оси Y φугол поворота робота Vлинейная скорость робота ωугловая скорость робота aтангенциальное ускороение робота
Матрица наблюдений Y(2x1):yaпоказания акселерометраygпоказания гироскопа
модель системы (t — временой шаг):x = x + cos(φ) * (V * t + a * t^2/2)y = y + sin(φ) * (V * t + a * t^2/2)φ = φ + ω * tV = V + a * tω = ωa = a
матрица F(6x6):10-sin(φ) * (a * t^2 / 2 + V * t)t*cos(φ)0t^2 * cos(φ) / 201cos(φ) * (a * t^2 / 2 + V * t)t*cos(φ)0t^2 * sin(φ) / 20010t000010t0000t000000t
Матрица H(2x6):000001000010
Матрица I(6x6) — единичная
Матрица N(2x2) — единичная
Матрица V(2x2) — единичная
Матрица R(2x2) — единичная
Матрица W(2x2) — единичная
Матрица Q(6x6): — подбирал опытным путем, пробовал расчитывать на основе случайной выборки на основе дисперсии (для диагональных элементов) и ковариации для всех остальных
Матрица P(6x6) — изначально равна матрице Q
[xT] — транспонированная матрица, (x[-1]) — обратная матрица
собственно сам фильтр:
K = P*[HT] ((HP[HT] + VR[VT])[-1])
P=(I — KH)P
x = x + K(y — h(x))
P=FP[FT] + WQ[WT]
x = f(x,y)
что я в итоге получил:график ускоренияграфик угла поворота
в целом и общем, для ускорения график более-менее похож на правду, однако для угла поворота присходит полная анархия.
было замечено, что очень большое влияние на поведение фильтра оказывает матрица Q,
поэтому у меня два вопроса:
1) насколько я близок в правильности составления самого фильтра (не судите строго, если где сел в лужу, а лучше укажие)
2) как считается матрица Q? было даже такое — подобрал матрицу Q — все работает отлчино, изменил поведение системы (предположим, изменил ускорение) — и фильтр перестал работать.все расчетные моменты на листе маткада:
Не можешь разобраться в этой теме?
Обратись за помощью к экспертам
Гарантированные бесплатные доработки
Быстрое выполнение от 2 часов
Проверка работы на плагиат
Интересные статьи из справочника
Поможем написать учебную работу
Отвечай на вопросы, зарабатывай баллы и трать их на призы.
Подробнее
Подробнее
К сожалению, без возможности просмотра ваших расчетов на листе Маткада, я не могу дать точное решение на ваши вопросы. Однако, я могу дать некоторые рекомендации по проблемам, с которыми вы столкнулись.
1) Когда вы сталкиваетесь с проблемами в работе фильтра, важно проследить каждый шаг вашего решения. Убедитесь, что вы правильно реализовали все формулы и корректно работаете с матрицами.
2) При расчете матрицы Q, уделите внимание значению дисперсии и ковариации для различных переменных состояния вашей системы. Подберите значения так, чтобы они соответствовали характеристикам шумов, с которыми вы сталкиваетесь в вашей модели.
Если у вас остаются проблемы с фильтром Калмана для гусеничного робота, я рекомендую обратиться к специалистам в этой области, которые могут помочь вам более детально разобраться в вашей модели и помочь с настройкой фильтра.