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

Можно использовать расширенное дискретное время и алгоритмы сигма-точечного фильтра Калмана для онлайновой оценки состояния дискретного времени нелинейные системы. Если у вас есть система с серьезной нелинейностью, алгоритм сигма-точечного фильтра Калмана может дать лучшие результаты оценки. Можно выполнить оценку состояния в Simulink^® и в командной строке. Чтобы выполнить оценку состояния, вы сначала создаете нелинейную функцию изменения состояния и функцию измерения для вашей системы.

В командной строке вы используете функции, чтобы создать extendedKalmanFilter или unscentedKalmanFilter объект для желаемого алгоритма, и задает, являются ли шумовые части процесса и измерения в функциях дополнением или недополнением. После того, как вы создадите объект, вы используете predict и correct команды, чтобы оценить состояния с помощью данных реального времени. Для получения информации о порядке, в котором можно выполнить эти команды, смотрите predict и correct страницы с описанием.

В Simulink вы указываете, что они функционируют в блоках Unscented Kalman Filter и Extended Kalman Filter. Вы также задаете, являются ли шумовые части процесса и измерения в функциях дополнением или недополнением. В блоках программное обеспечение решает порядок, в котором сделаны предсказание и коррекция оценок состояния.

Расширенный алгоритм фильтра Калмана

extendedKalmanFilter команда и блок Extended Kalman Filter реализуют алгоритм Фильтра Калмана дискретного времени первого порядка. Примите, что уравнения изменения состояния и измерения в течение дискретного времени нелинейная система имеют шумовые части неаддитивного процесса и измерения с нулевыми средними и ковариационными матрицами Q и R, соответственно:

$\begin{array}{l} x [k + 1] = f (x [k], w [k], u_{s} [k]) \\ y [k] = h (x [k], v [k], u_{m} [k]) \\ w [k] ~ (0, Q [k]) \\ v [k] ~ (0, R [k]) \end{array}$

Здесь f является нелинейной функцией изменения состояния, которая описывает эволюцию состояний x от одного временного шага до следующего. Нелинейная функция измерения h связывает x к измерениям y на временном шаге k. Эти функции могут также иметь дополнительные входные параметры, которые обозначаются _us и _um. Шумом процесса и измерения является w и v, соответственно. Вы обеспечиваете Q и R.

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

Инициализируйте объект фильтра начальными значениями состояния, x[0], и ошибочная ковариационная матрица оценки состояния, P.
$\begin{array}{l} \hat{x} [0 | - 1] = E (x [0]) \\ P [0 | - 1] = E (x [0] - \hat{x} [0 | - 1]) (^{x [0] - \hat{x} [0 | - 1]) T} \end{array}$
Здесь $\hat{x}$ оценка состояния и $\hat{x} [k_{a} | k_{b}] $ обозначает оценку состояния на временном шаге _ka использование измерений на временных шагах _0,1,...,kb. Так $\hat{x} [0 | - 1]$ лучшее предположение значения состояния, прежде чем вы сделаете любые измерения. Вы задаете это значение, когда вы создаете фильтр.
Для временных шагов k = 0,1,2,3..., выполняют следующее:
1. Вычислите якобиан функции измерения и обновите ошибочную ковариацию оценки состояния и оценки состояния с помощью результатов измерений, y[k]. В командной строке, correct команда выполняет это обновление.
  $\begin{array}{l} C [k] = {\frac{\partial h}{\partial x} |}_{\hat{x} [k | k - 1]} \\ S [k] = {\frac{\partial h}{\partial v} |}_{\hat{x} [k | k - 1]} \end{array}$
  Программное обеспечение вычисляет эти якобиевские матрицы численно, если вы не задаете аналитический якобиан.
  $\begin{array}{l} K [k] = P [k | k - 1] C [^{k] T} (^{C [k] P [k | k - 1] C [^{k] T} + S [k] R [k] S [^{k] T}) - 1} \\ \hat{x} [k | k] = \hat{x} [k | k - 1] + K [k] (y [k] - h (\hat{x} [k | k - 1], 0, u_{m} [k]) \\ \hat{P} [k | k] = P [k | k - 1] - K [k] C [k] P [k | k - 1] \end{array}$
  Здесь K усиление Кальмана.
2. Вычислите якобиан функции изменения состояния и предскажите ошибочную ковариацию оценки состояния и оценки состояния на следующем временном шаге. В программном обеспечении, predict команда выполняет это предсказание.
  $\begin{array}{l} A [k] = {\frac{\partial f}{\partial x} |}_{\hat{x} [k | k]} \\ G [k] = {\frac{\partial f}{\partial w} |}_{\hat{x} [k | k]} \end{array}$
  Программное обеспечение вычисляет эти якобиевские матрицы численно, если вы не задаете аналитический якобиан. Этот численный расчет может увеличить время вычислений и числовую погрешность оценки состояния.
  $\begin{array}{l} P [k + 1 | k] = A [k] P [k | k] A [^{k] T} + G [k] Q [k] G [^{k] T} \\ \hat{x} [k + 1 | k] = f (\hat{x} [k | k], 0, u_{s} [k]) \end{array}$
  correct функционируйте использует эти значения в следующем временном шаге. Для лучшей числовой эффективности программное обеспечение использует факторизацию квадратного корня ковариационных матриц. Для получения дополнительной информации об этой факторизации см. [2].

Блок Extended Kalman Filter поддерживает несколько функций измерения. Эти измерения могут иметь различные шаги расчета, пока их шаг расчета является целочисленным кратным шаг расчета изменения состояния. В этом случае отдельный шаг коррекции выполняется, соответствуя измерениям от каждой функции измерения.

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

Если шум процесса w дополнение, которое является уравнением изменения состояния, имеет форму $x [k] = f (x [k - 1], u_{s} [k - 1]) + w [k - 1]$ , затем якобиевский матричный G[k] единичная матрица.
Если шум измерения v дополнение, которое является уравнением измерения, имеет форму $y [k] = h (x [k], u_{m} [k]) + v [k]$ , затем якобиевский матричный S[k] единичная матрица.

Аддитивные шумовые части в состоянии и функциях перехода уменьшают время вычислений.

Первый порядок расширил линейные аппроксимации использования Фильтра Калмана к нелинейным функциям изменения состояния и измерения. В результате алгоритм не может быть надежным, если нелинейность в вашей системе серьезна. Алгоритм сигма-точечного фильтра Калмана может привести к лучшим результатам в этом случае.

Алгоритм сигма-точечного фильтра Калмана

Алгоритм сигма-точечного фильтра Калмана и блок Unscented Kalman Filter используют недушистое преобразование, чтобы получить распространение статистических свойств оценок состояния через нелинейные функции. Алгоритм сначала генерирует набор названных точек сигмы значений состояния. Эти точки сигмы получают среднее значение и ковариацию оценок состояния. Алгоритм использует каждую из точек сигмы как вход к функциям изменения состояния и измерения, чтобы получить новый набор преобразованных точек состояния. Среднее значение и ковариация преобразованных точек затем используются, чтобы получить оценки состояния и ошибочную ковариацию оценки состояния.

Примите, что уравнения изменения состояния и измерения для M - дискретное время состояния нелинейная система имеют шумовые части аддитивного процесса и измерения с нулевым средним значением и ковариациями Q и R, соответственно:

$\begin{array}{l} x [k + 1] = f (x [k], u_{s} [k]) + w [k] \\ y [k] = h (x [k], u_{m} [k]) + v [k] \\ w [k] ~ (0, Q [k]) \\ v [k] ~ (0, R [k]) \end{array}$

Вы вводите начальные значения Q и R в ProcessNoise и MeasurementNoise свойства объекта сигма-точечного фильтра Калмана.

Инициализируйте объект фильтра начальными значениями состояния, x[0], и ошибочная ковариация оценки состояния, P.
$\begin{array}{l} \hat{x} [0 | - 1] = E (x [0]) \\ P [0 | - 1] = E (x [0] - \hat{x} [0 | - 1]) (^{x [0] - \hat{x} [0 | - 1]) T} \end{array}$
Здесь $\hat{x}$ оценка состояния и $\hat{x} [k_{a} | k_{b}] $ обозначает оценку состояния на временном шаге _ka использование измерений на временных шагах _0,1,...,kb. Так $\hat{x} [0 | - 1]$ лучшее предположение значения состояния, прежде чем вы сделаете любые измерения. Вы задаете это значение, когда вы создаете фильтр.
Для каждого временного шага k обновите ошибочную ковариацию оценки состояния и оценки состояния с помощью результатов измерений, y[k]. В программном обеспечении, correct команда выполняет это обновление.
1. Выберите точки сигмы ${\hat{x}}^{(i)} [k | k - 1]$ на временном шаге k.
  $\begin{array}{l} {\hat{x}}^{(0)} [k | k - 1] = \hat{x} [k | k - 1] \\ {\hat{x}}^{(i)} [k | k - 1] = \hat{x} [k | k - 1] + Δ x^{(i)} i = 1, ..., 2 M \\ Δ x^{(i)} = (\sqrt{c P [k | k - 1]}) {}_{i} i = 1, ..., M \\ Δ x^{(M + i)} = - {(\sqrt{c P [k | k - 1]})}_{i} i = 1, ..., M \end{array}$
  Где $c = α^{2} (M + κ)$ масштабный коэффициент на основе количества состояний M и параметры α и κ. Для получения дополнительной информации о параметрах, смотрите Эффект Альфы, Беты и Параметров Каппы. $\sqrt{c P}$ матричный квадратный корень из cP, таким образом что $\sqrt{c P} {(\sqrt{c P})}^{T} = c P$ и ${(\sqrt{c P})}_{i}$ i th столбец $\sqrt{c P}$ .
2. Используйте нелинейную функцию измерения, чтобы вычислить предсказанные измерения для каждой из точек сигмы.
  ${\hat{y}}^{(i)} [k | k - 1] = h ({\hat{x}}^{(i)} [k | k - 1], u_{m} K i = 0, 1, ..., 2 M$
3. Объедините предсказанные измерения, чтобы получить предсказанное измерение во время k.
  $\begin{array}{l} \hat{y} [k] = \sum_{i = 0}^{2 M} W_{M}^{(i)} {\hat{y}}^{(i)} [k | k - 1] \\ W_{M}^{(0)} = 1 - \frac{M}{α^{2} (M + κ)} \\ W_{M}^{i} = \frac{1}{2 α^{2} (M + κ)} i = 1, 2, ..., 2 M \end{array}$
4. Оцените ковариацию предсказанного измерения. Добавьте R[k] с учетом аддитивного шума измерения.
  $\begin{array}{l} P_{y} = \sum_{i = 0}^{2 M} W_{c}^{(i)} ({\hat{y}}^{(i)} [k | k - 1] - \hat{y} [k]) (^{{\hat{y}}^{(i)} [k | k - 1] - \hat{y} [k]) T} + R [k] \\ W_{c}^{(0)} = (2 - α^{2} + β) - \frac{M}{α^{2} (M + κ)} \\ W_{c}^{i} = 1 / (2 α^{2} (M + κ)) i = 1, 2, ..., 2 M \end{array}$
  Для получения информации о параметре β смотрите Эффект Альфы, Беты и Параметров Каппы.
5. Оцените перекрестную ковариацию между $\hat{x} [k | k - 1]$ и $\hat{y} [k]$ .
  $P_{x y} = \frac{1}{2 α^{2} (m + κ)} \sum_{i = 1}^{2 M} ({\hat{x}}^{(i)} [k | k - 1] - \hat{x} [k | k - 1]) {({\hat{y}}^{(i)} [k | k - 1] - \hat{y} [k])}^{T}$
  Суммирование начинает с i = 1 потому что ${\hat{x}}^{(0)} [k | k - 1] - \hat{x} [k | k - 1] = 0$ .
6. Получите предполагаемую ошибочную ковариацию оценки состояния и оценки состояния на временном шаге k.
  $\begin{array}{l} K = P_{x y} P_{y}^{- 1} \\ \hat{x} [k | k] = \hat{x} [k | k - 1] + K (y [k] - \hat{y} [k]) \\ P [k | k] = P [k | k - 1] - K P_{y} K_{k}^{T} \end{array}$
  Здесь K усиление Кальмана.
Предскажите ошибочную ковариацию оценки состояния и оценки состояния на следующем временном шаге. В программном обеспечении, predict команда выполняет это предсказание.
1. Выберите точки сигмы ${\hat{x}}^{(i)} [k | k]$ на временном шаге k.
  $\begin{array}{l} {\hat{x}}^{(0)} [k | k] = \hat{x} [k | k] \\ {\hat{x}}^{(i)} [k | k] = \hat{x} [k | k] + Δ x^{(i)} i = 1, ..., 2 M \\ Δ x^{(i)} = (\sqrt{c P [k | k]}) {}_{i} i = 1, ..., M \\ Δ x^{(M + i)} = - {(\sqrt{c P [k | k]})}_{i} i = 1, ..., M \end{array}$
2. Используйте нелинейную функцию изменения состояния, чтобы вычислить предсказанные состояния для каждой из точек сигмы.
  ${\hat{x}}^{(i)} [k + 1 | k] = f ({\hat{x}}^{(i)} [k | k], u_{s} K$
3. Объедините предсказанные состояния, чтобы получить предсказанные состояния во время k+1. Эти значения используются correct команда в следующем временном шаге.
  $\begin{array}{l} \hat{x} [k + 1 | k] = \sum_{i = 0}^{2 M} W_{M}^{(i)} {\hat{x}}^{(i)} [k + 1 | k] \\ W_{M}^{(0)} = 1 - \frac{M}{α^{2} (M + κ)} \\ W_{M}^{i} = \frac{1}{2 α^{2} (M + κ)} i = 1, 2, ..., 2 M \end{array}$
4. Вычислите ковариацию предсказанного состояния. Добавьте Q[k] с учетом шума аддитивного процесса. Эти значения используются correct команда в следующем временном шаге.
  
  $\begin{array}{l} P [k + 1 | k] = \sum_{i = 0}^{2 M} W_{c}^{(i)} ({\hat{x}}^{(i)} [k + 1 | k] - \hat{x} [k + 1 | k]) {({\hat{x}}^{(i)} [k + 1 | k] - \hat{x} [k + 1 | k])}^{T} + Q [k] \\ W_{c}^{(0)} = (2 - α^{2} + β) - \frac{M}{α^{2} (M + κ)} \\ W_{c}^{i} = 1 / (2 α^{2} (M + κ)) i = 1, 2, ..., 2 M \end{array}$

Блок Unscented Kalman Filter поддерживает несколько функций измерения. Эти измерения могут иметь различные шаги расчета, пока их шаг расчета является целочисленным кратным шаг расчета изменения состояния. В этом случае отдельный шаг коррекции выполняется, соответствуя измерениям от каждой функции измерения.

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

Если шумовые части являются недополнением, основные изменения в алгоритме:

correct команда генерирует 2*(M+V)+1 точки сигмы с помощью P[k|k-1] и R[k], где V число элементов в шуме измерения v[k]. R[k] термин больше не добавляется на шаге 2 (d) алгоритма, потому что дополнительные точки сигмы получают удар шума измерения на Py.
predict команда генерирует 2*(M+W)+1 точки сигмы с помощью P[k|k] и Q[k], где W число элементов в шуме процесса w[k]. Q[k] термин больше не добавляется на шаге 3 (d) алгоритма, потому что дополнительные точки сигмы получают удар шума процесса на P[k+1|k].

Эффект альфы, беты и параметров каппы

Чтобы вычислить состояние и его статистические свойства на следующем временном шаге, алгоритм сигма-точечного фильтра Калмана генерирует набор значений состояния, распределенных вокруг среднего значения состояния. Алгоритм использует каждую сигму точки в качестве входа к функциям изменения состояния и измерения, чтобы получить новый набор преобразованных точек состояния. Среднее значение и ковариация преобразованных точек затем используются, чтобы получить оценки состояния и ошибочную ковариацию оценки состояния.

Распространением точек сигмы вокруг среднего значения состояния управляют два параметра α и κ. Третий параметр, β, влияет на веса преобразованных точек во время состояния и вычислений ковариации измерения.

α — Определяет распространение точек сигмы вокруг среднего значения состояния. Это обычно - маленькое положительное значение. Распространение точек сигмы пропорционально α. Меньшие значения соответствуют точкам сигмы ближе к среднему состоянию.
κ — Второй масштабный коэффициент, который обычно устанавливается в 0. Меньшие значения соответствуют точкам сигмы ближе к среднему состоянию. Распространение пропорционально квадратному корню κ.
β — Включает предварительные знания распределения состояния. Для Распределений Гаусса β = 2 оптимален.

Вы задаете эти параметры в Alpha\kappa, и Beta свойства сигма-точечного фильтра Калмана. Если вы знаете распределение ковариации состояния и состояния, можно настроить эти параметры, чтобы получить преобразование моментов высшего порядка распределения. Алгоритм может отследить только один пик в вероятностном распределении состояния. Если существует несколько peaks в распределенности вашей системы, можно настроить эти параметры так, чтобы точки сигмы остались вокруг одного пика. Например, выберите маленький Alpha сгенерировать точки сигмы близко к среднему значению состояния.

Ссылки

[1] Саймон, Дэн. Оптимальная оценка состояния: Кальман, H бесконечность и нелинейные подходы. Хобокен, NJ: Джон Вайли и сыновья, 2006.

[2] Ван дер Мерв, Рудольф и Эрик А. Вань. “Сигма-точечный фильтр Калмана Квадратного корня для состояния и Оценки Параметра”. 2 001 Международная конференция IEEE по вопросам Акустики, Речи и Обработки сигналов. Продолжения (CAT. № 01CH37221), 6:3461–64. Солт-Лейк-Сити, UT, США: IEEE, 2001. https://doi.org/10.1109/ICASSP.2001.940586.