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

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

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

В Simulink вы задаете эти функции в блоках Extended Kalman Filter и Unscented 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 блок используют неароматизированное преобразование, чтобы захватить распространение статистических свойств оценок состояния через нелинейные функции. Алгоритм сначала генерирует набор значений состояний, называемых sigma точек. Эти сигма- точки фиксируют среднее значение и ковариацию государственных оценок. Алгоритм использует каждую из точек сигмы как вход в функции перехода и измерения состояния, чтобы получить новый набор преобразованных точек состояния. Среднее значение и ковариация преобразованных точек затем используются, чтобы получить оценки состояния и ковариацию ошибки расчета состояния.

Предположим, что переходное и измерительное уравнения для нелинейной системы 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-й столбец $\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 sigma точек использование P[k|k-1] и R[k], где V количество элементов в измерительном шуме v[k]. The R[k] термин больше не добавляется в шаг 2 (d) алгоритма, потому что дополнительные точки захватывают влияние шума измерения на Py.
predict команда генерирует 2*(M+W)+1 sigma точек использование P[k|k] и Q[k], где W количество элементов в технологическом шуме w[k]. The Q[k] термин больше не добавляется в шаг 3 (d) алгоритма, потому что дополнительная сигма точек захватывать влияние шума процесса на P[k+1|k].

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

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

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

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

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

Ссылки

[1] Simon, Dan. Optimal State Estimation: Kalman, H Infinity и нелинейные подходы. Хобокен, Нью-Джерси: Джон Уайли и сыновья, 2006.

[2] Ван дер Мерве, Рудольф и Эрик А. Ван. «The Квадратного корня Сигма-точечного фильтра Калмана for State and Parameter-Estimation». 2001 Международная конференция IEEE по акустике, речи и обработке сигналов. Труды (кат. № 01CH37221), 6:3461–64. Солт-Лейк-Сити, ЮТ, США: IEEE, 2001. https://doi.org/10.1109/ICASSP.2001.940586.