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

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

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

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

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

Команда 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.

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

Блок 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 поддерживает несколько функций измерения. Эти измерения могут иметь различные шаги расчета, пока их шаг расчета является целочисленным кратным шаг расчета изменения состояния. В этом случае отдельный шаг исправления выполняется, соответствуя измерениям от каждой функции измерения.

Предыдущий алгоритм реализован, приняв аддитивные шумовые условия в уравнениях изменения состояния и измерения. Если шумовые условия являются недополнением, основные изменения в алгоритме:

Команда 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] Саймон, D. Оптимальная оценка состояния: Кальман, H бесконечность и нелинейные подходы. John Wiley and Sons Inc., 2006.

Смотрите также

Функции

extendedKalmanFilter | unscentedKalmanFilter

Блоки

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

Документация