Notas de estudio de ESKF

Cinemática de estado de error de sistemas controlados por IMU

Introducción a los filtros de Kalman de estado de error

  En la formulación del filtro de estado de error hablamos del estado verdadero, el estado nominal y el valor del estado de error, donde el estado verdadero se representa como una combinación apropiada (suma lineal, producto de cuaterniones o producto de matriz) del estado nominal y el error estado. La idea es tratar el estado nominal como una señal grande (integrable de forma no lineal) y el estado de error como una señal pequeña (por lo tanto, integrable linealmente y susceptible de filtrado gaussiano lineal).

  El filtro de estado de error se puede explicar así. Por un lado, los datos IMU de alta frecuencia ${\text{tu}}_{}$está integrado en el estado nominal $\text{x}$ pulg. El estado nominal no considera el término de ruido$\text{w}$ y otros posibles defectos del modelo. Por lo tanto, acumulará errores. Estos errores se recopilan en el estado de error$\delta \text{x}$ y se estima mediante un filtro de Kalman de estado de error (ESKF), esta vez incluyendo todo el ruido y las perturbaciones. El estado de error consiste en un pequeño semáforo cuya función de evolución está determinada por un sistema dinámico lineal (variante en el tiempo) bien definido cuyas matrices de dinámica, control y medición se calculan a partir de los valores del estado nominal. Paralelamente a la integración del estado nominal, el ESKF predice una estimación gaussiana del estado de error. Solo hace predicciones porque no hay otras medidas disponibles para corregir estas estimaciones. Las correcciones de filtro se realizan cuando llega información más allá de la IMU (por ejemplo, GPS, visión, etc.), lo que permite observar los errores y, por lo general, es mucho más bajo que la etapa de integración. Esta corrección proporciona una estimación gaussiana a posteriori del estado de error. Posteriormente, el promedio del estado de error se inyecta en el estado nominal y luego se restablece a cero. La matriz de covarianza de los estados de error se actualiza convenientemente para reflejar este reinicio. El sistema siempre funcionará así.

Cinemática del sistema de tiempo continuo

Todas las variables en el estado de error Filtro de Kalman:

magnitud del estado de error	valor actual	valor nominal	error	combinación	Mediciones	ruido
colección de variables de estado	${\text{X}}_{}$	$\text{X}$	$\delta x\_\text{\_}$	$X_{}=X\oplus \delta x\_\_$
Ubicación	${pag}_{}$	$pag$	$pag\_\_$	${pag}_{}=pag+pag\_\_$
velocidad	$v_{}$	$v$	$v\_\_$	$v_{}=v+v\_\_$
Cuaternio	$q_{}$	$q$	$q\_\_$	$q_{}=q\otimes q\_\_$
matriz de rotación	$R_{}$	$R$	$\delta R\_$	$R_{}=R\delta R$
angulo vectorial			$yo\_$	$\begin{array}{c}q\_\_={mi}^{d\theta /2}\\ \delta R\_={mi}^{[re\theta {]}_{}}\end{array}$
sesgo del acelerómetro	$a_{bt}$	$a_{}$	$un{\text{\_}}_{}$	$a_{bt}=a_{}+un{\_}_{}$		$a_{}$
Sesgo del giroscopio	${Vaya}_{bt}$	${Vaya}_{}$	$hacer{\_}_{}$	${Vaya}_{bt}={Vaya}_{}+hacer{\_}_{}$		${Vaya}_{}$
peso vectorial	${gramo}_{}$	$gramo$	$g\_\text{\_}$	${gramo}_{}=gramo+g\_\_$
aceleración	$a_{}$				$a_{}$	$a_{}$
velocidad angular	${Vaya}_{}$				${Vaya}_{}$	${Vaya}_{}$

Cinemática de Bienes Raíces

Configuración predeterminada:
$$\begin{array}{l}{pag}_{}=v_{}\\ {\dot{v}}_{}=a_{}\\ q_{}=\frac{}{2}{q}_{}\otimes {Vaya}_{}\\ {\dot{a}}_{bt}=a_{}\\ {\dot{Vaya}}_{bt}={Vaya}_{}\\ {gramo}_{}=\end{array}$$
  Lecturas del sensor de aceleración en el sistema de coordenadas del cuerpo $a_{}$y la lectura del sensor de velocidad angular $w_{}$Se utiliza para obtener la aceleración real en \mathbf{a}_{t} del robot en el momento actual$a_{}$y la velocidad angular $w_{}$. Debido al ruido del propio sensor, $a_{}$y $w_{}$Defina la ecuación
$$\begin{array}{l}{a}_{}=R_t^{}({un}_{}-{gramo}_{})+a_{bt}+a_{}\\ {Vaya}_{}={Vaya}_{}+{Vaya}_{bt}+{Vaya}_{}\end{array}$$
Interpretación $R_{}\triangleq R\left\{q_{}\right\}$ definición
$$\begin{array}{l}{a}_{}=R_{}({un}_{}-a_{bt}-a_{})+{gramo}_{}\\ {Vaya}_{}={Vaya}_{}-{Vaya}_{bt}-{Vaya}_{}\end{array}$$
Entonces
$$\begin{array}{l}{\text{pag}}_{}={\text{v}}_{}\\ {\text{v}}_{}={\text{R}}_{}({\text{un}}_{}-{\text{a}}_{bt}-{\text{a}}_{})+{\text{gramo}}_{}\\ {\text{q}}_{}=\frac{}{2}{\text{q}}_{}\otimes ({ay}_{}-{Vaya}_{bt}-{Vaya}_{})\\ {\dot{\text{a}}}_{bt}={\text{a}}_{}\\ {\dot{Vaya}}_{}={Vaya}_{}\\ {\text{gramo}}_{}=\end{array}$$
设${\dot{X}}_{}=F_{}(X_{},tu,w)$ , el sistema se ve afectado por la lectura de ruido de la IMU${tu}_{}$Controlado por ruido gaussiano blanco w \textbf{w}función$\text{w}$
$$X_{}=\left[\begin{array}{c}{pag}_{}\\ v_{}\\ q_{}\\ a_{}\\ {gramo}_{}\end{array}\right]tu=\left[\begin{array}{c}{a}_{}-a_{}\\ {Vaya}_{}-{Vaya}_{}\end{array}\right]v=[\begin{array}{c}{a}_{}\\ {Vaya}_{}\end{array}$$
]

  Vector de gravedad ${gramo}_{}$estimado por el filtro. El sistema parte de una orientación inicial fija y arbitrariamente conocida, es decir, $q_{}(t=0)=q_{}$, dado que la dirección inicial generalmente no está en el plano horizontal, el vector de gravedad inicial generalmente se desconoce y la dirección inicial generalmente se establece en $q_{}=(1,0,0,0)$ Por lo tanto,$R_{}=R\left\{q_{}\right\}=yo$ _ También es posible eliminar todas las ecuaciones relacionadas con la gravedad del sistema y usar un enfoque más tradicional, que asume que el vector de gravedad es$gramo\stackrel{D}{=}\left(0,0,-9.8xx\right)$ , donde$xx$ es el número apropiado de lugares decimales para el vector de gravedad en el sitio experimental, asumiendo la dirección inicial$q_{}$es incierto.

cinemática de estado nominal

  p ˙ = vv ˙ = R ( am − ab ) +
$$\begin{array}{l}pag=v\\ \dot{v}=R({un}_{}-a_{})+gramo\\ q=\frac{}{2}a\otimes ({ay}_{}-{Vaya}_{})\\ {\dot{a}}_{}=0\\ {\dot{Vaya}}_{}=0\\ gramo=\end{array}$$

cinemática de estado de error

δ ̇ pags = δ v δ ̇ v = − R [ am − ab ] × δ θ − R δ ab + δ gramo − R an δ ̇ θ = − [ ω metro   − { \textbf{a}}_w\\ &\dot{\delta}{ {\ omega}}_b={\omega}_w\\ &{ { \delta}}{\dot {\textbf{g}}}= 0\end{alineado}
$$\begin{array}{rl}& \stackrel{pag}{d}\text{\_}=v\_\text{\_}\\ & \stackrel{\dot{}v}{d}\text{\_}=-\text{R}{\left[{\text{un}}_{}-{\text{a}}_{}\right]}_{}yo\_-\text{R}\delta {\text{a}}_{}+g\_\text{\_}-\text{Ra}{\text{\_}}_{}\\ & \dot{d}\theta =-{\left[{ay}_{}-{Vaya}_{}\right]}_{}yo\_-hacer{\_}_{}-{Vaya}_{}\\ & \dot{d}{\text{un}}_{}={\text{a}}_{}\\ & \dot{d}{\omega }_{}={Vaya}_{}\\ & d\text{gramo}=\end{array}$$
Entre ellos, las ecuaciones de los errores de velocidad y orientación requieren cierta manipulación de las ecuaciones de error de los vectores de velocidad angular y gravedad para obtener dinámicas linealizadas.

Cinemática del sistema en tiempo discreto

  La ecuación diferencial anterior debe integrarse en una ecuación en diferencias para considerar el intervalo de tiempo discreto ∆t>0, y el método de integración puede ser diferente. En algunos casos, se encuentran disponibles soluciones exactas de forma cerrada. En otros casos, se pueden utilizar métodos de integración numérica de diferente precisión. Ver los detalles relevantes sobre el método de integración en el apéndice.

Es necesario integrar los siguientes subsistemas:

1. estado nominal.
2. estado de error
   (a) Parte determinista: dinámica de estado y control.
   (b) Parte estocástica: ruido y perturbación.

  Para estados nominales y de error, es necesario integrar las partes determinística y estocástica. La parte determinista incluye dinámica de estado y control, y la parte estocástica incluye ruido y perturbaciones. Para diferentes subsistemas y métodos de integración, se requiere una selección y ajuste adecuados para obtener resultados precisos.

Cinemática del Estado Nominal

$$\begin{array}{l}\text{pag}\leftarrow \text{pag}+\text{v}\Delta t+\frac{}{2}\left(\text{R}\right({\text{un}}_{}-{\text{a}}_{})+\text{g})\Delta t^2\\ \text{v}\leftarrow \text{v}+\left(\text{R}\right({\text{un}}_{}-{\text{a}}_{})+\text{g})\Delta t\\ \text{q}\leftarrow \text{q}\otimes \text{q}\{(o_{}-{Vaya}_{}\left)\Delta t\right\}\\ {\text{a}}_{}\leftarrow {\text{a}}_{}\\ \text{gramo}\leftarrow \end{array}$$

donde $X\leftarrow f(x,\bullet )$ significa$X_{k+}=f(x_{},{\bullet }_{})$ escriba actualización de tiempo,$R\triangleq R\left\{q\right\}$ es la dirección nominal actual$matriz\; de\; rotación\; relacionada\; con\; q$ ,$q\left\{v\right\}$ es la rotación vvconEl cuaternión asociado con$v\; .$

Cinemática de los estados de error

$$\begin{array}{l}pag\_\text{\_}\leftarrow pag\_\text{\_}+\delta \text{v}\Delta t\\ v\_\text{\_}\leftarrow v\_\text{\_}+(-\text{R}\left[{\text{un}}_{}-{\text{a}}_{}\right]\times yo\_-\text{R}\delta {\text{a}}_{}+\delta \text{g})\Delta t+{\text{v}}_{}\\ yo\_\leftarrow {\text{R}}^{\top }\left\{({ay}_{}-{Vaya}_{})\Delta t\right\}yo\_-hacer{\_}_{}\Delta t\_+i_{}\\ un{\text{\_}}_{}\leftarrow un{\text{\_}}_{}+{\text{a}}_{}\\ hacer{\_}_{}\leftarrow hacer{\_}_{}+{Vaya}_{}\\ g\_\text{\_}\leftarrow g\_\end{array}$$

Entre ellos, ${\text{v}}_{}$、$i_{}$、${\text{a}}_{}$suma ${Vaya}_{}$son impulsos aleatorios que actúan sobre estimaciones de velocidad, dirección y sesgo, modelados mediante un proceso gaussiano blanco. Su media es cero, y la matriz de covarianza se obtiene poniendo $a_{}$, ${Vaya}_{}$、$a_{}$suma ${Vaya}_{}$La integral de covarianza del paso de tiempo $\Delta t$ es infinitesimal
$$\begin{array}{rlrl}{\text{V}}_{}& ={pag}_{{\tilde{a}}_{}}^{}\Delta t{\_}^2\text{yo}& & [{metro}^2/s^2]\\ {el}_{}& ={pag}_{{\widetilde{Vaya}}_{}}^{}\Delta t{\_}^2\text{yo}& & \left[run{d}^2\right]\\ {\text{A}}_{}& ={pag}_{a_{}}^{}\Delta t\text{yo}\_& & [{metro}^2/s^4]\\ {Oh}_{}& ={pag}_{{Vaya}_{}}^{}\Delta t& & [run{d}^2/s^2\end{array}$$

其中, ${pag}_{{\tilde{a}}_{}}[{metro}^2/s^2]$,${pag}_{{\widetilde{Vaya}}_{}}[rad/s]$ ,${pag}_{a_{}}\left[m/s^2\sqrt{s}\right]$和${pag}_{{Vaya}_{}}\left[rad/s\sqrt{s}\right]$ se determinará a partir de la información de la hoja de datos de la IMU o de mediciones experimentales.

Matriz jacobiana y matriz de perturbación del estado de error

  El jacobiano se puede obtener mediante una simple derivación de la ecuación de diferencia de estado de error de la sección anterior. Para escribir estas ecuaciones en forma compacta, considere el vector de estado nominal $x$ , vector de estado de error$\delta x$ , vector de entrada${tu}_{}$Y el vector de impulso de perturbación $yo$ ,dado
$$X=\left[\begin{array}{c}pag\\ v\\ q\\ a_{}\\ {Vaya}_{}\\ \end{array}\right],\delta x\_\_=\left[\begin{array}{c}pag\_\_\\ v\_\_\\ yo\_\\ un{\_}_{}\\ hacer{\_}_{}\\ g\_\end{array}\right],{tu}_{}=\left[\begin{array}{c}{a}_{}\\ {Vaya}_{}\end{array}\right],i=\left[\begin{array}{c}{v}_{}\\ i_{}\\ a_{}\\ {Vaya}_{}\end{array}\right]$$
Define la función inversa:
$$\delta x\_\text{\_}\leftarrow f(\text{x},\delta \text{x},{\text{tu}}_{},\text{yo})={\text{F}}_{}(\text{X},{\text{tu}}_{})\cdot \delta x\_\text{\_}+{\text{F}}_{}\cdot \text{i}$$
Funciones ESKF:
$$\begin{array}{l}\widehat{\delta x\_\_}\leftarrow F_{}(X,{tu}_{})\cdot \widehat{\delta x\_\_}\\ PAG\leftarrow F_{}P{F}_X^{}+F_{}{q}_{}{F}_i^{}\end{array}$$
Por ejemplo, $\delta x\_\_\sim norte\{\widehat{\delta x\_\_},P\}$；$F_{}$和$F_{}$es $f\left(\right)$ con respecto a la matriz jacobiana de los vectores de error y perturbación,$q_{}$es la matriz de covarianza del pulso de perturbación.

  A continuación se detallan las expresiones de la matriz jacobiana y la matriz de covarianza mencionadas anteriormente. Todos los valores relacionados con el estado que aparecen aquí se extraen directamente del estado nominal.
$${\text{F}}_{}=\frac{\partial }{\partial \delta \text{x}}{|}_{\text{x},{\text{tu}}_{}}=\left[\begin{array}{cccccc}\text{I}& \text{Yo}\Delta t& 0& 0& 0& 0\\ 0& \text{I}& -\text{R}[{\text{un}}_{}-{\text{a}}_{}{]}_{}\Delta t\_& -\text{R}\Delta t& 0& \text{Yo}\Delta t\\ 0& 0& {\text{R}}^{\top }\{(o_{}-{Vaya}_{}\left)\Delta t\right\}& 0& -\text{yo}\Delta t& 0\\ 0& 0& 0& \text{I}& 0& 0\\ 0& 0& 0& 0& \text{I}& 0\\ & & & & & \end{array}\right]$$

$${\text{F}}_{}=\frac{\partial }{\partial \text{yo}}{|}_{\text{x},{\text{tu}}_{}}=\left[\begin{array}{cccc}0& 0& 0& 0\\ \text{I}& 0& 0& 0\\ 0& \text{I}& 0& 0\\ 0& 0& \text{I}& 0\\ 0& 0& 0& \text{I}\\ & & & \end{array}\right],{\text{q}}_{}=\left[\begin{array}{cccc}{\text{V}}_{}& 0& 0& 0\\ 0& {el}_{}& 0& 0\\ 0& 0& {\text{A}}_{}& 0\\ & & & {Oh}_{}\end{array}\right]$$

Fusión de IMU con datos de sensores complementarios

  ESKF se corrige cuando hay más variedad de información que IMU, como GPS o información visual. En un sistema bien diseñado, esto debería hacer observables los sesgos de la IMU y permitir que el ESKF los estime correctamente. Hay innumerables posibilidades, siendo las más populares GPS+IMU, Monocular Vision+IMU y Stereo Vision+IMU.

  Si bien la información de la IMU se ha utilizado hasta ahora para hacer predicciones al ESKF, esta información adicional se utiliza para corregir el filtro, observando así el error de sesgo de la IMU. La corrección implica tres pasos:

1. Los estados de error de observación se corrigen mediante filtros,
2. Inyecte el error observado en el estado nominal, y
3. Restablecer estado de error.

Estado de error de observación corregido por filtrado

Supongamos, como de costumbre, que la información proporcionada por el sensor depende del estado, por ejemplo:
$$y=h\left(x_{}\right)+v$$
donde$h\left(\right)$ es una función general no lineal del estado del sistema (estado verdadero),$v$ es una covarianzaV \textbf{V}Ruido gaussiano blanco de$\text{V}$
$$v\sim norte\{0,V\}$$
  El filtro está estimando el estado incorrecto, por lo que la ecuación de corrección del filtro es:
$$\begin{array}{rl}\text{k}& ={\text{PH}}^{\top }(\text{HPH}{\text{\_}}^{\top }+\text{V}{)}^{-1}\\ & \widehat{\delta x\_\text{\_}}\leftarrow \text{K}(\text{y}-h({\hat{\text{X}}}_{}))\\ & \text{PAG}\leftarrow (\text{yo}-\text{K}\text{H})\end{array}$$
Requiere la matriz jacobiana $\text{H}$ relativa al estado de error$\delta \text{x}$ está definido, y en la mejor estimación del estado verdadero${\hat{X}}_{}=X\oplus \widehat{\delta x\_\_}$ para evaluación. Dado que la media del estado de error es cero en esta etapa (no se ha observado), entonces${\hat{X}}_{}=x$ , y se puede usar el error nominal$\text{x}$ como el punto de evaluación, lo que resulta en lo siguiente:
$$H\equiv {\frac{\partial }{\partial \delta x}|}_{}$$

Matriz jacobiana para cálculo de corrección de filtros

La matriz jacobiana descrita anteriormente se puede calcular de varias maneras. El enfoque más ilustrativo es usar la regla de la cadena,
$$\text{H}\triangleq {\frac{\partial }{\partial \delta \text{x}}|}_{}={\frac{\partial }{\partial {\text{x}}_{}}|}_{}{\frac{\partial {\text{x}}_{}}{\partial \delta \text{x}}|}_{}={\text{H}}_{}{\text{X}}_x$$
其中，$\text{H}\triangleq {\frac{\partial }{\partial \delta \text{x}}|}_{}$es $h\left(\right)$ con respecto al coeficiente de Jacobs estándar de su propio parámetro (es decir, el coeficiente de Jacobs utilizado en EKF convencional). La primera parte de la regla de la cadena depende de las capacidades de medición del sensor en particular que se utilice.

  第二部分，${\text{X}}_x\triangleq {\frac{\partial {\text{x}}_{}}{\partial \delta \text{x}}|}_{}$, es la matriz jacobiana del estado verdadero con respecto al estado de error. Esta parte se puede derivar aquí ya que solo depende de la composición ESKF del estado. tiene una derivada,
$$X_x=\left[\begin{array}{ccccccc}\frac{\partial (pags+\delta pags}{\partial \delta pag}& & & & & & \\ & \frac{\partial (v+\delta v}{\partial \delta v\_}& & & & 0& \\ & & \frac{\partial (q\otimes \delta q}{\partial \delta \theta }& & & & \\ & & & & \frac{\partial ({un}_{}+un{\_}_{}}{\partial \delta {un}_{}}& & \\ & 0& & & & \frac{\partial (o_{}+hacer{\_}_{}}{\partial \delta {\omega }_{}}& \\ & & & & & & \frac{\partial (gramo+\delta gramo}{\partial \delta gramo}\end{array}\right]$$
Esto da como resultado $4\times 3Ecuación$ Q δ θ = ∂ ( q ⊗ δ q ) / ∂ δ θ \textbf{Q}_{\delta\ballsymbol{\theta}}=\parcial(\textbf{q}\otimes\delta \${\text{q}}_{yo}=\partial (\text{q}\otimes \delta \text{q})/\partial \delta \theta$ Todas las identidades excepto$3\times 3Ecuación$ ,$\frac{\partial (\text{pags}+\delta \text{pags}}{\partial \delta \text{pag}}={\text{I}}_{}$) En otras palabras, infinitesimal,
$${\text{X}}_x\triangleq {\frac{\partial {\text{x}}_{}}{\partial \delta \text{x}}|}_{}=\left[\begin{array}{ccc}{\text{I}}_{}& 0& 0\\ 0& {\text{q}}_{yo}& 0\\ & & {\text{I}}_{}\end{array}\right]$$
Término de cuaternión ${\text{q}}_{yo}$Forma:
$${\text{q}}_{yo}=\frac{}{2}\left[\begin{array}{ccc}-q_{}& -q_{}& -q_{}\\ q_{}& -q_{}& q_{}\\ q_{}& q_{}& -q_{}\\ -q_{}& q_{}& q_{}\end{array}\right]$$

Inyectar el error de observación en el estado nominal

Después de la actualización de ESKF, el estado de error observado se actualiza con el estado nominal usando la combinación adecuada (suma o producto de cuaterniones).
$$\text{X}\leftarrow \text{X}\oplus \widehat{\delta x\_\text{\_}}$$
Por ejemplo,
$$\begin{array}{l}pag\leftarrow pag+pag\_\_\\ v\leftarrow v+\widehat{v\_\_}\\ q\leftarrow q\otimes q\left\{\widehat{yo\_}\right\}\\ a_{}\leftarrow a_{}+un{\_}_{}\\ {Vaya}_{}\leftarrow {Vaya}_{}+d{\widehat{Vaya}}_{}\\ gramo\leftarrow gramo+g\_\_\end{array}$$

Restablecer ESKF

Después de inyectar el error en el estado nominal, la media del estado del error es $\widehat{\delta x\_\text{\_}}$ se restablecerá. Esto es especialmente importante en la parte de orientación, ya que el nuevo error de orientación se representará localmente en relación con el marco de orientación del nuevo estado nominal. Para que la actualización de ESKF esté completa, la covarianza de error debe actualizarse de acuerdo con esta modificación.

  Llame a la función de reinicio de error $g\left(\right)$ Determine:
$$\delta x\_\_\leftarrow g\left(\delta x\right)=\delta x\_\_\ominus \widehat{\delta x\_\_}$$
donde,$\ominus$ significa⊕ \oplusEl inverso de la combinación de$\oplus \; .$Por lo tanto, la operación de reinicio de error de ESKF es:
$$\begin{array}{l}\widehat{\delta x\_\text{\_}}\leftarrow 0\\ \text{PAG}\leftarrow \text{GPG}{\text{\_}}^{}\end{array}$$
Donde G es la matriz jacobiana, definida como sigue:
$$\text{GRAMO}\triangleq \frac{\partial }{\partial \delta \text{x}}{|}_{\delta x\_\text{\_}}$$
  Similar a lo que sucede arriba para actualizar el jacobiano, este jacobiano es el mismo en todos los bloques diagonales excepto por el error de orientación. La expresión completa se da aquí, $\partial \delta {\theta }^{+}/\partial \delta \theta =I-{\left[\frac{}{2}\widehat{yo\_}\}\right]}_{}$,
$$\text{GRAMO}=\left[\begin{array}{ccc}{\text{I}}_{}& 0& 0\\ 0& \text{I}-{\left[\begin{array}{c}\frac{}{2}\widehat{yo\_}\end{array}\right]}_{}& 0\\ & & {\text{I}}_{}\end{array}\right]$$
  En la mayoría de los casos, el término de error ˇδθ puede ignorarse y solo una matriz jacobiana $\text{GRAMO}={\text{I}}_{}$, lo que resulta en un ligero reinicio de error.

ESKF usando error global

  Esta sección explora las diferencias entre definir el error angular en un marco de referencia global y las definiciones locales utilizadas hasta ahora. Definir el error angular δθ en el marco de referencia global significa combinar a la izquierda, es decir:
$$q_{}=q\_\_\otimes q=q\left\{\delta \theta \right\}\otimes q\; Aquí$$
  , independientemente de si el error angular se define global o localmente, el vector de velocidad angular$La\; definición\; local\; de\; \omega$ , es decir, en tiempo continuo,$q=\frac{}{2}q\otimes \omega$ , entonces en tiempo discreto,$\text{q}\leftarrow \text{q}\otimes \text{q}\left\{\omega \Delta t\right\}$ . Esto es por comodidad, ya que las medidas de velocidad angular proporcionadas por el giroscopio se refieren al sistema de coordenadas del cuerpo, que es el sistema de coordenadas local.

Cinemática del sistema en tiempo continuo

Cinemática de estado real y estado nominal

La cinemática verdadera y la cinemática nominal no contienen errores y sus ecuaciones no cambian.

cinemática de estado de error

Defina la función de infinitivo
$$\begin{array}{rl}& pag\_\text{\_}=v\_\text{\_}\\ & \dot{v\_\text{\_}}=-{\left[\text{R}({\text{un}}_{}-b{\text{un}}_{})\right]}_{}yo\_-\text{R}\delta {\text{a}}_{}+g\_\text{\_}-{\text{Real academia de bellas artes}}_{}\\ & \dot{yo\_}=-\text{R}re{\omega }_{}-\text{R}{\omega }_{}\\ & d{\dot{\text{a}}}_{}={\text{a}}_{}\\ & d{\dot{Vaya}}_{}={Vaya}_{}\\ & g\_\text{\_}=\end{array}$$

Cinemática del sistema en tiempo discreto

Estado nominal

La ecuación de estado nominal no implica errores y por lo tanto es la misma que la ecuación en el caso de errores angulares definidos localmente.

estado de error

$$\begin{array}{l}pag\_\text{\_}\leftarrow pag\_\text{\_}+\delta \text{v}\Delta t\\ v\_\text{\_}\leftarrow v\_\text{\_}+(-{\left[\text{R}({\text{un}}_{}-{\text{a}}_{})\right]}_{}yo\_-\text{R}\delta {\text{a}}_{}+\delta \text{g})\Delta t+{\text{v}}_{}\\ yo\_\leftarrow yo\_-\text{R}do{\_}_{}\Delta t\_+i_{}\\ un{\text{\_}}_{}\leftarrow un{\text{\_}}_{}+{\text{a}}_{}\\ hacer{\_}_{}\leftarrow hacer{\_}_{}+{Vaya}_{}\\ g\_\text{\_}\leftarrow g\_\end{array}$$

Estado de error jacobiano y matriz de perturbación

Determinar la ecuación, que es la siguiente:
$$F_{}=\left[\begin{array}{cccccc}I& Yo\Delta t& 0& 0& 0& 0\\ 0& I& -\left[R\right({un}_{}-a_{}){]}_{}\Delta t\_& -R\Delta t& 0& Yo\Delta t\\ 0& 0& I& 0& -R\Delta t& 0\\ 0& 0& 0& I& 0& 0\\ 0& 0& 0& 0& I& 0\\ & & & & & \end{array}\right]$$
F yo = [ 0 0 0 0 yo 0 0 0 0 yo 0 0 0 0
$${\text{F}}_{}=\left[\begin{array}{cccc}0& 0& 0& 0\\ \text{I}& 0& 0& 0\\ 0& \text{I}& 0& 0\\ 0& 0& \text{I}& 0\\ 0& 0& 0& \text{I}\\ & & & \end{array}\right],{\text{q}}_{}=\left[\begin{array}{cccc}{\text{V}}_{}& 0& 0& 0\\ 0& {el}_{}& 0& 0\\ 0& 0& {\text{A}}_{}& 0\\ & & & {Oh}_{}\end{array}\right]$$

Fusión con datos de sensores complementarios

La ecuación de fusión que involucra el mecanismo ESKF cambia solo ligeramente cuando se considera el error de ángulo global. Estos cambios se revisarán mediante la observación del estado de error, la inyección de errores en el estado nominal y los pasos de reinicio en la corrección de ESKF.

Observación de estado de error

En comparación con la definición de error local, la única diferencia es el bloque jacobiano de la función de observación que relaciona la orientación con el error angular.
$$q_{yo}\triangleq \frac{}{2}\left[\begin{array}{ccc}-q_{}& -q_{}& -q_{}\\ q_{}& q_{}& -q_{}\\ -q_{}& q_{}& q_{}\\ q_{}& -q_{}& q_{}\end{array}\right]$$

Inyectar el error de observación en el estado nominal

$$\begin{array}{l}\text{pag}\leftarrow \text{pag}+pag\_\text{\_}\\ \text{v}\leftarrow \text{v}+v\_\text{\_}\\ \text{q}\leftarrow \text{q}\left\{\widehat{yo\_}\right\}\otimes \text{q}\\ {\text{a}}_{}\leftarrow {\text{a}}_{}+un{\text{\_}}_{}\\ {Vaya}_{}\leftarrow {Vaya}_{}+hacer{\_}_{}\\ \text{gramo}\leftarrow \text{gramo}+g\_\end{array}$$

Restablecer ESKF

De acuerdo con la siguiente fórmula, la media del error ESKF se restablece y la covarianza se actualiza:
$$\begin{array}{l}\widehat{\delta x\_\text{\_}}\leftarrow 0\\ \text{PAG}\leftarrow \text{GPG}{\text{\_}}^{}\end{array}$$
Matriz equivalente:
$$\text{GRAMO}=\left[\begin{array}{ccc}{\text{I}}_{}& 0& 0\\ 0& \text{I}+{\left[\begin{array}{c}\frac{}{2}\widehat{yo\_}\end{array}\right]}_{}& 0\\ & & {\text{I}}_{}\end{array}\right]$$