Розширений фільтр Кальмана з лазерним скануванням + відома карта

Зараз я працюю над проектом для школи, де мені потрібно реалізувати розширений фільтр Kalman для точкового робота з лазерним сканером. Робот може обертатися з радіусом повороту 0 градусів і рухатися вперед. Усі рухи кусочно лінійні (привод, обертання, привід).

Симулятор, який ми використовуємо, не підтримує прискорення, весь рух миттєвий.

У нас також є відома карта (png-зображення), яку нам потрібно локалізувати. Ми можемо простежити зображення, щоб імітувати лазерне сканування.

Ми з партнером мало розгублені щодо моделей руху та сенсорів, які нам потрібно використовувати.

Поки ми моделюємо стан як вектор .$(x,y,\theta)$

Ми використовуємо рівняння оновлення наступним чином

void kalman::predict(const nav_msgs::Odometry msg){
    this->X[0] += linear * dt * cos( X[2] ); //x
    this->X[1] += linear * dt * sin( X[2] ); //y
    this->X[2] += angular * dt; //theta

    this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
    this->F(1,2) =  linear * dt * cos( X[2] ); //t+1 ?

    P = F * P * F.t() + Q;

    this->linear = msg.twist.twist.linear.x;
    this->angular = msg.twist.twist.angular.z;
    return;
}

Ми думали, що у нас все працює, поки не помітили, що ми забули ініціалізувати Pі що це нуль, це означає, що виправлення не відбувається. Мабуть, наше поширення було дуже точним, оскільки ми ще не внесли шум у систему.

Для моделі руху ми використовуємо таку матрицю для F:

$F = \begin{bmatrix}1 & 0 & -v*\Delta t*sin(\theta) \\ 0 & 1 & v*\Delta t*cos(\theta) \\ 0 & 0 & 1 \end{bmatrix}$

Як це якобіанські наші формули оновлення. Це правильно?

Для моделі сенсорів ми наближаємось до якобіанського (H), приймаючи кінцеві відмінності позицій роботів , та та відстеження променів на карті. Ми поговорили з ТА, яка сказала, що це буде працювати, але я все ще не впевнений, що це буде. Наш професор відсутній, тому ми не можемо його запитати на жаль. Ми використовуємо 3 лазерні вимірювання за крок корекції, так що H - 3x3.$x$$y$$\theta$

Інше питання, де потрібно ініціалізувати P. Ми спробували 1 10000, і всі вони розміщують робота за межами карти в (-90, -70), коли карта лише 50x50.

Код нашого проекту можна знайти тут: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

Будь-яка порада високо цінується.

Редагувати:

У цей момент я дістав фільтр, щоб стабілізуватись з базовим шумом руху, але фактичного руху немає. Як тільки робот починає рухатися, фільтр розходяться досить швидко і виходить з карти.

• Використання EKF для локалізації на основі лазерного сканування та відомої карти є поганою ідеєю і не буде працювати, оскільки одне з основних припущень EKF недійсне: Модель вимірювання не відрізняється.

Я б запропонував заглянути в Локалізацію Монте-Карло (фільтри частинок). Це не тільки вирішить проблему з вашою моделлю вимірювання, але також дозволить глобальну локалізацію (початкова поза всередині карти абсолютно невідома).

Редагувати: Вибачте, що забув згадати ще один важливий момент:

• Якщо ви хочете застосувати EKF, ваша модель вимірювання, а також ваша модель руху можуть включати лише шум Гаусса . Це означає, що вам потрібно мати можливість записати модель вимірювання як . Це суворе обмеження, оскільки воно не дозволяє дещо складнішої моделі, на зразок beam_range_finder_model в імовірнісній робототехніці, яка також враховує динамічні об'єкти перед роботом, недійсні (макс) вимірювання та абсолютно випадкові показання. Ви застрягли б у випромінюванні для частини і додали гауссового шуму.$z_t = h(x_t) + N(0, Q_t)$$h(x_t)$

Перш за все, ви нічого не згадали про те, яку локалізацію ви використовуєте. Це з відомими чи невідомими відповідниками?

Тепер, щоб придбати якобіан в Матлабі, ви можете зробити наступне

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)

Результат

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]

Розширений фільтр Kalman вимагає, щоб система була лінійною, а шум - гауссовим. Тут система означає, що моделі руху та спостереження повинні бути лінійними. Лазерні датчики задають діапазон і кут до орієнтиру від кадру робота, тому модель вимірювання не є лінійною. Про P, якщо припустити , що це буде великим , то ваш EKF оцінка буде поганий на початку і він спирається на вимірювання , так як попередній вектор стану не доступний. Однак, як тільки ваш робот почне рухатись і чути, EKF буде покращуватися, оскільки він використовує моделі руху та вимірювання для оцінки пози робота. Якщо ваш робот не відчуває жодних орієнтирів, невизначеність різко зростає. Також потрібно бути обережним щодо кута. В C ++,cos and sin, кут повинен бути в радіані. У цьому коді немає нічого про цю проблему. Якщо ви припускаєте, що шум кута в градусі при розрахунку в радіані, то ви можете отримати дивні результати, оскільки один градус як шум, тоді як обчислення в радіані вважаються величезними.