DigitalServo

Blog

About

Library

    Board Reference

    Flight Controller
    Field Oriented Controller

    Motor Drive

    PMSM

    Motion Control

    Attitude Control

    Tutorial

    Mathematics
    Classical Control
    State Estimation
    Feedforward Control
    Disturbance Observer
    Motion Control

Quaternion Observer

System model

In SO(3), a following governing equation holds.
q˙=Jaco(q)ω, \begin{align} \dot{\bm{q}} = \bm{J}_{\rm aco}(\bm{q})\bm{\omega}, \end{align}
where
q≡{[r0r]∈R4∣r02+rTr=1}Jaco(q)≡12[−rTr0I+S(r)]∈R4×3S:x↦{S(x)∈R3×3 ∣ S(x)y=x×y}. \begin{align} \bm{q} &\equiv \left\{ \left. \begin{bmatrix} r_0 \\ \bm{r} \end{bmatrix} \in \mathbb{R}^4 \right| r_0^2 + \bm{r}^{\mathrm T}\bm{r} = 1 \right\} \\ \bm{J}_{\rm aco}(\bm{q}) &\equiv \frac{1}{2} \begin{bmatrix} -\bm{r}^{\mathrm T} \\ r_0\bm{I} + S(\bm{r}) \end{bmatrix} \in \mathbb{R}^{4\times 3}\\ S &: \bm{x} \mapsto \left\{\left. S(\bm{x}) \in \mathbb{R}^{3\times 3}\ \right|\ S(\bm{x})\bm{y} = \bm{x}\times \bm{y}\right\}. \end{align}

Observation process

When q\bm{q} is given, a map rr that maps a vector from a reference frame to a body frame is expressed as
r:x∈R3↦Rrb(q)x∈R3,\begin{align} r: \bm{x} \in \mathbb{R}^3 \mapsto \bm{R}_{\rm rb}(\bm{q})\bm{x} \in \mathbb{R}^3, \end{align}
where
Rrb(q)≡[1−2(r22+r32)2(r1r2+r0r3)2(r1r3−r0r2)2(r1r2−r0r3)1−2(r21+r32)2(r2r3+r0r1)2(r1r3+r0r2)2(r2r3−r0r1)1−2(r11+r22)].\begin{align} \hspace{-2mm} \bm{R}_{\rm rb}(\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^1 + 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^1 + r_2^2\right) \end{bmatrix}. \end{align}
A gravitational acceleration ar\bm{a}_{\rm r} and terrestrial magnetism mr\bm{m}_{\rm r} on a reference frame are mapped to
ab=r(ar)mb=r(mr).\begin{align} \bm{a}_{\rm b} &= r(\bm{a}_{\rm r})\\ \bm{m}_{\rm b} &= r(\bm{m}_{\rm r}). \end{align}
We can get a measured value yb\bm{y}_{\rm b} of them from sensor and a predict value y^b\hat{\bm{y}}_{\rm b} from the formula above, where
yb=[abmb]∈R6.\begin{align} \bm{y}_{\rm b} = \begin{bmatrix} \bm{a}_{\rm b} \\ \bm{m}_{\rm b} \end{bmatrix} \in \mathbb{R}^6. \end{align}

Observer design

We set a system state x\bm{x} as
x=[qωb]∈R7,\begin{align} \bm{x} = \begin{bmatrix} \bm{q} \\ \bm{\omega}_{\rm b} \end{bmatrix} \in \mathbb{R}^7, \end{align}
and assume that a system behaves as
x˙=f(x)f:x∈R7↦Jaco(q)(ωs+ωb)∈R4,\begin{align} \dot{\bm{x}} &= f(\bm{x})\\ f&: \bm{x} \in \mathbb{R}^{7} \mapsto \bm{J}_{\rm aco}(\bm{q})(\bm{\omega}_{\rm s}+\bm{\omega}_{\rm b}) \in \mathbb{R}^{4}, \end{align}
where ωs\bm{\omega}_{\rm s} and ωb\bm{\omega}_{\rm b} denote a measured angular velocity and sensor bias. A system input is ωs\bm{\omega}_{\rm s}. Here, we can estimate a measurable state
y=h(x)h:x↦[r(ar)r(mr)].\begin{align} \bm{y} &= h(\bm{x})\\ h&: \bm{x} \mapsto \begin{bmatrix} r(\bm{a}_{\rm r}) \\ r(\bm{m}_{\rm r}) \end{bmatrix}. \end{align}
Now, we introduce a map h^+\hat{h}^{+} such that y\bm{y} locally maps to x\bm{x}. We designed such a map by local linearization:
h^+:y∈R6↦Xy∈R7,\begin{align} \hat{h}^{+}:\bm{y} \in \mathbb{R}^{6} \mapsto \bm{X}\bm{y} \in \mathbb{R}^{7}, \end{align}
where
X≡(hxThx)−1hxT,hx≡∂h(x)∂x=[∂h(x)∂q∂h(x)∂ωb]=[∂h(x)∂q∂h(x)∂q∂q∂ωb]=∂h(x)∂q[I4Jaco(q)].\begin{align} \bm{X} &\equiv \left(\bm{h}_{\bm{x}}^{\mathrm T}\bm{h}_{\bm{x}}\right)^{-1}\bm{h}_{\bm{x}}^{\mathrm T},\\ \bm{h}_{\bm{x}} &\equiv \frac{\partial h(\bm{x})}{\partial x}\\ &= \begin{bmatrix} \cfrac{\partial h(\bm{x})}{\partial \bm{q}} & \cfrac{\partial h(\bm{x})}{\partial \bm{\omega}_{\rm b}} \end{bmatrix}\\ &= \begin{bmatrix} \cfrac{\partial h(\bm{x})}{\partial \bm{q}} & \cfrac{\partial h(\bm{x})}{\partial \bm{q}}\cfrac{\partial \bm{q}}{\partial \bm{\omega}_{\rm b}} \end{bmatrix}\\ &= \cfrac{\partial h(\bm{x})}{\partial \bm{q}} \begin{bmatrix} \bm{I}_4 & \bm{J}_{\rm aco}(\bm{q}) \end{bmatrix}. \end{align}
This map provides an estimation error as
ex=h^+(ey)∵x−x^=X(y−y^),\begin{align} \bm{e}_{\bm{x}} &= \hat{h}^{+}(\bm{e}_{\bm{y}})\\ \because \bm{x} - \hat{\bm{x}} &= \bm{X}(\bm{y} - \hat{\bm{y}}), \end{align}
where
ex≡x−x^ey≡y−y^.\begin{align} \bm{e}_{\bm{x}} &\equiv \bm{x} - \hat{\bm{x}}\\ \bm{e}_{\bm{y}} &\equiv \bm{y} - \hat{\bm{y}}. \end{align}
Then, a state can be estimated with correcting term
x˙^=f(x^)+gex,\begin{align} \hat{\dot{\bm{x}}} = f(\hat{\bm{x}}) + \bm{g}\bm{e}_{\bm{x}}, \end{align}
where g∈R7×7\bm{g} \in \mathbb{R}^{7\times 7} stands for a observer gain. Here, we can get the error dynamics that is expressed as
e˙x=−gex+f(x)−f(x^).\begin{align} \dot{\bm{e}}_{\bm{x}} = -\bm{g}\bm{e}_{\bm{x}} + f(\bm{x}) - f(\hat{\bm{x}}). \end{align}
Using local linearization, the formula is transformed to
e˙x=−gex+fxex=(fx−g)ex,\begin{align} \dot{\bm{e}}_{\bm{x}} &= -\bm{g}\bm{e}_{\bm{x}} + \bm{f}_{\bm{x}}\bm{e}_{\bm{x}}\\ &= (\bm{f}_{\bm{x}}-\bm{g})\bm{e}_{\bm{x}}, \end{align}
where
fx≡∂f(x)∂x.\begin{align} \bm{f}_{\bm{x}} \equiv \cfrac{\partial f(\bm{x})}{\partial \bm{x}}. \end{align}
Setting g=fx+k\bm{g}=\bm{f}_{\bm{x}} + \bm{k}, the error dynamics becomes time-invariant:
e˙x=−kex.\begin{align} \dot{\bm{e}}_{\bm{x}} &= -\bm{k}\bm{e}_{\bm{x}}. \end{align}
Note that this corrector works if a linearization error is as small as it could be ignored.

CONTENTS

© DigitalServo