Калибровка магнитометра с применением обобщенного фильтра Калмана

5 Апр в 22:37

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

Про калибровку магнитометров писали уже неоднократно. Однако, калибровка обобщённым фильтром Калмана учёта магнитомягких материалов обнаружилась лишь в статьях Extended Kalman Filter Based Gyroscope (авторы Ke Han, He Han, Zhifeng Wang, Feng Xu) и Real-Time Magnetic Field Calibration Method Based on Extended Kalman Filter (авторы LI Wenkuan, CAI Haoyuan, ZHAO Shenglin, LIU Chunxiu). Возможно, есть и другие статьи, но я ориентировался на вот эти. Так как сходу разобраться в этих статьях не так-то просто (я разбирался две недели с коллегой-математиком, ибо в статьях есть и ошибки и нет акцента на важных моментах из-за чего легко ошибиться при построении собственного фильтра), то ниже я дам пошаговый алгоритм калибровки магнитометра по методике из этих статей.

Итак, поехали!

1 Основные положения

Пусть имеется внешнее магнитное поле $\overrightarrow{B_{\text{вн}}}$ (например, магнитное поле Земли 🙂 ). Тогда истинное значение вектора магнитного поля при вращении определяется как $\overrightarrow{B_{\text{ист}}}=\mathbf{R}\cdot\overrightarrow{B_{\text{вн}}}$ , где $\mathbf{R}$ — матрица поворота. Влияние магнитомягких и магнитотвёрдых материалов (всё это вращается вместе с магнитометром, естественно! Например, это могут быть корпуса микросхем или элементы конструкции.) на показания магнитометров можно описать с помощью матрицы анизотропии магнитной проницаемости $\mathbf{W}$ и вектора смещения нуля $\overrightarrow{V}$ магнитометра, соответственно. В этом случае показания магнитометра (без учёта шума измерения) будут определяться как $\overrightarrow{B_{\text{изм}}}=\mathbf{W}\cdot\mathbf{R}\cdot\overrightarrow{B_{\text{вн}}}+\overrightarrow{V}$
, где $\overrightarrow{V}=\left\{ V_{X},V_{Y},V_{Z}\right\}$ и $\mathbf{W}=\left[\begin{array}{ccc} W_{11} & W_{12} & W_{13}\\ W_{21} & W_{22} & W_{23}\\ W_{31} & W_{32} & W_{33} \end{array}\right]$ . Матрица $\mathbf{W}$ должна быть симметричная, что позволяет задать её как $\mathbf{W}=\left[\begin{array}{ccc} W_{11} & W_{12} & W_{13}\\ W_{12} & W_{22} & W_{23}\\ W_{13} & W_{23} & W_{33} \end{array}\right]$ .
Для определения матрицы $\mathbf{W}$ и вектора $\overrightarrow{V}$ можно применить обобщённый фильтр Калмана.

Пусть вектор состояния $\overrightarrow{X}=\left\{ \overrightarrow{B_{\text{ист}}},W_{11},W_{22},W_{33},W_{12},W_{13},W_{23},\overrightarrow{V}\right\}$
$=\left\{ B_{\text{ист}X},B_{\text{ист}Y},B_{\text{ист}Z},W_{11},W_{22},W_{33},W_{12},W_{13},W_{23},V_{X},V_{Y},V_{Z}\right\}$
содержит предсказанные фильтром истинные значения магнитного поля $\overrightarrow{B_{\text{ист}}}=\left\{ B_{\text{ист}X},\:B_{\text{ист}Y},\:B_{\text{ист}Z}\right\}$ элементы матрицы $\mathbf{W}$ (эта матрица симметричная, поэтому достаточно всего 6 элементов) и определённые смещения нулей магнитометров $V_{X},V_{Y},V_{Z}$ .

На каждой итерации работы фильтра должны быть известны показания измерений магнитометра $\overrightarrow{B_{\text{изм}}}$ и три угла вращения (крен $inline$ , тангаж $inline$ и рысканье $inline$ ) по которым задаётся матрица поворота $\mathbf{R}$ , имеющая для каждой итерации $inline$ вид

$\mathbf{R_{k}}=\left[\begin{array}{ccc} cy\cdot cp & -cr\cdot sy+sr\cdot cy\cdot sp & sr\cdot sy+cr\cdot cy\cdot sp\\ sy\cdot cp & cr\cdot cy+sr\cdot sy\cdot sp & -sr\cdot cy+cr\cdot sy\cdot sp\\ -sp & sr\cdot cp & cr\cdot cp \end{array}\right],$

где $cr=cos(roll),\:sr=sin(roll)$ , $\:cp=cos(pitch),\:sp=sin(pitch)$ , $\:cy=cos(yaw),\:sy=sin(yaw)$ .
Сами абсолютные значения углов вращения как таковые значения не имеют, но вот их изменения от итерации к итерации должны соответствовать действительному изменению углов. То есть, если был поворот на 10 градусов между итерациями, то переданный на следующую итерацию угол должен быть изменён на эти же 10 градусов. Так-то эта матрица вовсе не нужна, если вы можете построить матрицу вращения за итерацию непосредственно по гироскопам.

2 Инициализация фильтра Калмана.

1. Инициализируем вектор состояний шага $inline$ как $X_{0}=\left\{ B_{\text{изм}X},B_{\text{изм}Y},B_{\text{изм}Z},1,1,1,0,0,0,0,0,0\right\}$ . То есть вектор $\overrightarrow{B_{\text{ист}}}$ приравняем к текущим показаниям магнитометров, матрицу $\mathbf{W}$ зададим единичной, а вектор $\overrightarrow{V}$ зададим нулевым.

2. Инициализируем ковариационную матрицу состояний $\mathbf{P\mathrm{_{0}=\left[I_{9x9}\right]}}$ . На диагонали этой матрицы расположены дисперсии изменений элементов вектора состояний. Имеет смысл поставить для инициализации максимальное значение, перекрывающее диапазон изменения элемента вектора состояний. У меня в модели показания магнитометров изменяются от -1 до 1, а потому я задаю эту матрицу единичной.

3. Инициализируем матрицу вращения предыдущего шага $\mathbf{R}_{0}$ равной текущей матрице вращения, т.е. $\mathbf{R}_{0}=\mathbf{R}_{k}$ .

3 Предсказание

1. Вычисляем изменение матрицы поворота $\triangle\mathbf{R}=\mathbf{R}_{k}^{T}\cdot\mathbf{R}_{k-1}$ .

2. Задаём матрицу динамики (прогнозирования) $\mathbf{F}$ . Так как у нас обобщённый фильтр Калмана, матрица динамики должна содержать частные производные функции динамики $\overrightarrow{f}(\overrightarrow{X})$ . Функция динамики определяется как

$\overrightarrow{f}(\overrightarrow{X_{k-1}})=\left[\begin{array}{c} B_{\text{ист}X,k}=B_{\text{ист}X,k-1}\cdot\triangle R_{11}+B_{\text{ист}Y,k-1}\cdot\triangle R_{12}+B_{\text{ист}Z,k-1}\cdot\triangle R_{13}\\ B_{\text{ист}Y,k}=B_{\text{ист}X,k-1}\cdot\triangle R_{21}+B_{\text{ист}Y,k-1}\cdot\triangle R_{22}+B_{\text{ист}Z,k-1}\cdot\triangle R_{23}\\ B_{\text{ист}Z,k}=B_{\text{ист}X,k-1}\cdot\triangle R_{31}+B_{\text{ист}Y,k-1}\cdot\triangle R_{32}+B_{\text{ист}Z,k-1}\cdot\triangle R_{33}\\ W_{11,k}=W_{11,k-1}\\ W_{22,k}=W_{22,k-1}\\ W_{33,k}=W_{33,k-1}\\ W_{12,k}=W_{12,k-1}\\ W_{13,k}=W_{13,k-1}\\ W_{23,k}=W_{23,k-1}\\ V_{X,k}=V_{X,k-1}\\ V_{Y,k}=V_{Y,k-1}\\ V_{Z,k}=V_{Z,k-1} \end{array}\right]$

Таким образом, матрица частных производных функции динамики (якобиан) определяется как

$\mathbf{F}=\frac{\partial\overrightarrow{f}(\overrightarrow{X})}{d\overrightarrow{X}}=\left[\begin{array}{cccccccccccc} \triangle R_{11} & \triangle R_{12} & \triangle R_{13} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ \triangle R_{21} & \triangle R_{22} & \triangle R_{23} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ \triangle R_{31} & \triangle R_{32} & \triangle R_{33} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{array}\right]$

$=\left[\begin{array}{cc} \mathbf{\bigtriangleup R}_{3x3} & 0_{3x9}\\ 0_{9x3} & I_{9x9} \end{array}\right]$

3. Вычисляем прогноз вектора состояний $\mathbf{\mathbf{\overrightarrow{\mathrm{X_{\text{прогноз}}}}}}$ . Так как функция $\overrightarrow{f}(\overrightarrow{X})$ линейная, то $\mathbf{\overrightarrow{\mathrm{X_{\text{прогноз}}}}}=\overrightarrow{f}(\overrightarrow{X})=\mathbf{F}\cdot\overrightarrow{X_{k-1}}$ .

4. Вычисляем прогноз ковариационной матрицы состояний $\mathbf{P}=\mathbf{F}\cdot\mathbf{P}_{k-1}\cdot\mathbf{F^{T}}+\mathbf{Q}$ . Для этого прогноза должна быть известна матрица ковариации ошибки модели $\mathbf{Q}$ . Если установить очень маленькие значения, то это будет значить, что мы считаем, что наша модель точно описывает процесс. Если же установить большое значение $\mathbf{Q}$ , то это означает, что наша модель может содержать неточности. В нашем случае будем считать, что модель точно описывает процесс, а потому матрицу $\mathbf{Q}$ сделаем равной нулю. В работе Real-Time Magnetic Field Calibration Method Based on Extended Kalman Filter предложена модель для каждой итерации фильтра использовать $\mathbf{Q\mathrm{_{k-1}=\left[\begin{array}{cc} \mathbf{A}_{3x3} & 0_{3x9}\\ 0_{9x3} & 0_{9x9} \end{array}\right]}}$
, где $\mathbf{A}=\overrightarrow{B}_{\text{изм}}\cdot\overrightarrow{B}_{\text{изм}}^{T}\cdot\mathbf{\sigma_{\triangle R}^{2}}\cdot\bigtriangleup t^{2}$ . Значение $\bigtriangleup t$ — интервал работы фильтра, а матрица
$\mathbf{\sigma_{\triangle R}^{2}}$ определена из эксперимента для используемого оборудования как $\mathbf{\sigma_{\triangle R}^{2}}=\left[\begin{array}{ccc} 1.2\cdot10^{\text{−}8} & 3.0\cdot10^{\text{−}3} & 1.4\cdot10^{\text{−}3}\\ 3.0\cdot10^{\text{−}3} & 4.0\cdot10^{\text{−}8} & 2.5\cdot10^{\text{−}3}\\ 1.4\cdot10^{\text{−}3} & 2.5\cdot10^{\text{−}3} & 6.5\cdot10^{\text{−}8} \end{array}\right]$ . Если я напрямую применю их матрицу (а почему бы и нет для моделирования-то?), мой фильтр разваливается. Это потому, что диапазон показаний датчиков у них вовсе не -1…1, а гораздо больше. Поэтому для моего диапазона все значения надо поделить эдак на 10000.

4 Корректировка

1. Из полученного прогнозного вектора состояний $\mathbf{\overrightarrow{\mathrm{X_{\text{прогноз}}}}}$ можно построить матрицу измерений $\mathbf{H}$ . Для обобщённого фильтра Калмана она должна содержать частные производные функции измерений

$\overrightarrow{h}(\overrightarrow{X)}=\left[\begin{array}{c} B_{\text{изм}X}=B_{\text{ист}X}\cdot W_{11}+B_{\text{ист}Y}\cdot W_{12}+B_{\text{ист}Z}\cdot W_{13}+V_{X}\\ B_{\text{изм}Y}=B_{\text{ист}X}\cdot W_{21}+B_{\text{ист}Y}\cdot W_{22}+B_{\text{ист}Z}\cdot W_{23}+V_{Y}\\ B_{\text{изм}Z}=B_{\text{ист}X}\cdot W_{31}+B_{\text{ист}Y}\cdot W_{32}+B_{\text{ист}Z}\cdot W_{33}+V_{Z} \end{array}\right]$

.
Таким образом, с учётом симметричности матрицы $\mathbf{W}$ , мы получаем якобиан

$\mathbf{H}=\frac{\partial\overrightarrow{h}(\overrightarrow{X})}{d\overrightarrow{X}}=\left[\begin{array}{ccc} W_{11} & W_{12} & W_{13}\\ W_{12} & W_{22} & W_{23}\\ W_{13} & W_{23} & W_{33} \end{array}\begin{array}{ccc} B_{\text{изм}X} & 0 & 0\\ 0 & B_{\text{изм}Y} & 0\\ 0 & 0 & B_{\text{изм}Z} \end{array}\begin{array}{ccc} B_{\text{изм}Y} & B_{\text{изм}Z} & 0\\ B_{\text{изм}X} & 0 & B_{\text{изм}Z}\\ 0 & B_{\text{изм}X} & B_{\text{изм}\text{Y}} \end{array}\begin{array}{ccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{array}\right]$

.
Элементы этой матрицы нужно взять из прогнозного вектора состояний $\mathbf{\overrightarrow{\mathrm{X_{\text{прогноз}}}}}$ .

2. Применим функцию измерений к предсказанному вектору состояний:
$\overrightarrow{D}=f(\mathbf{\overrightarrow{\mathrm{X_{\text{прогноз}}}})=W\cdot\overrightarrow{\mathrm{B_{\text{ист}}}}+\mathrm{\overrightarrow{V}}}$ .

3. Вычисляем ошибку $\overrightarrow{Y}=\left[\begin{array}{c} B_{\text{изм}X}\\ B_{\text{изм}Y}\\ B_{\text{изм}Z} \end{array}\right]-\overrightarrow{D}$ .

4. Зададим матрицу ковариации шума измерений $\mathbf{E}=\sigma^{2}\cdot\mathbf{I}_{3x3}$ . Для шума $\sigma^{2}$ примем равной $inline$ (для моего диапазона показаний от -1 до 1 я взял такое число для моделирования). Считаем, что измерения не влияют друг на друга, а потому присутствуют только диагональные элементы матрицы.

5. Считаем усиление фильтра: $\mathbf{K}=\mathbf{P}\cdot\mathbf{H}^{T}\cdot\mathbf{S}^{-1}$ , где $\mathbf{S}=\mathbf{H}\cdot\mathbf{P}\cdot\mathbf{H}^{T}+\mathbf{E}$ .

6. Обновляем вектор состояния: $\overrightarrow{X_{k}}=\overrightarrow{\mathrm{X_{\text{прогноз}}}}+\mathbf{K}\cdot\overrightarrow{Y}$ .

7. Обновляем ковариационную матрицу состояний: $\mathbf{P}_{k}=(\mathbf{I}_{12x12}-\mathbf{K}\cdot\mathbf{H})\cdot\mathbf{P}$ .

8. Обновляем старые переменные в новое состояние: $\overrightarrow{X_{k-1}}=\overrightarrow{X}_{k} , \mathbf{P}_{k-1}=\mathbf{P}_{k}, \mathbf{R}_{k-1}=\mathbf{R}_{k}$ .

9. Переходим к первому пункту этапа Предсказание.

Через некоторое время в компонентах вектора $\overrightarrow{X_{k}}$ будет определена матрица учёта магнитомягких материалов и смещений. Осталось только из реальных показаний магнитометра вычесть эти смещения и умножить полученный вектор на обратную матрицу учёта магнитомягких материалов $\mathbf{W}$ . То, что вышло и есть искомый вектор внешнего магнитного поля. Вот и всё! Хочу, правда, предупредить, что при больших значениях (десятые доли) матрицы $\mathbf{W}$ её компоненты могут быть определены не очень точно. То же касается и смещений. Во всяком случае, у меня при моделировании получалось так.