Фильтр Калмана для положения и скорости: введение оценки скорости

Спасибо всем, кто вчера оставил комментарии / ответы на мой запрос ( внедрение фильтра Калмана для положения, скорости, ускорения ). Я смотрел на то, что было рекомендовано, и в частности на (а) пример Википедии об одномерном положении и скорости, а также на другой веб-сайт, который рассматривает аналогичную вещь .

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

Оба примера, на которые я ссылаюсь во вступительном параграфе выше, предполагают, что измеряется только позиция. Однако ни один из примеров не имеет каких-либо расчетов для скорости. Например, пример Wikipedia определяет матрицу как , что означает, что вводится только позиция. Сосредоточив внимание на примере из Википедии, вектор состояния фильтра Калмана содержит положение и скорость , т.е.H H = [ 1 0 ] x k x k ˙ x$(x_k-x_{k-1})/dt$${\bf H}$${\bf H} = [1\ \ \ 0]$${\bf x}_k$$x_k$$\dot{x}_{k}$

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$

Предположим, что измерение положения в момент времени равно . Тогда, если положение и скорость в момент времени были и , и если - постоянное ускорение, которое применяется в интервале времени от до , из измерения , что можно вывести значение для , используя формулуй к к - 1 х к - 1 ˙ х к - 1 к - 1 к $k$$\hat{x}_k$$k-1$$x_{k-1}$$\dot{x}_{k-1}$$a$$k-1$$k$$\hat{x}$$a$

$$\hat{x}_k = x_{k-1} + \dot{x}_{k-1} dt + \frac{1}{2} a dt^2$$

Это подразумевает, что в момент времени измерение скорости определяется как˙ х$k$$\hat{\dot{x}}_k$

$$\hat{\dot{x}}_k = \dot{x}_{k-1} + a dt = 2 \frac{\hat{x}_k - {x}_{k-1}}{dt} - \dot{x}_{k-1}$$

Все величины в правой части этого уравнения (т.е. , и ) являются нормально распределенными случайными величинами с известными средними и стандартными отклонениями , поэтому матрица для вектора измеренияхк-1 ˙ х к-1$\hat{x}_k$$x_{k-1}$$\dot{x}_{k-1}$$\bf R$

$$\begin{align*} \mathbf{\hat{x}}_{k} & =\left(\begin{array}[c]{c}\hat{x}_{k}\\ \hat{\dot{x}}_{k}\end{array} \right) \end{align*}$$

можно рассчитать. Является ли это правильным способом введения оценок скорости в процесс?

Является ли это правильным способом введения оценок скорости в процесс?

Если вы выберете свой штат соответствующим образом, то оценки скорости приходят «бесплатно». Смотрите вывод модели сигнала ниже (для простого 1-D случая, который мы рассматривали).

Модель сигнала, дубль 2

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

$$\begin{array} xx_{k+1} &=& x_{k} + \dot{x}_{k} \Delta t + \frac{1}{2} a (\Delta t)^2\\ \dot{x}_{k+1} &=& \dot{x}_{k} + a \Delta t \end{array}$$

Если наше состояние такое же, как и раньше: тогда уравнение обновления состояния будет просто: где теперь наш - это нормально распределенное ускорение.

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$ $$\mathbf{x}_{k+1} = \left(\begin{array}[c]{c} 1\ \ \Delta t\\ 0\ \ 1\end{array} \right) \mathbf{x}_{k} + \left(\begin{array}[c]{c} \frac{(\Delta t)^2}{2} \\ \Delta t \end{array} \right) a_k$$ $a_k$

Это дает матрицу отличную от предыдущей версии, но матрицы и должны быть одинаковыми.$\mathbf{G}$$\mathbf{F}$$\mathbf{H}$

---

Если я реализую это в scilab(извините, нет доступа к Matlab), это выглядит так:

// Signal Model
DeltaT = 0.1;
F = [1 DeltaT; 0 1];
G = [DeltaT^2/2; DeltaT];
H = [1 0];

x0 = [0;0];
sigma_a = 0.1;

Q = sigma_a^2;
R = 0.1;

N = 1000;

a = rand(1,N,"normal")*sigma_a;

x_truth(:,1) = x0;
for t=1:N,
    x_truth(:,t+1) = F*x_truth(:,t) + G*a(t);
    y(t) = H*x_truth(:,t) + rand(1,1,"normal")*sqrt(R);
end

Затем я могу применить уравнения фильтра Калмана к этому (шумные измерения).$y$

// Kalman Filter
p0 = 100*eye(2,2);

xx(:,1) = x0;
pp = p0;
pp_norm(1) = norm(pp);
for t=1:N,
    [x1,p1,x,p] = kalm(y(t),xx(:,t),pp,F,G,H,Q,R);
    xx(:,t+1) = x1;
    pp = p1;
    pp_norm(t+1) = norm(pp);
end

Итак, у нас есть измерения с шумом , и мы применили к ним фильтр Калмана и использовали ту же модель сигнала для генерации что и при применении фильтра Калмана (иногда довольно большое предположение!).$y$$y$

Затем следующие графики показывают результат.

График 1 : и зависимости от времени.$y$$x_k$

График 2 : увеличенное изображение первых нескольких образцов:

Сюжет 3 : Что-то, чего вы никогда не получите в реальной жизни, истинная позиция против оценки состояния позиции.

Сюжет 4 : То, чего вы никогда не получите в реальной жизни, истинная скорость в сравнении с оценкой состояния скорости.

Сюжет 5 : Норма матрицы ковариации государства (то, что вы всегда должны отслеживать в реальной жизни!). Обратите внимание, что он очень быстро переходит от своего начального очень большого значения к чему-то очень маленькому, поэтому я показал только первые несколько образцов.

График 6 : Графики погрешности между истинным положением и скоростью и их оценками.

Если вы изучите случай, когда измерения положения точны, то вы обнаружите, что уравнения ускорения Калмана дают точные результаты для ОБА и положения и скорости. Математически легко понять почему. Используя те же обозначения, что и в статье в Википедии , точные измерения означают, что . Если вы предполагаете, что начальная позиция и скорость известны так, что , то и матрица усиления Калмана определяется как$\mathbf{z}_{k+1}=x_{k+1}$$\mathbf{P}_k=0$$\mathbf{P}_{k+1}^{-}=\mathbf{Q}$$\mathbf{K}_{k+1}$

$$\mathbf{K}_{k+1} = \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right)$$

Это означает, что процедура обновления Kalman производит

$$\begin{align*} \mathbf{\hat{x}}_{k+1} & = \mathbf{F}_{k+1}\mathbf{x}_k + \mathbf{K}_{k+1}\left(\mathbf{z}_{k+1} - \mathbf{H}_{k+1} \mathbf{F}_{k+1}\mathbf{x}_k\right)\\ & = \left(\begin{array}[c]{c}x_k + \dot{x}_k dt\\ \dot{x}_k\end{array} \right) + \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right) \left(x_{k+1} - \left( x_k + \dot{x}_k dt\right) \right)\\ & = \left(\begin{array}[c]{c}x_{k+1}\\ 2 \left(x_{k+1} - x_k \right) /dt - \dot{x}_k\end{array} \right) \end{align*}$$

Как видите, значение скорости определяется именно той формулой, которую вы предлагали использовать для оценки скорости. Таким образом, хотя вы не могли видеть какие-либо вычисления для скорости, на самом деле они все-таки скрыты.$(x_k-x_{k-1})/dt$