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

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

Оновлення 26 квітня 2013 року : початкове запитання тут містило деякі помилки, пов’язані з тим, що я неправильно зрозумів приклад вікіпедії на одній мірній позиції та швидкості . Маючи вдосконалене розуміння того, що відбувається, я зараз переробив це питання і більш чітко зосередив його.

Обидва приклади, на які я згадуюсь у вступному пункті вище, припускають, що вимірюється лише позиція. Однак жоден приклад не має жодного виду обчислення для швидкості. Наприклад, приклад Вікіпедії визначає матрицю як , що означає, що вводиться лише позиція. Орієнтуючись на приклад Вікіпедії, вектор стану фільтра Калмана містить положення та швидкість , тобто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}$$

Усі величини з правого боку цього рівняння (тобто $\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}$$

Якщо наш стан як і раніше: тоді рівняння оновлення стану просто: де тепер наш - це нормально розподілене прискорення. xk+1=

$$\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 : Діаграми помилки між справжнім положенням та швидкістю та їх оцінками.

Якщо ви вивчаєте випадок, коли вимірювання положення точні, то ви виявите, що рівняння калманського уппату дають точні результати для положення БОТУ і швидкості. Математично зрозуміти, чому просто. Використовуючи те саме позначення, що і у статті вікіпедії , точні вимірювання означають, що . Якщо припустити, що початкове положення і швидкість відомі так, що , то і матриця посилення Кальмана задаєтьсяP k =$\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$