¿Cómo estimar la posición de ángulos ruidosos?

Question

Primero de todo: yo creo que este problema ha sido estudiado muy bien (supongo que en el campo de la topografía o de navegación), pero yo era incapaz de encontrar cualquier información que yo no sé cómo se llama.

EDIT: Como @Rahul mencionado esto se llama resección problema. Wikipedia señala a varios métodos, pero en general requieren exacta de las mediciones y asumir que usted ha $n=3$.

---

Supongamos que sabemos la posición exacta de un número $n \geq 3$ puntos (en el ejemplo de abajo $n=4$) $P_i$ en el plano. (Podemos suponer que esos puntos son en algunos no degenerados de configuración.) Además, estamos en algún lugar desconocido $P_0$ buscando en una dirección desconocida $\hat x_1$. Ahora podemos observar los ángulos $\alpha_i$ de lo mucho que la dirección en la que nos vemos cada punto se desvía de nuestra dirección $\hat x_1$.

Con estas observaciones $\alpha_1,\ldots,\alpha_n$ le gustaría determinar el $P_0$ (así como la dirección $\hat x_1$, pero eso es trivial una vez que conocemos $P_0$). El problema es que los ángulos $\alpha_i$ son afectadas por el ruido, es decir, no puede ser medido exactamente. (Podemos suponer que tienen un por ejemplo, normalmente se distribuyen de error, o lo que es conveniente. )

Es allí una manera de "firmeza" estimación $P_0$?

¿Hay un nombre conocido para este problema?

Answer 1

Cada una de las $\ i=1,2,...,n\ $ cojinete de mediciones puede ser expresado como $$ \alpha_i = \ángulo\delta_i - \alpha_0 $$ donde $\alpha_0$ es el ángulo desconocido de $x_1$ $\hat{x}_1$y $$ \delta_i := P_i-P_0\\ \ángulo\delta_i = \bronceado^{-1}({\delta_i}_2\ /\ {\delta_i}_1) $$

Si definimos, $$ q := \begin{bmatrix} P_0 \\ \alpha_0 \end{bmatrix}\ \ \ \ \ \ z := \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \vdots \\ \alpha_n \end{bmatrix} $$ y asumir nuestra sensor de rodamiento está sujeto a ruido aditivo, entonces podemos escribir, $$ z = h(q) + \epsilon $$ como nuestro modelo de medición, donde $\epsilon$ es una variable aleatoria con algunas características conocidas, quizás como cero la media y la covarianza $R$.

También sería bueno / común tener algo de "antes" probabilístico de la información acerca de nuestro estado $q$. Es decir, incluso si no lo sabemos exactamente, tenemos una intuición acerca de lo que podría ser descrita por una distribución de probabilidad $q \sim \rho(q)$.

Por desgracia, no veo una manera de reformular $h(q)$ a ser lineal en $q$, así que no sólo puede proporcionar usted con la forma cerrada de la solución óptima: es decir, el $q$ que maximiza la probabilidad condicional de la distribución de $\rho(q|z)$. Como lo que yo puedo decir, sólo podemos aproximar esta solución con el uso de los enfoques generales no lineal de la teoría de la estimación.

He implementado un filtro extendido de Kalman como un primer intento. Esto implica una linealización de $h(q)$, así que calcula su Jacobiano por la mano, $$ \frac{dh_i}{dq} = \begin{bmatrix} \frac{{\delta_i}_2}{||\delta_i||^2_2} & \frac{{-\delta_i}_1}{||\delta_i||^2_2} & -1\end{bmatrix} $$

Otra cosa a tener en cuenta es que, dado que estamos tratando con ángulos, nuestras mediciones (y parte del estado) son realmente miembros de $\mathbb{SO}2$$\mathbb{R}$. Por lo tanto se debe tener cuidado al trabajar en el álgebra de la Mentira, que para esto simplemente significa "desenvolver" cualquier ángulo diferencias en $[-\pi, \pi]$.

Para realizar plenamente su problema, he asumido que el aditivo sensor de rodamiento de ruido es Gaussiano con cero de la media y una desviación estándar de $5$ grados. Puedo colocar arbitrariamente $7$ puntos de referencia. La animación de abajo muestra el EKF convergentes. Voy a empezar con un estado incorrecto de adivinar y una gran covarianza para reflejar mi antes de incertidumbre. También se puede ver que el cojinete de mediciones (amarillo rayos) no llegan a alcanzar sus respectivos puntos de referencia (puntos verdes). Obviamente el "modelo de proceso" en este caso es estacionaria (la verdadera situación en la que no se está moviendo). Yo alinear acerca de la primera conjetura, calcular el mejor estimador lineal (es decir, EKF corrector de paso), luego relinearize acerca de la actualización de la media y "reset" mi covarianza de vuelta a la previa antes de intentar otro linealización y actualización. Para ser claros, sólo $7$ total de mediciones fueron tomadas (uno para cada punto de referencia), y todos los $7$ son incorporados al mismo tiempo - el "iteraciones" son sólo relinearizations. El código está aquí. Si usted tiene alguna pregunta no dude en preguntar.

Aquí es uno más de la visualización de este tiempo dando el sensor de rodamiento ridículo ruido - una desviación estándar de $30$ grados (puedes ver cómo se forma el amarillo rayos). El filtro hace un buen trabajo, con su posterior covarianza razonablemente que refleja la dirección a lo largo de los cuales todavía tenemos la incertidumbre. Tenga en cuenta que la rúbrica siempre converge bien porque el modelo del sensor es realmente lineal en ese estado.