Подходит ли фильтр Калмана для фильтрации положений спроецированных точек с учетом углов Эйлера устройства захвата?

Моя система следующая. Я использую камеру мобильного устройства для отслеживания объекта. Из этого трекинга я получаю четыре 3D-точки, которые проецирую на экран, чтобы получить четыре 2D-точки. Эти 8 значений довольно шумные из-за обнаружения, поэтому я хочу отфильтровать их, чтобы сделать движение более плавным и реалистичным. В качестве второго измерения я использую выход гироскопа устройства, который обеспечивает три угла Эйлера (то есть положение устройства). Они более точные и с большей частотой (до 100 Гц), чем 2D-позиции (около 20 Гц).

Моя первая попытка была с простым фильтром нижних частот, но задержка была важна, поэтому я сейчас пытаюсь использовать фильтр Калмана, надеясь, что он сможет сгладить позиции с небольшой задержкой. Как видно из предыдущего вопроса , одним из ключевых моментов в фильтре Калмана является связь между измерениями и внутренними переменными состояния. Здесь измерения - это мои 8 координат 2D-точки и 3 угла Эйлера, но я не уверен, что мне следует использовать в качестве внутренних переменных состояния и как я должен соединять углы Эйлера с 2D-точками. Отсюда основной вопрос: подходит ли фильтр Калмана для этой проблемы? И если да, то как?

filters kalman-filters state-space

— Стефан Пешар
источник

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

— Ниарен

@niaren: Спасибо за комментарий, я тоже буду изучать это.

— Стефан Пешар

Неясно, каковы ваши измерения. В структуре фильтра Калмана «измерения» относятся к величинам, которые вы фактически наблюдаете. Если вы измеряете четыре трехмерные точки (например, объединяя несколько изображений с камеры), то это ваши измерения. Вам также необходимо решить, какие переменные состояния вы пытаетесь оценить. Вы пытаетесь отслеживать местоположение 3D-объектов с течением времени? Если так, то это ваши переменные состояния. Может быть целесообразным, чтобы 2D-представление можно было использовать только для отображения и не включать в качестве части вашей модели. Дополнительные детали помогут предложить подход.

— Джейсон Р

Как говорит Jsaon, ваши измерения не ясны. Вы говорите:

From this tracking, I get four 3D points that I project on a mobile device screen, to get four 2D points. These 8 values are kinda noisy

а потом вы говорите What's available to me is the device's gyroscope output, which provides three Euler angles (i.e. the device attitude).. Что он? Четыре 2D точки или три угла Эйлера? Или процесс обработки идет по углам Эйлера -> 3D точки -> 2D точки?

— Питер К.

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

— Стефан Пешар

Ответы:

Низкочастотная фильтрация

Было бы хорошо узнать, что вы подразумеваете под «простым фильтром нижних частот».

Например, если ваши измерения в момент времени являются $k$

p_{k} = [\begin{matrix} x_{k} \\ y_{k} \end{matrix}]

$p_{k} = \left[ \begin{array}{c} x_k \\ y_k \end{array} \right]$

и ваши отфильтрованные оценки нижних частот:

p_{k}^{L P F} = α p_{К - 1}^{L п F} + (1 - α) п_{К}

$p^{\tt LPF}_k = \alpha p^{\tt LPF}_{k-1} + (1-\alpha)p_k$

тогда у вас будет довольно большая групповая задержка в фильтре около (для альфа, близкой к 1). $1/(1-\alpha)$

Моделирование сигнала: упрощенный подход

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

Обычно это выглядит так:

, где является процесс (вождение) шум,матрица перехода состояния, а ваш входной матрицы.

п_{К + 1}^{T р U Е} знак равно A п_{К}^{T р U Е} + В ε_{К}

$p^{\tt TRUE}_{k+1} = {\mathbf A} p^{\tt TRUE}_k + {\mathbf B} \epsilon_k$

ϵ_{k}

$\epsilon_k$

A

${\mathbf A}$

B

${\mathbf B}$

И тогда ваши измеренные значения : где - выходной (измерительный) шум, - выходная матрица, а - ваша измерительная шумовая матрица. $p_k$

п_{К} знак равно С п_{К}^{T р U Е} + D ν_{К}

$p_k = {\mathbf C} p^{\tt TRUE}_k + {\mathbf D} \nu_k$

ν_{k}

$\nu_k$

C

${\mathbf C}$

D

${\mathbf D}$

Здесь «состояние» модели выбирается в качестве истинных позиций, а измеряемые вами вещи являются выходом.

Затем вы можете применить к нему уравнения фильтра Калмана, чтобы получить оценки состояния истинного положения. $\hat{p^{\tt TRUE}_k }$

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

Моделирование сигнала: начало лучшего подхода

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

п_{К}^{T р U Е} знак равно {[{Икс}_{К} Y_{К} Z_{К} {\dot{Икс}}_{К} {\dot{Y}}_{К} {\dot{Z}}_{К} {\ddot{Икс}}_{К} {\ddot{Y}}_{К} {\ddot{Z}}_{К} φ ψ θ \dot{φ} \dot{ψ} \dot{θ} \ddot{φ} \ddot{ψ} \ddot{θ}]}^{T}

$p^{\tt TRUE}_{k} = \left[ x_k\ y_k\ z_k\ \dot{x}_k\ \dot{y}_k\ \dot{z}_k\ \ddot{x}_k\ \ddot{y}_k\ \ddot{z}_k\ \phi\ \psi\ \theta\ \dot{\phi}\ \dot{\psi}\ \dot{\theta}\ \ddot{\phi}\ \ddot{\psi}\ \ddot{\theta}\ \right]^T$

и измерения (выход)

p_{k} = {[x_{k} y_{k} z_{k} ϕ ψ θ]}^{T}

$p_k = \left[ x_k\ y_k\ z_k\ \phi\ \psi\ \theta\ \right ]^T$

x_{k}^{T R U E} = \sum_{n = 0}^{k} {\dot{x}}_{n}^{T R U E} n Δ t + \frac{1}{2} \sum_{n = 0}^{k} {\ddot{x}}_{n}^{T R U E} (n Δ t)^{2}

$x^{\tt TRUE}_k = \sum_{n=0}^k \dot{x}^{\tt TRUE}_n n\Delta t + \frac{1}{2} \sum_{n=0}^k \ddot{x}^{\tt TRUE}_n (n\Delta t)^2$

x, y,

$x,y,$

z

$z$

Это просто классические «уравнения движения». Смотрите уравнение (3) здесь.

— Питер К.
источник

p_{k} = α p_{k - 1} + (α - 1) p_{k}

$p_k = \alpha p_{k-1} + (\alpha - 1) p_k$

α

$\alpha$

Δ T; \frac{1}{2} (Δ^{2})

$\Delta t \qquad ; \qquad \frac{1}{2} (\Delta^2)$

Δ t

$\Delta t$

1 / f_{s}

$1/f_s$

f_{s}

$f_s$

Δ t

$\Delta t$

Δ t

$\Delta t$

\frac{1}{2} Δ t^{2}

$\frac{1}{2} \Delta t^2$

Ваш фильтр нижних частот может быть как;

p_{k} = α p_{k - 1} + (1 - α) z_{k}

$p_k = \alpha p_{k-1} + (1-\alpha) z_k$

$z_k$ $k$ $p_k$ $k$

LPF может быть деформирован следующим образом:

p_{k} = p_{k - 1} + K (z_{k} - p_{k - 1})

$p_k = p_{k-1} + K (z_k - p_{k-1})$

K = (1 - α)

$K = (1-\alpha)$

$K$

— Fumio Ueda
источник