Косвенный фильтр Калмана для инерциальной навигационной системы

Question

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

https://fenix.tecnico.ulisboa.pt/downloadFile/395137332405/dissertacao.pdf

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

Так:

Wmeas = Wtrue + BiasW   (Gyro meas)
Ameas = Atrue + BiasA.  (Accelerometer meas)

Следовательно,

когда я распространяю уравнения механизации (уравнения 3-29, 3-37 и 3-41), я должен использовать "истинные" значения или лучше:

Wmeas - BiasW
Ameas - BiasA

где BiasW и BiasA - последняя доступная оценка смещения. Правильно?

Относительно фазы обновления EKF, если уравнение измерения

dzV = VelGPS_est - VelGPS_meas

матрица H должна иметь единичную матрицу в соответствии с переменными состояния ошибки скорости dx(VEL) и 0 в других местах. Правильно?

Сказал, что я не уверен, как я должен распространять переменную состояния после фазы обновления. Распространение переменной состояния должно быть (на мой взгляд):

POSk|k = POSk|k-1 + dx(POS);
VELk|k = VELk|k-1 + dx(VEL);
...

Но это не сработало. Поэтому я попробовал:

POSk|k = POSk|k-1 - dx(POS);
VELk|k = VELk|k-1 - dx(VEL);

это тоже не сработало... Я попробовал оба решения, даже если, по моему мнению, следует использовать "+". Но так как оба не работают (у меня есть другая ошибка в другом месте), я бы спросил вас, есть ли у вас какие-либо предложения.

Вы можете увидеть фрагмент кода по следующей ссылке: http://pastebin.com/aGhKh2ck.

Благодарю.

Answer 1

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

    % Apply corrections
    Pned = Pned + dx(1:3);
    Vned = Vned + dx(4:6);

Теоретически, когда вы используете косвенную форму, вы свободно интегрируете IMU (этот процесс называется механизацией в этом документе) и время от времени запускаете IKF, чтобы обновить его исправление. Теоретически непроверенная двойная интеграция акселерометра приводит к большим (или для дешевых MEMS MEU, огромным) значениям ошибки в Pned а также Vned, Это, в свою очередь, заставляет IKF производить соответственно большие значения dx(1:6) с течением времени и непроверенная интеграция в ИДУ все дальше и дальше отдаляется от истины. Теоретически, вы можете определить свою позицию в любое время, как Pned +/- dx(1:3) (знак не важен - вы можете установить это в любом случае). Важной частью здесь является то, что вы не модифицируете Pned от IKF, потому что оба работают независимо друг от друга, и вы добавляете их вместе, когда вам нужен ответ.

На практике вы не хотите принимать разницу между двумя double значения, потому что вы потеряете точность (потому что многие биты значений и были необходимы для представления огромной части вместо точности, которую вы хотите). Вы поняли, что на практике вы хотите рекурсивно обновить Pned на каждом обновлении. Однако, когда вы отклоняетесь от теории таким образом, вы должны предпринять соответствующий (и несколько неочевидный) шаг обнуления значения вашей коррекции из вектора состояния IKF. Другими словами, после того, как вы Pned = Pned + dx(1:3) Вы "использовали" коррекцию, и вам нужно сбалансировать уравнение с dx(1:3) = dx(1:3) - dx(1:3) (Упрощенный: dx(1:3) = 0), чтобы вы случайно не интегрировали коррекцию с течением времени.

Почему это работает? Почему это не портит остальную часть фильтра? Как выясняется, KF-ковариация процесса P на самом деле не зависит от государства x, Это зависит от функции обновления и шума процесса Q и так далее. Поэтому фильтру не важно, что это за данные. (Теперь это упрощение, потому что часто Q а также R включают термины ротации и R может варьироваться в зависимости от других переменных состояния и т. д., но в этих случаях вы фактически используете состояние вне фильтра (совокупное положение и ориентация), а не необработанные значения коррекции, которые сами по себе не имеют значения).