Puedes intentar cancelar el filtro. Esto puede eliminar el retraso, pero también aumenta el ruido de alta frecuencia. Después de hacer esto, puede intentar controlar el robot en función de la nueva estimación de rumbo. Para hacer esto, debe experimentar para resolver los parámetros del filtro de paso bajo. Por ejemplo, en un tiempo discreto, puede encontrar:
donde θ ( t ) es el título estimado (salida de brújula) en el tiempo t , θ es el título real (verdad básica) en el tiempo t .
θ^( t ) = a0 0θ ( t ) + a1θ ( t - 1 ) + ⋯ + akθ ( t - k )
θ^( t )tθt
Puede encontrar los parámetros haciendo un experimento en el que mide la verdad básica utilizando algún otro medio externo. Dadas n + k + 1 muestras, tiene esta ecuación:
unyon + k + 1
⎡⎣⎢⎢⎢θ^( k )⋮θ^( k + n )⎤⎦⎥⎥⎥= ⎡⎣⎢⎢θ ( k )⋮θ ( k + n )θ ( k - 1 )⋮θ ( k + n - 1 )⋯⋯θ ( 0 )⋮θ ( n )⎤⎦⎥⎥⎡⎣⎢⎢⎢⎢un0 0un1⋮unk⎤⎦⎥⎥⎥⎥
Y puede resolverlo encontrando:
, dondeM+es la matriz pseudo-inversa deM. No hay una forma definitiva de hacer ejerciciok
⎡⎣⎢⎢⎢⎢un0 0un1⋮unk⎤⎦⎥⎥⎥⎥= ⎡⎣⎢⎢θ ( k )⋮θ ( k + n )θ ( k - 1 )⋮θ ( k + n - 1 )⋯⋯θ ( 0 )⋮θ ( n )⎤⎦⎥⎥+⎡⎣⎢⎢⎢θ^( k )⋮θ^( k + n )⎤⎦⎥⎥⎥
METRO+METROk, así que probablemente solo adivinarás. Para los puntos de bonificación, esto supone que el ruido es blanco e independiente, pero puede blanquearlo primero para eliminar el sesgo y, por lo tanto, mejorar su estimación de los parámetros.
Puede convertir esto en una función de transferencia (también conocida como transformación Z en el dominio de tiempo discreto):
Θ^( z)Θ ( z)= a0 0+ a1z- 1+ . . . + akz- k
θ¯( t )
Θ¯( z)Θ^( z)= 1un0 0+ a1z- 1+ ⋯ + akz- k
Convertir de nuevo al dominio del tiempo:
un0 0θ¯( t ) + a1θ¯( t - 1 ) + ⋯ + akθ¯( t - k ) = θ^( t )
θ¯( t ) = θ^( t ) - a1θ¯( t - 1 ) - ⋯ - akθ¯( t - k )un0 0
θ¯
θ¯
La solución anterior todavía no es la mejor manera. La estimación ruidosa puede no ser muy útil. Si ponemos esto en una ecuación de espacio de estado, podemos diseñar un filtro de Kalman y un controlador de retroalimentación de estado completo usando LQR (regulador cuadrático lineal). La combinación de un filtro Kalman y un controlador LQR también se conoce como un controlador LQG (gaussiano cuadrático lineal), y utiliza la recuperación de transferencia de bucle para obtener un buen controlador.
Para hacer esto, invente las ecuaciones de espacio de estado (tiempo discreto):
X⃗ ( t ) = A x⃗ ( t - 1 ) + B u⃗ ( t - 1 )y⃗ ( t ) = CX⃗ ( t )
X⃗ ( t ) = ⎡⎣⎢⎢⎢⎢θ ( t )θ ( t - 1 )⋯θ ( t - k )⎤⎦⎥⎥⎥⎥= ⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢UN110 0⋮0 00 0UN20 01⋮0 00 0⋯⋯⋯⋯⋯0 00 00 0⋮10 00 00 00 0⋮0 010 00 00 0⋮0 00 0⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥X⃗ ( t - 1 ) + ⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢si0 0si10 0⋮0 00 0⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥tu⃗ ( t - 1 )
y⃗ ( t ) = [ θ^( t )] = ⎡⎣⎢⎢⎢⎢un0 0un1⋮unk⎤⎦⎥⎥⎥⎥X⃗ ( t )
dónde tu⃗ ( t - 1 )UN0 0UN1si0 0si1siUN
QoRo
QCRCQC= ⎡⎣⎢⎢⎢⎢10 0⋮0 00 00 0⋮0 00 00 0⋮0 0⋯⋯⋯0 00 0⋮0 0⎤⎦⎥⎥⎥⎥RC= [ 1 ]J= ∑ ( x⃗ TQ x⃗ + u⃗ TR u⃗ )
Luego, simplemente lo pones a través de la ecuación algebraica Riccati de tiempo discreto:
P=Q+AT(P−PB(R+BTPB)−1BTP)A
P
Por lo tanto, su ley de control puede ser dada por:
u⃗ (t)=−K(x⃗ (t)−x⃗ ref(t))
K=(R+BTPB)−1(BTPA)
θ¯
Qo=Q0+q2BVBTQ0QVV=Iqq→∞Qo la matriz se desintoniza, lo que significa que se vuelve menos óptima.
qRcQc
Un giroscopio es la respuesta simple. Siempre he escuchado, giroscopio para las mediciones cortas, brújula para las largas. Y de manera realista, una taza de filtro de Kallman entre los dos la mayor parte del tiempo. El precio de una placa de giroscopio / acc 6DOF es menos de $ 20 en estos días, demasiado barato para no usarlo.
En un momento, trabajé a través del filtro Kallman de otra persona . y lo puse a trabajar. Un filtro de Kallman es en realidad más un enfoque, no una implementación exacta, y en el caso del giroscopio, el resultado final no necesita usar matemáticas de matriz. Hace que el código sea mucho más simple.
fuente