доброго времени суток
достаточно долго курил литературу по этмоу вопросу (в основном буржуйские статьи), однако осталось много вопросов
(далее будет много формул, записаных в текстовом виде, кто не хочет себя мучать — в конце текста будет ссылка на скриншот маткадовского листа, там формулы попонятнее выглядят)
имеем:
гусеничная платформа, на которой установлены акселерометр и гироскоп
получил вот такой фильтр:
Вектор состояния 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) |
| φ = φ + ω * t |
| V = V + a * t |
| ω = ω |
| a = a |
матрица F(6x6):
| 1 | 0 | -sin(φ) * (a * t^2 / 2 + V * t) | t*cos(φ) | 0 | t^2 * cos(φ) / 2 |
| 0 | 1 | cos(φ) * (a * t^2 / 2 + V * t) | t*cos(φ) | 0 | t^2 * sin(φ) / 2 |
| 0 | 0 | 1 | 0 | t | 0 |
| 0 | 0 | 0 | 1 | 0 | t |
| 0 | 0 | 0 | 0 | t | 0 |
| 0 | 0 | 0 | 0 | 0 | t |
Матрица H(2x6):
Матрица 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 — все работает отлчино, изменил поведение системы (предположим, изменил ускорение) — и фильтр перестал работать.
все расчетные моменты на листе маткада: 
