姿勢を表すクオタニオン
q \bm{q} q に関する支配方程式は、剛体系の角速度を
ω \bm{\omega} ω として次のように記述される。
q ˙ = 1 2 [ − r 1 − r 2 − r 3 r 0 − r 3 r 2 r 3 r 0 − r 1 − r 2 r 1 r 0 ] ω ≡ J q ( q ) ω , \begin{align}
\dot{\bm{q}} &=
\frac{1}{2}
\begin{bmatrix}
-r_1 & -r_2 & -r_3 \\
r_0 & -r_3 & r_2\\
r_3 & r_0 & -r_1\\
-r_2 & r_1 & r_0
\end{bmatrix} \bm{\omega} \\
&\equiv\bm{J}_{q}(\bm{q})\bm{\omega},
\end{align} q ˙ = 2 1 − r 1 r 0 r 3 − r 2 − r 2 − r 3 r 0 r 1 − r 3 r 2 − r 1 r 0 ω ≡ J q ( q ) ω , ただし、
J q ( q ) \bm{J}_{q}(\bm{q}) J q ( q ) はヤコビ行列を表す。
計算機で状態の予測と修正を行うために、離散時間モデルを導出する。計算機のサンプリング時間を
T s T_{\rm s} T s として、クオタニオンの
1 1 1 ステップ先予測モデルを次のように設計する。
q k + 1 = q k + T s J q ( q k ) ω k \begin{align}
\bm{q}_{k+1} &= \bm{q}_{k} + T_{\rm s}\bm{J}_{q}(\bm{q}_{k})\bm{\omega}_{k}
\end{align} q k + 1 = q k + T s J q ( q k ) ω k ただし、下添字
k k k はサンプリング点を表すものとする。このモデルでは矩形積分による予測を行っており、積分誤差はプロセス雑音と見做すものとする。本誤差はサンプリング時間が小さくなるにつれて減少し、観測器の誤差修正によってこれによる影響を抑圧するものとする。
観測器の修正は、基準となる状態量と予測された状態量を比較し、その誤差が減少するように行われる。回転するシステムでは機体座標系が基準座標系に対して回転するため、状態量をいずれかの座標系に射影して観測量の比較を行う必要がある。機体の姿勢を表すクオタニオン
q \bm{q} q が与えられた場合、機体座標系で
x b ∈ R 3 \bm{x}^{\rm b} \in \mathbb{R}^{3} x b ∈ R 3 と表されるベクトルは基準座標系で次のように表される。
x r = R b r ( q ) x b \begin{align}
\bm{x}^{\rm r} &= \bm{R}_{\rm br}(\bm{q}) \bm{x}^{\rm b}
\end{align} x r = R br ( q ) x b ただし、
R b r \bm{R}_{\rm br} R br は以下に示す回転行列とする。
R b r ( q ) ≡ [ 1 − 2 ( r 2 2 + r 3 2 ) 2 ( r 1 r 2 − r 0 r 3 ) 2 ( r 1 r 3 + r 0 r 2 ) 2 ( r 1 r 2 + r 0 r 3 ) 1 − 2 ( r 2 2 + r 3 2 ) 2 ( r 2 r 3 − r 0 r 1 ) 2 ( r 1 r 3 − r 0 r 2 ) 2 ( r 2 r 3 + r 0 r 1 ) 1 − 2 ( r 1 2 + r 2 2 ) ] \begin{align}
\bm{R}_{\rm br}(\bm{q}) \equiv
\begin{bmatrix}
1 - 2\left(r_2^2 + r_3^2\right) & 2\left(r_1r_2 - r_0r_3\right) & 2\left(r_1r_3 + r_0r_2\right)\\
2\left(r_1r_2 + r_0r_3\right) & 1 - 2\left(r_2^2 + r_3^2\right) & 2\left(r_2r_3 - r_0r_1\right)\\
2\left(r_1r_3 - r_0r_2\right) & 2\left(r_2r_3 + r_0r_1\right) & 1 - 2\left(r_1^2 + r_2^2\right)
\end{bmatrix}
\end{align} R br ( q ) ≡ 1 − 2 ( r 2 2 + r 3 2 ) 2 ( r 1 r 2 + r 0 r 3 ) 2 ( r 1 r 3 − r 0 r 2 ) 2 ( r 1 r 2 − r 0 r 3 ) 1 − 2 ( r 2 2 + r 3 2 ) 2 ( r 2 r 3 + r 0 r 1 ) 2 ( r 1 r 3 + r 0 r 2 ) 2 ( r 2 r 3 − r 0 r 1 ) 1 − 2 ( r 1 2 + r 2 2 ) また、回転行列の性質より、逆変換は次のように表現される。
x b = R b r T ( q ) x r \begin{align}
\bm{x}^{\rm b} &= \bm{R}_{\rm br}^{\mathrm T}(\bm{q}) \bm{x}^{\rm r}
\end{align} x b = R br T ( q ) x r 非線形システムの予測誤差の二乗和を最小化することを目的として、ガウス・ニュートン法を使用する。ガウス・ニュートン法は非線形写像のパラメータ推定に使用することができ、元および像が
x , y ∈ R m \bm{x}, \bm{y} \in \mathbb{R}^{m} x , y ∈ R m として与えられた時、以下で与えられる予測残差
e k \bm{e}_{k} e k の
L 2 L_2 L 2 ノルムを最小化する写像
f f f を反復計算によって求める。
e k = y k − y ^ k y ^ k = f ( x , p ) \begin{align}
\bm{e}_{k} &= \bm{y}_{k} - \hat{\bm{y}}_{k} \\
\hat{\bm{y}}_{k} &= f(\bm{x}, \bm{p})
\end{align} e k y ^ k = y k − y ^ k = f ( x , p ) ただし、
k k k は反復試行数、
p ∈ R n ( n ≤ m ) \bm{p} \in \mathbb{R}^{n}\ (n\le m) p ∈ R n ( n ≤ m ) は写像のパラメータであり、写像
f f f を求めることはこのパラメータ
p \bm{p} p を求めることと等価となる。ここで、関数
f f f のヤコビ行列
J f \bm{J}_{f} J f を用いて次のようにパラメータ更新を行うことで、予測残差の
L 2 L_2 L 2 ノルムが減少する方向に写像が修正される。
J f ≡ ∂ f ∂ p ∈ R m × n p k + 1 = p k + J f + ( p k ) e k \begin{align}
\bm{J}_{f} &\equiv \frac{\partial f}{\partial \bm{p}} \in \mathbb{R}^{m\times n}\\
\bm{p}_{k+1} &= \bm{p}_{k} + \bm{J}_{f}^{+}(\bm{p}_{k})\bm{e}_{k}
\end{align} J f p k + 1 ≡ ∂ p ∂ f ∈ R m × n = p k + J f + ( p k ) e k ただし、
J f + \bm{J}_{f}^{+} J f + は
J f \bm{J}_{f} J f の一般化逆行列であり、
n ≤ m n\le m n ≤ m の条件下で次のように定義される。
J f + = ( J f T J f ) − 1 J f T \begin{align}
\bm{J}_{f}^{+} = \left(\bm{J}_{f}^{\mathrm T}\bm{J}_{f}\right)^{-1}\bm{J}_{f}^{\mathrm T}
\end{align} J f + = ( J f T J f ) − 1 J f T この一般化逆行列
J f + \bm{J}_{f}^{+} J f + は
J f \bm{J}_{f} J f の左逆元となることに留意する。この方法は、初期推定値が真値から離れている場合や写像
f f f の非線形性が大きい場合にはパラメータ推定値が発散する恐れがある。そこで、縮小因子
γ \bm{\gamma} γ を導入して更新方程式を次のように修正する。
p k + 1 = p k + γ J f + e k \begin{align}
\bm{p}_{k+1} &= \bm{p}_{k} + \bm{\gamma}\bm{J}_{f}^{+}\bm{e}_{k}
\end{align} p k + 1 = p k + γ J f + e k
ガウス・ニュートン法を使用する条件として、参照データ
y \bm{y} y の次元
m m m が パラメータ
p \bm{p} p の次元
n n n 以上であることとしている。これは、写像によって情報が退化した場合にパラメータ修正則が定まらないためと考えられる。
便宜上、写像
f f f のヤコビ行列
J f \bm{J}_{f} J f を用いて、
y ^ k \hat{\bm{y}}_{k} y ^ k を近似的に次のように表現する。
y ~ k = J f p k \begin{align}
\tilde{\bm{y}}_{k} &= \bm{J}_{f}\bm{p}_{k}
\end{align} y ~ k = J f p k このとき、パラメータ修正項を
Δ \bm{\Delta} Δ とすると、これは近似的に次のように表される。
Δ k ≈ J f + ( y k − y ~ k ) = J f + y k − J f + J f p k \begin{align}
\bm{\Delta}_{k} &\approx \bm{J}_{f}^{+}(\bm{y}_{k} - \tilde{\bm{y}}_{k}) \\
&= \bm{J}_{f}^{+}\bm{y}_{k} - \bm{J}_{f}^{+}\bm{J}_{f}\bm{p}_{k}
\end{align} Δ k ≈ J f + ( y k − y ~ k ) = J f + y k − J f + J f p k ここで、
J f + \bm{J}_{f}^{+} J f + が
J f \bm{J}_{f} J f の左逆元となる場合には、右辺第二項は
p k \bm{p}_{k} p k に復元される。一方で、
n > m n > m n > m となる場合には、一般可逆行列は左逆元とならないために復元されない。これは
J f \bm{J}_{f} J f の零空間 (退化空間) に起因する退化が原因と考えられる。パラメータ
p k \bm{p}_{k} p k が 零空間
K e r ( J f ) \rm Ker({\it \bm{J}_{f}}) Ker ( J f ) の成分
p n u l l \bm{p}_{\rm null} p null を持ち、次のように記述される場合について考える。
p k = p m + p n u l l p m ∉ K e r ( J f ) \begin{align}
\bm{p}_{k} &= \bm{p}_{\rm m} + \bm{p}_{\rm null}\\
\bm{p}_{\rm m} &\notin {\rm Ker}(\bm{J}_{f})
\end{align} p k p m = p m + p null ∈ / Ker ( J f ) ここで、零空間成分
p n u l l \bm{p}_{\rm null} p null は写像
f f f によって退化し、像の観測情報から元を逆推定することができない。実際に、パラメータ修正項
Δ \bm{\Delta} Δ の近似値は次のように記述される。
Δ k ≈ J f + y k − p m \begin{align}
\bm{\Delta}_{k} &\approx \bm{J}_{f}^{+}\bm{y}_{k} - \bm{p}_{\rm m}
\end{align} Δ k ≈ J f + y k − p m このように、零空間の成分が修正項に反映されず、零空間成分が不定となる。したがって、
n > m n > m n > m の条件下ではパラメータ決定に冗長自由度が発生し、真値を推定することが困難となる。
上記で設計した状態更新モデル、観測モデル、および推定量修正則に従って姿勢オブザーバを設計する。ここではシステムが慣性計測ユニットおよび地磁気センサを搭載することを想定し、機体角速度
ω \bm{\omega} ω 、機体加速度
a \bm{a} a 、および地磁気
m \bm{m} m が測定されるものとする。また。地磁気センサの出力は適切な写像によって単位球上に移されているものとする。
システムの状態予測に当たっては以下の離散モデルを使用する。
q k + 1 = q k + T s J q ( q k ) ω k \begin{align}
\bm{q}_{k+1} &= \bm{q}_{k} + T_{\rm s}\bm{J}_{q}(\bm{q}_{k})\bm{\omega}_{k}
\end{align} q k + 1 = q k + T s J q ( q k ) ω k ここで、
ω k \bm{\omega}_{k} ω k は角速度の真値であるが、予測ステップではセンサ計測値からこの値を推定する必要がある。角速度センサはオフセットを持つことがあり、その値が時間の経過に伴い変動することから、角速度の推定値
ω ^ \hat{\bm{\omega}} ω ^ を測定値
ω s \bm{\omega}^{\rm s} ω s および バイアスモデル
ω b \bm{\omega}^{\rm b} ω b を用いて次のように表す。
ω ^ = ω s + ω b \begin{align}
\hat{\bm{\omega}} &= \bm{\omega}^{\rm s} + \bm{\omega}^{\rm b}
\end{align} ω ^ = ω s + ω b また、バイアスの時間変動が十分に小さいものとし、動的モデルを次のように設定する。
d d t ω b = 0 \begin{align}
\frac{d}{dt}\bm{\omega}^{\rm b} = \bm{0}
\end{align} d t d ω b = 0 以上より、システムの状態
x \bm{x} x を次のように設定し、予測ステップを設計する。
x = [ q ω b ] ∈ R 7 x ~ k + 1 = [ I 4 T s J q ( q ^ k ) 0 0 ] x ^ k + [ T s J q ( q ^ k ) 0 ] ω k s \begin{align}
\bm{x} &=
\begin{bmatrix}
\bm{q} \\ \bm{\omega}_{\rm b}
\end{bmatrix} \in \mathbb{R}^7\\
\tilde{\bm{x}}_{k+1} &=
\begin{bmatrix}
\bm{I}_{4} & T_{\rm s}\bm{J}_{q}(\hat{\bm{q}}_{k}) \\ \bm{0} & \bm{0}
\end{bmatrix} \hat{\bm{x}}_{k}
+
\begin{bmatrix}
T_{\rm s}\bm{J}_{q}(\hat{\bm{q}}_{k}) \\ \bm{0}
\end{bmatrix} \bm{\omega}^{\rm s}_{k}
\end{align} x x ~ k + 1 = [ q ω b ] ∈ R 7 = [ I 4 0 T s J q ( q ^ k ) 0 ] x ^ k + [ T s J q ( q ^ k ) 0 ] ω k s だたし、上添字付き変数
X ^ , X ~ \hat{X}, \tilde{X} X ^ , X ~ はそれぞれ
X X X の推定値および予測値を表すものとする。
観測情報から得られる修正勾配を用いて、状態を修正する。観測系における基準情報は、基準座標系において計測される重力加速度
a g r \bm{a}^{\rm r}_{\rm g} a g r と地磁気
m e r \bm{m}^{\rm r}_{\rm e} m e r とする。
a g r = [ 0 0 g ] T m e r = [ 1 0 0 ] T \begin{align}
\bm{a}^{\rm r}_{\rm g} &=
\begin{bmatrix} 0 & 0 & g \end{bmatrix}^{\mathrm T} \\
\bm{m}^{\rm r}_{\rm e} &=
\begin{bmatrix} 1 & 0 & 0 \end{bmatrix}^{\mathrm T}
\end{align} a g r m e r = [ 0 0 g ] T = [ 1 0 0 ] T ただし、
g g g は重力加速度を表す。これらを纏めて基準観測量
y \bm{y} y とする。
y = [ a g r m e r ] ∈ R 6 \begin{align}
\bm{y} = \begin{bmatrix} \bm{a}^{\rm r}_{\rm g} \\ \bm{m}^{\rm r}_{\rm e} \end{bmatrix} \in \mathbb{R}^{6}
\end{align} y = [ a g r m e r ] ∈ R 6 機体の姿勢を表すクオタニオン が
q \bm{q} q と表される場合、機体座標系で計測された重力加速度 および地磁気を
a g b , m e b \bm{a}^{\rm b}_{\rm g}, \bm{m}^{\rm b}_{\rm e} a g b , m e b とすると、基準座標系の重力加速度
a g r \bm{a}^{\rm r}_{\rm g} a g r および地磁気
m e r \bm{m}^{\rm r}_{\rm e} m e r は次のように推定される。
a ^ g r = R b r ( x ^ k ) a g b m ^ e r = R b r ( x ^ k ) m e b \begin{align}
\hat{\bm{a}}^{\rm r}_{\rm g} &= \bm{R}_{\rm br}(\hat{\bm{x}}_{k}) \bm{a}^{\rm b}_{\rm g}\\
\hat{\bm{m}}^{\rm r}_{\rm e} &= \bm{R}_{\rm br}(\hat{\bm{x}}_{k}) \bm{m}^{\rm b}_{\rm e}
\end{align} a ^ g r m ^ e r = R br ( x ^ k ) a g b = R br ( x ^ k ) m e b ただし、
R r b \bm{R}_{\rm rb} R rb は以下に示す回転行列とする。
R r b ( x ) ≡ [ 1 − 2 ( r 2 2 + r 3 2 ) 2 ( r 1 r 2 + r 0 r 3 ) 2 ( r 1 r 3 − r 0 r 2 ) 2 ( r 1 r 2 − r 0 r 3 ) 1 − 2 ( r 2 2 + r 3 2 ) 2 ( r 2 r 3 + r 0 r 1 ) 2 ( r 1 r 3 + r 0 r 2 ) 2 ( r 2 r 3 − r 0 r 1 ) 1 − 2 ( r 1 2 + r 2 2 ) ] \begin{align}
\bm{R}_{\rm rb}(\bm{x}) \equiv
\begin{bmatrix}
1 - 2\left(r_2^2 + r_3^2\right) & 2\left(r_1r_2 + r_0r_3\right) & 2\left(r_1r_3 - r_0r_2\right)\\
2\left(r_1r_2 - r_0r_3\right) & 1 - 2\left(r_2^2 + r_3^2\right) & 2\left(r_2r_3 + r_0r_1\right)\\
2\left(r_1r_3 + r_0r_2\right) & 2\left(r_2r_3 - r_0r_1\right) & 1 - 2\left(r_1^2 + r_2^2\right)
\end{bmatrix}
\end{align} R rb ( x ) ≡ 1 − 2 ( r 2 2 + r 3 2 ) 2 ( r 1 r 2 − r 0 r 3 ) 2 ( r 1 r 3 + r 0 r 2 ) 2 ( r 1 r 2 + r 0 r 3 ) 1 − 2 ( r 2 2 + r 3 2 ) 2 ( r 2 r 3 − r 0 r 1 ) 2 ( r 1 r 3 − r 0 r 2 ) 2 ( r 2 r 3 + r 0 r 1 ) 1 − 2 ( r 1 2 + r 2 2 ) 以上より、予測観測量
y ^ k \hat{\bm{y}}_{k} y ^ k は次のように記述される。
y ^ k = [ a ^ g r m ^ e r ] = [ 2 ( r ^ 1 r ^ 3 − r ^ 0 r ^ 2 ) g 2 ( r ^ 2 r ^ 3 + r ^ 0 r ^ 1 ) g ( 1 − 2 ( r ^ 1 2 + r ^ 2 2 ) ) g 1 − 2 ( r ^ 2 2 + r ^ 3 2 ) 2 ( r ^ 1 r ^ 2 − r ^ 0 r ^ 3 ) 2 ( r ^ 1 r ^ 3 + r ^ 0 r ^ 2 ) ] \begin{align}
\hat{\bm{y}}_{k} =
\begin{bmatrix} \hat{\bm{a}}^{\rm r}_{\rm g} \\ \hat{\bm{m}}^{\rm r}_{\rm e} \end{bmatrix}
=
\begin{bmatrix}
2\left(\hat{r}_1\hat{r}_3 - \hat{r}_0\hat{r}_2\right)g\\
2\left(\hat{r}_2\hat{r}_3 + \hat{r}_0\hat{r}_1\right)g\\
(1 - 2\left(\hat{r}_1^2 + \hat{r}_2^2\right))g\\
1 - 2\left(\hat{r}_2^2 + \hat{r}_3^2\right)\\
2\left(\hat{r}_1\hat{r}_2 - \hat{r}_0\hat{r}_3\right)\\
2\left(\hat{r}_1\hat{r}_3 + \hat{r}_0\hat{r}_2\right)
\end{bmatrix}
\end{align} y ^ k = [ a ^ g r m ^ e r ] = 2 ( r ^ 1 r ^ 3 − r ^ 0 r ^ 2 ) g 2 ( r ^ 2 r ^ 3 + r ^ 0 r ^ 1 ) g ( 1 − 2 ( r ^ 1 2 + r ^ 2 2 ) ) g 1 − 2 ( r ^ 2 2 + r ^ 3 2 ) 2 ( r ^ 1 r ^ 2 − r ^ 0 r ^ 3 ) 2 ( r ^ 1 r ^ 3 + r ^ 0 r ^ 2 ) 設計の便宜上、
y ^ k \hat{\bm{y}}_{k} y ^ k の算出に関して写像
h h h を用いて次のように記述する。
y ^ k = h ( x ^ k ) \begin{align}
\hat{\bm{y}}_{k} = h(\hat{\bm{x}}_{k})
\end{align} y ^ k = h ( x ^ k ) 以上の
y , y ^ k \bm{y}, \hat{\bm{y}}_{k} y , y ^ k を使用して予測残差
e k \bm{e}_{k} e k が算出される。この予測残差
e k \bm{e}_{k} e k を小さくするようにパラメータ
x \bm{x} x を修正するためガウス・ニュートン法を適用したいが、今回の設計条件では写像
h h h のヤコビ行列には零空間が存在する。
J h ≡ ∂ h ∂ x ^ ∈ R 6 × 7 \bm{J}_{h} \equiv \frac{\partial h}{\partial \hat{\bm{x}}} \in \mathbb{R}^{6\times 7} J h ≡ ∂ x ^ ∂ h ∈ R 6 × 7 そこで、
ω b \bm{\omega}^{\rm b} ω b について以下の仮定を置き、観測系のヤコビ行列のランクを下げる。
∂ h ∂ ω b ≈ 0 → J h ≈ [ ∂ h ∂ q ^ 0 ] \begin{align}
\frac{\partial h}{\partial \bm{\omega}^{\rm b}} &\approx \bm{0} \\
\rightarrow \bm{J}_{h} &\approx \begin{bmatrix} \cfrac{\partial h}{\partial \hat{\bm{q}}} & \bm{0} \end{bmatrix}
\end{align} ∂ ω b ∂ h → J h ≈ 0 ≈ [ ∂ q ^ ∂ h 0 ] 低次元化されたヤコビ行列
J h ′ \bm{J}_{h'} J h ′ を用いて、予測誤差
e k \bm{e}_{k} e k から
q \bm{q} q に関する修正項
Δ k q \bm{\Delta}^{q}_{k} Δ k q を得る。
J h ′ ≡ ∂ h ∂ q ^ = [ − 2 g r ^ 2 2 g r ^ 3 − 2 g r ^ 0 2 g r ^ 1 2 g r ^ 1 2 g r ^ 0 2 g r ^ 3 2 g r ^ 2 2 g r ^ 0 − 2 g r ^ 1 − 2 g r ^ 2 2 g r ^ 3 2 r ^ 0 2 r ^ 1 − 2 r ^ 2 − 2 r ^ 3 − 2 r ^ 3 2 r ^ 2 2 r ^ 1 − 2 r ^ 0 2 r ^ 2 2 r ^ 3 2 r ^ 0 2 r ^ 1 ] J h ′ + ≡ ( J h ′ T J h ′ ) − 1 J h ′ T Δ k q = J h ′ + e k \begin{align}
\bm{J}_{h'} &\equiv \frac{\partial h}{\partial \hat{\bm{q}}} =
\begin{bmatrix}
-2g \hat{r}_2 & 2g \hat{r}_3 & -2g \hat{r}_0 & 2g \hat{r}_1 \\
2g \hat{r}_1 & 2g \hat{r}_0 & 2g \hat{r}_3 & 2g \hat{r}_2 \\
2g \hat{r}_0 & -2g \hat{r}_1 & -2g \hat{r}_2 & 2g \hat{r}_3 \\
2 \hat{r}_0 & 2 \hat{r}_1 & -2 \hat{r}_2 & -2 \hat{r}_3 \\
-2 \hat{r}_3 & 2 \hat{r}_2 & 2 \hat{r}_1 & -2 \hat{r}_0 \\
2 \hat{r}_2 & 2 \hat{r}_3 & 2 \hat{r}_0 & 2 \hat{r}_1 \\
\end{bmatrix}\\
\bm{J}^{+}_{h'} &\equiv \left(\bm{J}_{h'}^{\mathrm T}\bm{J}_{h'}\right)^{-1}\bm{J}_{h'}^{\mathrm T} \\
\bm{\Delta}^{q}_{k} &= \bm{J}^{+}_{h'} \bm{e}_{k}
\end{align} J h ′ J h ′ + Δ k q ≡ ∂ q ^ ∂ h = − 2 g r ^ 2 2 g r ^ 1 2 g r ^ 0 2 r ^ 0 − 2 r ^ 3 2 r ^ 2 2 g r ^ 3 2 g r ^ 0 − 2 g r ^ 1 2 r ^ 1 2 r ^ 2 2 r ^ 3 − 2 g r ^ 0 2 g r ^ 3 − 2 g r ^ 2 − 2 r ^ 2 2 r ^ 1 2 r ^ 0 2 g r ^ 1 2 g r ^ 2 2 g r ^ 3 − 2 r ^ 3 − 2 r ^ 0 2 r ^ 1 ≡ ( J h ′ T J h ′ ) − 1 J h ′ T = J h ′ + e k 続いて、
q \bm{q} q と
ω b \bm{\omega}^{\rm b} ω b の関係から、
ω b \bm{\omega}^{\rm b} ω b の修正項
Δ k ω \bm{\Delta}^{\omega}_{k} Δ k ω を次のように算出する。
Δ k ω = J ω + Δ k q J ω + ≡ ( J ω T J ω ) − 1 J ω T J ω ≡ ∂ q ∂ ω ^ k b ≒ T s J q ( q ^ k ) \begin{align}
\bm{\Delta}^{\omega}_{k} &= \bm{J}^{+}_{\omega} \bm{\Delta}^{q}_{k} \\
\bm{J}^{+}_{\omega} &\equiv \left(\bm{J}_{\omega}^{\mathrm T}\bm{J}_{\omega}\right)^{-1}\bm{J}_{\omega}^{\mathrm T} \\
\bm{J}_{\omega} &\equiv \frac{\partial \bm{q}}{\partial \hat{\bm{\omega}}^{\rm b}_{k}} \fallingdotseq T_{\rm s}\bm{J}_{q}(\hat{\bm{q}}_{k})
\end{align} Δ k ω J ω + J ω = J ω + Δ k q ≡ ( J ω T J ω ) − 1 J ω T ≡ ∂ ω ^ k b ∂ q ≒ T s J q ( q ^ k ) 以上を用いて、状態
x \bm{x} x の修正項
Δ \bm{\Delta} Δ を次のように設計する。
Δ k = [ Δ k q Δ k ω ] ∈ R 7 \begin{align}
\bm{\Delta}_{k} &=
\begin{bmatrix}
\bm{\Delta}^{q}_{k} \\ \bm{\Delta}^{\omega}_{k}
\end{bmatrix} \in \mathbb{R}^{7}
\end{align} Δ k = [ Δ k q Δ k ω ] ∈ R 7 上記の修正項に縮小因子
γ \bm{\gamma} γ を組み合わせ、予測値
x ~ k + 1 \tilde{\bm{x}}_{k+1} x ~ k + 1 を修正することで推定値
x ^ k + 1 \hat{\bm{x}}_{k+1} x ^ k + 1 とする。
x ^ k + 1 = x ~ k + 1 + γ Δ k \begin{align}
\hat{\bm{x}}_{k+1} = \tilde{\bm{x}}_{k+1} + \bm{\gamma}\bm{\Delta}_{k}
\end{align} x ^ k + 1 = x ~ k + 1 + γ Δ k 縮小因子
γ \bm{\gamma} γ はオブザーバゲインに相当し，収束速度を決定するパラメータとなる。