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

24

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

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

Оба примера, на которые я ссылаюсь во вступительном параграфе выше, предполагают, что измеряется только позиция. Однако ни один из примеров не имеет каких-либо расчетов для скорости. Например, пример Wikipedia определяет матрицу как , что означает, что вводится только позиция. Сосредоточив внимание на примере из Википедии, вектор состояния фильтра Калмана содержит положение и скорость , т.е.H H = [ 1 0 ] x k x k ˙ x k(xkxk1)/dtHH=[1   0]xkxkx˙k

xk=(xkx˙k)

Предположим, что измерение положения в момент времени равно . Тогда, если положение и скорость в момент времени были и , и если - постоянное ускорение, которое применяется в интервале времени от до , из измерения , что можно вывести значение для , используя формулуй к к - 1 х к - 1 ˙ х к - 1 к - 1 к йkx^kk1xk1x˙k1ak1kx^a

x^k=xk1+x˙k1dt+12adt2

Это подразумевает, что в момент времени измерение скорости определяется как˙ х кkx˙^k

x˙^k=x˙k1+adt=2x^kxk1dtx˙k1

Все величины в правой части этого уравнения (т.е. , и ) являются нормально распределенными случайными величинами с известными средними и стандартными отклонениями , поэтому матрица для вектора измеренияхк-1 ˙ х к-1Рx^kxk1x˙k1R

x^k=(x^kx˙^k)

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

стохастически
источник
2
Я не просмотрел все ваши расчеты. Однако, говоря о примере из Википедии, вы, кажется, немного запутались в его структуре. Вы правы в том, что измеряется только позиция. Однако используется так называемая модель с постоянной скоростью. Это означает, что скорость считается постоянной в матрице перехода состояний.
Джейсон Р.
3
Изменения скорости моделируются с использованием матрицы шумов процесса. Следовательно, вы изначально предполагаете, что скорость будет меняться случайным образом с некоторой заданной ковариацией. Удивительно, но это часто работает хорошо. Таким способом принято использовать технологический шум на одну производную выше самой высокой производной от переменной состояния. Например, если вы включили ускорение в вашу модель, то вы могли бы включить случайный компонент рывка, включенный в шум процесса.
Джейсон Р.
@JasonR с моделью википедии (в предположении нулевой начальной ковариации между положением и скоростью) оценка скорости всегда является ее начальным значением (как вы говорите, модель с постоянной скоростью). Однако дисперсия скорости монотонно возрастает из-за шума процесса, и нет никаких измерений, которые могли бы его уменьшить. В чем преимущество этого по сравнению с моделью, которая только моделирует положение и предполагает постоянную скорость?
Стохастически
2
Дисперсия оценки скорости не должна увеличиваться монотонно. Шум процесса просто вводит стохастический компонент в уравнение перехода состояния, позволяя вам выразить некоторую неопределенность в том, как именно состояние системы будет изменяться от шага к шагу времени. Если вы не включите шум процесса, тогда ваш фильтр будет действительно выводить постоянную скорость. Это, вероятно, не то, что вы хотите.
Джейсон Р
Хорошо @JasonR, если вы посмотрите на модель Википедии, вы увидите, что монотинно увеличивающаяся дисперсия скорости - это то, что она вам дает!
Стохастически

Ответы:

24

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

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

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

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

xk+1=xk+x˙kΔt+12a(Δt)2x˙k+1=x˙k+aΔt

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

xk=(xkx˙k)
xk+1=(1  Δt0  1)xk+((Δt)22Δt)ak
ak

Это дает матрицу отличную от предыдущей версии, но матрицы и должны быть одинаковыми.GFH


Если я реализую это в 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

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

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

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

введите описание изображения здесь

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

введите описание изображения здесь

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

введите описание изображения здесь

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

введите описание изображения здесь

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

введите описание изображения здесь

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

введите описание изображения здесь

Если вы изучите случай, когда измерения положения точны, то вы обнаружите, что уравнения ускорения Калмана дают точные результаты для ОБА и положения и скорости. Математически легко понять почему. Используя те же обозначения, что и в статье в Википедии , точные измерения означают, что . Если вы предполагаете, что начальная позиция и скорость известны так, что , то и матрица усиления Калмана определяется какzk+1=xk+1Pk=0Pk+1=QKk+1

Kk+1=(12/dt)

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

x^k+1=Fk+1xk+Kk+1(zk+1Hk+1Fk+1xk)=(xk+x˙kdtx˙k)+(12/dt)(xk+1(xk+x˙kdt))=(xk+12(xk+1xk)/dtx˙k)

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

Питер К.
источник
Спасибо за вашу помощь. Мой оригинальный вопрос содержал некоторые недоразумения, поэтому я попытался перефокусировать его и попытаться ответить на ваш вопрос о z_k.
Стохастически
1
спасибо В.М. за все ваши усилия :-). Твоя работа подтолкнула меня к тому, чтобы я сделал немного математики, которую я добавил к твоему ответу, надеюсь, ты не возражаешь. Во всяком случае, теперь я на 100% убежден, что стандартный метод хорош, потому что в конце концов я могу видеть формулы скорости. Еще раз спасибо
Стохастически
Рад, что смог помочь! Нет проблем с добавлением в ответ.
Питер К.