VIDEO SLAM D 02

Ahora vamos a echar un vistazo a la distribucin normal de nuevo, as que decimos que X se
distribuye normalmente con los parmetros mu y Sigma cuadrados si su funcin de densidad era
as que hay un normalizador veces e elevado a la potencia de menos media veces X Menos mu
cuadrado dividido por Sigma al cuadrado y as. Esto no era 1d que significa que la funcin de
densidad de probabilidad parece que tenemos nuevo aqu y luego tenemos esta curva en forma de
campana gaussiana con puntos de inflexin aqu y aqu que estn en MU menos sigma nu plus
Sigma y si dibujo una variable aleatoria Como con una probabilidad del sesenta y ocho por ciento
dentro de su rea tan 4 ms menos 1 Sigma es sesenta y ocho por ciento ms menos 2 Sigma es
95.5% + 4 ms menos 3 Sigma es 99.7 por ciento y as que si miras el exponente X menos mu
Cuadrado dividido por Sigma cuadrado es igual a 1 si y slo si x Sigma cuadrado es igual a 1 si y
slo si x es igual a mu ms menos Sigma que es exactamente este rango y ahora para generalizar
esto que est en 1d a dos dimensiones tendremos que seguirte ahora Podemos observar esto
como sigma x cuadrado ms y menos nu por lo que esto tiene que ser nuevo por qu y por lo que

Llamar a este nuevo x al cuadrado dividido por Llamar a este nuevo x al cuadrado dividido por
sigma y al cuadrado y por lo que hemos visto si esto es 1 esto significa en nuestro mundo 1d que
estamos aqu o aqu ress si esto es 1 esto significa ahora en nuestro mundo 2d donde tenemos mu
X Aqu y mu Y aqu que estar en algn lugar entre aqu y aqu, pero tambin entre aqu y aqu por
lo que esta condicin se cumple con los puntos que se encuentran en esta elipse ahora se puede
expresar esta frmula de manera diferente que vemos podemos escribir esto como una forma
cuadrtica Donde tenemos una matriz en este caso una matriz 2 x 2 de clido dividido por sigma x
cuadrado + 1 dividido por sigma y cuadrado en la diagonal y cero en cualquier otra parte Times x
menos mu x y y menos mu y como un vector de columna multiplicado desde El derecho y el mismo
que un vector de hileras lo multiplican desde la izquierda porque esto da el vector x este vector de
hilera lo que da el primer elemento x 2 primero ms el segundo Elemento x el segundo que da
exactamente esta frmula que es la misma que esta arriba Aqu y ahora vamos hav Echa una
mirada a esta matriz para que esta matriz en nuestra forma cuadrtica esta es nuestra varianza
elevada a la potencia de menos uno por lo que es la inversa de la matriz de covarianza porque si la
matriz de covarianza es Diagonal I puede obtener la inversa de esta matriz por un elemento sabio
Inversin Y as vemos si nuestra matriz de covarianza parece que no son una funcin de densidad
de probabilidad ser algunos veces normalizador e elevado a la potencia de menos la mitad veces
X menos mu veces Sigma elevado a la potencia de menos una vez X Minus mu donde ahora
usamos una forma vectorial y x es igual a nuestro XY & u Equals nuevo hacha nuevo vino y ya que
usamos vectores de columna necesitamos un vector de hilera aqu y tenemos que transponer esta
funcin no todas las densidades de probabilidad es un factor veces esta expresin Entonces en 3d
parecer que aqu est X aqu es por qu y este ser nuestro PDF por lo que este ser el primer
componente de nuevo este ser el segundo y nuestro pico estar aqu ahora Esta ser nuestra
forma de campana en 3d y aqu usted Tendr la elipse ofrecer un punto de inflexin que por lo
general dibujar en 2d y aqu el PDF se extiende en todas las direcciones al infinito y esto se llama la
elipse de un Sigma error as que ahora echemos un vistazo a que una vez ms en nuestro caso
nuestra matriz de covarianza Fue esto y obtuvimos una elipse de error as que aparte de la
Diagonal principal no tenamos entradas no nulas y este es el caso no correlacionado donde las
variables aleatorias xey son independientes porque X no me dice nada acerca de por qu
responder En el caso general nuestra matriz de covarianza se parece a esto, as que tendremos dos
varianzas en la diagonal principal y tambin tendremos las covarianzas fuera de diagonal que no
son cero, lo que significa que xyy ahora estn correlacionados para que nuestra elipse aparezca por
ejemplo Que por lo que ahora se convertir y se ver as, as que todava tendremos esta longitud
del eje principal que ser ahora aqu y la longitud del segundo eje que ser ahora aqu, pero estos
no ahora c Oncede ms con algunos puntos min y Max a lo largo del eje por lo que si desea dibujar
esta elipse lo que normalmente se hace es un valor propio eigen vector de descomposicin de
Sigma que es una matriz real y simtrica y por lo que se obtiene una matriz tres veces una diagonal
Matriz que contiene los valores propios veces B transponer donde vamos a consistir en dos
vectores de columna en el caso 2d que son los autovectores de Sigma que corresponde a los dos
valores propios y por lo tanto si se dibuja esto si v1 es el vector propio perteneciente a v2 es el
vector propio perteneciente a El valor ms pequeo del artculo as que djeme probar su intuicin
aqu as que usted puede ser que tenga una distribucin bivariada de xyy que es normal distribuido
as que ve que es normal distribuido es un mu que sea una matriz del vector y de la covarianza y
ahora le digo que adems I Sabemos que X se distribuye normalmente con mu escalares y algunos
Sigma cuadrados y por qu es normal distribuidos sables a Mew por lo que este es el mismo que
usted y Sigma cuadrados y quiero saber para thi La distribucin bivariada cmo se ve el Sigma de
manera sigma x al cuadrado Sigma XY Sigma Y al cuadrado Sigma XY de modo que los labios de
error correspondiente se ven como que por lo que es un crculo perfecto o se ve como que por lo
que es X son paralelas al eje de coordenadas Pero es alargado en una direccin o se parece
probablemente a que por lo que se inclina por decir 45 grados y uno de la mitad de acceso es un
poco ms largo y quiere ser ms corto para un B y C qu crees que es correcto
VIDEO SLAM D 03

Y tan interesantemente diferentes soluciones Son correctas como si les digo que X es

Normalmente distribuido con mu y Sigma cuadrado y por qu se distribuye normalmente

Con dos mu y Sigma cuadrados y primeros De todos ustedes saben que C que tambin es

Distribuido como un nuevo vector que Es mu y la tumba de alguna cosa que usted

Saber con certeza es que la expectativa de C est en mu 2 mu ahora ya que te di la distribucin de

xyy sabes los dos Las distribuciones marginales de C y as Conocer los dos elementos diagonales
del Matriz de covarianza que son cuadrados Sigma Y Sigma cuadrados pero no lo sabes Cualquier
cosa sobre las otras dos buenas noticias Y por lo tanto es correcto que esto pueda ser un Crculo en
cuyo caso sera como Que sin embargo tambin podra ser el caso Que es Sigma cuadrado Hoff y
as sucesivamente Esto sera un error labios como eso Pero esto es aproximadamente en un punto
dos sigma y esto es en 0.7 sigma Sin embargo la otra solucin no fue Correcto porque esto
implicara un Matriz con elementos en la Diagonal y 0 en otra parte y puesto que es Ningn crculo
Sigma 1 no sera igual a Sigma 2 que no es el caso por lo que el La solucin a es correcta yc es
correcta A pero no ser ahora vamos a preguntarme un segundo Cosa que usted tiene que construir
un mecanismo Que mueve un cierto punto con el alto Exactitud hacia arriba y hacia abajo hasta
que Motor un motor lineal que se fija a De todo y as este punto se mueve hacia arriba y Aqu
abajo pero entonces te das cuenta que Mientras que el motor es muy preciso es Limitado en su
mximo movimiento y Usted no puede conseguir el asimiento de un motor con un Movimiento
ms grande y por lo tanto, Siguiente construccin mecnica Construye un brazo de palanca que
fijas aqu Tambin en una pared y as puede dar la vuelta Este eje aqu y luego aade otro Conjunta
aqu que luego activar su Aqu si esto se mueve por un Cierta cantidad, entonces esto se mover
Mayor cantidad y por lo tanto la construccin de la Distancia entre las dos articulaciones decir Esto
es l y la distancia entre esos Dos juntas es tambin L, as que usted mira en la hoja de datos del
motor que es Suministrado por el fabricante y Le dice que este actuador de este motor Tiene el
ruido con la esperanza de Siria as No hay ningn efecto sistmico, pero con Algn ruido de
posicin que viene dado por la varianza Sigma x squared as que aqu tienes X ahora Esta
articulacin aqu te interesa En el movimiento que afecta El resto de su configuracin mecnica
tiene Otro movimiento aqu por qu es tambin Normal y as esperas una Expectativa de 0 y una
varianza de Sigma Y entonces tenemos dos variables aleatorias aqu Ahora estn funcionalmente
conectados por este dispositivo mecnico para que cada Movimiento en X resulta en un
movimiento en Y Y por lo tanto, cmo los labios de error de la Distribucin comn de X Y parecen
as Tambin establecemos C que es X Y y estamos Interesados en la matriz de covarianza de C ,As
que primero vamos a echar un vistazo a cmo los labios de error parece as que es enviar hasta A
cero ahora se ve como esto como Un crculo perfecto o parece

Esto por lo que es alargado en la direccin Y o se ve as o finalmente se degener como este

Elipse que es bsicamente plana en uno Direccin y acaba de extenderse en el otro Direccin as
que cul es la solucin a B C O D
VIDEO SLAM D 04

Vamos a encontrar esto en busca de la matriz de tolerancia que es el valor esperado de C menos el
valor esperado de C veces C menos el valor esperado de C transponer y por lo que C es como
hemos definido slo XY que es por la construccin de nuestro brazo de palanca X + 2 X por lo que
es porque nuestro brazo de palanca pareca que aqu era nuestro motor y aqu estaba la otra
articulacin y esto fue yo y esto fue tambin l por lo que cada movimiento aqu en esta articulacin
se duplica en esa articulacin y que le sirven X Era normal distribuido con la expectativa 0 y la
varianza sigma x al cuadrado y as desde que vemos que la expectativa de X era por supuesto 0 y
as de que se sigue que la expectativa de C es cero cero y as la frmula de aqu arriba se convierte
en muy simple sigmund Los ojos slo la expectativa de cc transponer lo que es el mismo que x dos
x veces X 2 X como un vector de fila que es simplemente x al cuadrado 2x al cuadrado + 4 x
cuadrado y esto es simtrico, pero la expectativa de x al cuadrado es la varianza de X tan Esto
simplemente se convierte en Sigma x cuadrado veces la matriz 1224 y por lo tanto para calcular los
labios de rea que es calcular los vectores propios de esta matriz y ahora no se preocupan por
sigma x al cuadrado por lo que los vectores propios de 12 24 y si se calcula que se obtiene el vector
uno es 12 Porque 12 a 4 veces 12 es 1 x 1 + 2 x 2 + 1 x 2 ms 2 veces 4 que es 5 10 y as esto es 5
veces 1 2 y as el valor propio w quiere es cinco porque si pongo en este vector Salgo cinco veces el
vector y por lo tanto el segundo vector propio como menos 21 y esto conduce a la siguiente menos
dos ms dos y menos 4 ms 4 y por lo que este es 0 que es 0 x menos 2 1 y por lo que tenemos
nuestros labios error como Sigue es que uno es uno a la derecha y para arriba v 2 es a dos a la
izquierda y una aplicacin y por lo que esta es perpendicular nuestra elipse es de hecho estirado
en el wee una direccin w2 es cero por lo que es de hecho un degenerado elips que es slo esto
Lnea y esto refleja el hecho de que al suponer que este enlace aqu es un multiplicador perfecto
las dos vari Ables estar perfectamente acoplado por lo que si hay algn error en X, entonces se
acaba de duplicar el error y sin el ruido aadido hombre realidad no ser capaz de producir estos
componentes mecnicos perfectamente y por lo que en realidad mi brazo de palanca agregar un
poco de ruido que Llevar a un error labios como eso, pero como hemos creado el problema es de
hecho una degenerada elipse ahora permtanme preguntarle lo siguiente ahora para esta
configuracin mecnica que el pecho de la cabeza con nuestra elipse se degener y X siendo
normal distribuido de esa manera Por qu ser normal distribuido como eso cul es la varianza y
por qu es lo mismo que un X es dos veces la varianza en X o cuatro veces la varianza y los huevos
o cinco veces la varianza en X ahora lo que es la

Solucin correcta a B C o D

VIDEO SLAM D 05

Y djame hacerte una segunda pregunta para que

Esto ya est generado en los labios del rea y

Por lo que sabemos que esta mitad de acceso aqu que es cero ahora me gustara saber cul es la
extensin en la otra direccin por lo que es

Sigma X o es la raz cuadrada de dos

Sigma X 1 raz cuadrada de 4 Sigma X o

Raz cuadrada de 5 Sigma X por lo que elija la solucin correcta de ABC o D

VIDEO SLAM D 06:

Y por lo tanto la respuesta a esta nuestra matriz de covarianza es sigma x cuadrado Times 1 a 24 y
nos enteramos de que esto es sigma x cuadrado veces la matriz hecha De nuestros vectores
propios que necesitan ser normalizados veces la matriz diagonal con los dos valores propios
tiempo sweetie Y descubrimos que lambda 1 era 5

Lambda 2 fue 0 por lo que la combinacin de los dos que Obtener 5 sigma x cuadrado + 0 veces
nosotros y VT, as que en nuestro sistema de coordenadas giradas que

Tienen una varianza de 5 Sigma X cuadrado a lo largo de un eje y 0 a lo largo del otro eje y as que
sta es la raz cuadrada de 5 sueo ahora x cuadrado que es la raz cuadrada de cinco veces Sigma
X as

Ahora vamos a echar un vistazo a la funcin de densidad de probabilidad de nuevo por lo que en el
caso 1d estamos en el siguiente de X estaba caliente

Dividido por la raz cuadrada de 2 pi x sigma x E elevado a la potencia de menos la mitad veces X
menos mu dividido por calamares Sigma y ahora puedo ahora reescribir esto como 2 pi sigma
cuadrado elevado a la

El poder de menos una mitad, as que la cuadrcula de la Sigma, pero luego tomar la ruta de nuevo
y por lo que es exactamente lo mismo ya que Sigma no es negativo xe elevado a la potencia Menos
mitad Slo voy a escribir x menos mu x 1 dividido por Sigma cuadrado Que es Sigma elevado a la
potencia de menos 2 veces X menos mu as que en el caso 1d que

En realidad no tiene sentido separar el X menos mu cuadrado en un multiplicador antes y un

multiplicador despus del sumidero

Mi elevado a la potencia de menos dos, pero luego se puede ver entonces voy a hacer trampa y
dimensiones ahora mi Sigma ser un

Matriz por lo que voy a tomar dos veces pi Sigma y que contiene ya los elementos cuadrados por lo
que no tiene que cuadrar que
Y tendr que tomar el determinante de esto elevado a la potencia de menos una mitad de las
veces que has elevado al poder

De menos la mitad veces X menos nu y ahora el hacha y t son vectores x la matriz de covarianza y
ya est

Contiene los valores cuadrados por lo que se eleva a la potencia de menos una vez X menos mu y
ya que estos no son vectores de nuevo Necesito aqu transponer para obtener esta forma
cuadrtica por lo que este es el multi

Dimensional Gaussian o distribucin normal donde x es el vector n-dimensional y mu es tambin

un vector n-dimensional y

La matriz de covarianza es una matriz de n filas y n columnas ahora despus de que entendimos
multidimensional

Distribuciones vamos a echar un vistazo a nuestro filtro comn de nuevo dos como usted recuerda
una red de Bayes dinmica pareca que

Estamos en el estado x t menos 1 hicimos una transicin a nuestro nuevo estado XT usando una
adicin para controlar UT y 0

Estando all medimos y obtenemos la medida CT y as en el caso unidimensional tenemos la

siguiente prediccin as que asumimos el modelo de prediccin lineal donde el viejo

Estado se multiplica por un factor y entonces agregamos un cierto control ahora

convencionalmente en vez de apenas tomar UT introduciremos un factor BTW que multiplique el
control y as que convierte de

Controlar el espacio al espacio de estado por lo que esta era nuestra ecuacin funcional y
tendramos que aadir tambin un poco de ruido que

Tambin se llama el ruido del sistema y el uso de esa frmula que hemos obtenido sin estado
predicho es slo una multiplicacin de nuestro Viejo estado por un plus nuestro control y otra vez
la nica diferencia que es que ahora multiplicamos nuestro condominio por esto

Factor adicional BTW y para la varianza que se obtiene que es la antigua Varianza x nuestro factor
lineal que necesita ser cuadrado ms el ruido de nuestro sistema y por lo que esta fue nuestra
forma de representar nuestro

Predijo la lis utilizando el primer y segundo momento de una distribucin gaussiana y en el

segundo paso tuvimos una correccin por lo que medimos un valor C debe ser igual a CT veces XT
ms algn ruido ahora este es el ruido de medicin y se nos oblig a poner Juntos nuestra nueva
medicin y nuestra creencia predicha para obtener nuestra nueva creencia posterior y lo hicimos
mediante la definicin de una puerta comn y el uso de que hemos computado nuestro nuevo
nuevo que consisti en La visin predicha plots el aumento de ganancias comn la innovacin que
es nuestra medida menos la Predijo la medicin y tambin calcul nuestras nuevas variantes que
fue 1 menos Kalman ganar veces C veces para predecir variantes por lo que los dos valores donde
nuestra nueva creencia haba Eckstein ahora vamos a ver cmo se ve en n dimensiones ahora la
prediccin es exactamente la misma frmula Excepto que ahora usamos matrices en lugar de
escalares y para nuestro mutante predicho tambin obtenemos un vector que se calcula
exactamente de la misma manera que en el caso 1d con a y r Eplaced por las matrices y usted
substituido por un vector y para la nueva matriz de la covarianza obtenemos esta frmula donde la
multiplicacin con un cuadrado es substituida por una multiplicacin de la izquierda con a y la
multiplicacin derecha con 18 y el Sigma R cuadrado es substituido por la matriz de la covarianza
apagado La transicin y para la correccin tenemos de nuevo exactamente las mismas frmulas
con un escalar sustituido por una matriz y el escalar XT reemplazado por un vector XT y obtenemos
la siguiente ganancia de Kalman que parece ser ms complicado como en el caso 1d, pero si usted
tiene Una mirada ms cercana que vea el pleno en el numerador si usted tiene nuestra sigma
predicho x ver esto es exactamente lo mismo que aqu en el numerador y en el denominador que
tenemos esto para que el mar x sigma x ver que es el sigma c cuadrado Cuadrado ms la matriz de
covarianza de ruido de medicin es la misma que aqu traza la varianza de medicin por lo que es
la misma frmula adaptada para el caso N dimensional y luego obtenemos nuestro nuevo mu y
sorprendentemente Esto es exactamente la misma frmula slo que mu es ahora vector K es ahora
una matriz y c es una matriz y tambin para nuestra nueva covarianza obtenemos La matriz de
identidad menos K veces c veces el Kovarian predicho para que pueda ver las frmulas para El caso
N dimensional son generalizaciones del caso unidimensional y tambin la derivacin de esas
frmulas es un poco ms complicado se puede ver claramente las simetras entre el lado izquierdo
y derecho y por lo tanto, si est interesado en la derivacin de

Estas frmulas se explican en detalle en el libro de fronting o la robtica probabilstica de Gert y

Fox ahora asumen que nuestro estado de sistema que es un vector consiste en nuestra exposicin
nuestra posicin Y y estn dirigiendo theta y as que esto es una matriz 3 veces 1 o una Columna
vector luego asumir nuestro control consiste en nuestra izquierda y derecha motor de lectura por
lo que ser una matriz de 2 veces 1 ahora vamos a hacer slo el siguiente cuestionario Quiero
saber la dimensin de todas las matrices en las frmulas cul es la dimensin de un Es 2 x 2 a x 3
3 veces 2 o 3 veces 3.

VIDEO SLAM D13:

Ahora vamos a aplicar todo esto a nuestro robot antes de todo lo que necesitamos un modelo de
movimiento, pero como usted recuerda en que necesita un ya hemos configurado el modelo de
movimiento para nuestro robot hay la pista con W y hay un poco de movimiento a la izquierda y la
derecha de las pistas Y por eso el robot se mover en un segmento de la curva donde tres aos son
y despus de moverlo habr cambiado su orientacin por un ngulo de alfa y as obtenemos las
ecuaciones siguientes as que alfa era r menos L dividido por el ancho del robot & R el radio fue L
dividido por alfa que slo funciona para no son iguales L y por lo tanto para este caso obtuvimos el
nuevo estado que es el estado antiguo ms R ms la mitad del ancho veces el seno de theta ms
alfa menos el seno de theta Y una expresin similar para el segundo componente y para el tercer
componente que acaba de aadir alfa al ttulo de intentar y como se ve esto es slo una funcin de
nuestro viejo bistec y alfa y nuestro pero alfa yr se calculan en ltima instancia a partir de lyr y As
que usted ve que ste es el estado un D este es el control para que podamos escribir esta ecuacin
en la forma X primo es una funcin de X y usted y slo para la integridad si R es igual a L entonces
tenemos la situacin de que el robot se est moviendo en la direccin de su rumbo tan Esta vez
aqu es cero porque si lnr son iguales entonces el encabezado no cambia y tambin esto es l pero
podras tambin usar nuestro porque en este caso R iguala L as e implement esta funcin para ti
as que los nodos internos de esta unidad Usted encontrar este slam 7a extensin de tela de filtro
de Kalman y se inicia una nueva clase que se llama filtro extendido de Kalman y vamos a aprender
en breve por qu se llama extendido y por lo que esta es la funcin G que acabamos de introducir
por lo que llega a estado que contiene el XY y theta y obtiene el control que es el movimiento
izquierdo y derecho y, a continuacin, si los dos son diferentes, entonces aqu es el clculo que
acabamos de tener en la luz anterior por lo que calcula alfa el radio R y luego calcula los tres
componentes que llam C Hi 1 2 3 aqu la nica especialidad es que la teta ms alfa se normaliza
para organizar entre menos PI y ms PI por lo que se obtiene mediante la adicin de PI, a
continuacin, hacer un modelo o divisin de 2 pi y luego restando pastel de nuevo y as para el
otro Caso de que el robot no gira y el uso de las ecuaciones que slo se mueven en la direccin de
la partida y dejar la theta como es y aqu aqu ves una nueva construccin que se llama Arry ahora,
ya que tenemos que tratar con vectores y matrices ahora Importar la tubera numrica as que esto
contiene clases para manejar matrices y vectores as que al final tomamos estos tres valores
escalares que se han calculado aqu o all y construyendo tu matriz poniendo chivonne dos y tres
en una lista y llamando a este constructor de matriz que Convierte esta lista en una matriz de
flotadores y yo soy la funcin principal hay algunas constantes para nuestro robot y estos deben
ser familiares por lo que este es el factor de conversin de tics motor de dos milmetros que es el
arrastre b Irth del robot y este es este tipo de desplazamiento que necesitar slo para la salida y
luego comenzamos con nuestra posicin de inicio medida que es XY que es la esquina superior
derecha de nuestra arena y un cierto ttulo y lo ponemos tambin en un Array leemos el archivo de
registro y luego aqu este es el bucle principal y es muy muy simple que lea los tics del motor desde
el archivo de registro convertirlos a una matriz y multiplicar el rea con patadas de dos milmetros
para que la izquierda y la derecha se multiplican y Entonces simplemente llamar a nuestra funcin
por encima del filtro de Kalman extendido Chi la funcin de control de estado y el uso de variables
adicionales en este caso slo el robot y por lo que obtener un nuevo estado y slo dependen de
todos los estados a una lista y luego en el env Salida de todos los estados y ahora recuerda que los
estados son en realidad el centro de nuestro robot, pero que utiliza para realizar un seguimiento
del escner lser que se desplaza por este desplazamiento de kannur y por lo tanto aqu abajo en
la salida de la posicin que modificamos el posi De modo que la salida de la posicin del escner
lser sombrero en lugar del centro del robot y esto es slo para ser compatible con nuestro manejo
anterior del punto que es rastreado por nuestra cmara de arriba ahora vamos a correr esto ahora
despus de ejecutar esto Producir un archivo llamado States from Tex dot txt ahora si abre esto
vamos a ver nuestra curva familiar podemos cargar datos adicionales como los puntos de
referencia para que podamos ver claramente las trayectorias snoo pero no corregir todos los
robots de referencia por lo que debera parecer familiar porque Es exactamente el mismo
algoritmo que tenamos en la unidad a ahora vamos a ver cmo podemos combinar esto con
nuestro filtro comn y voy a escribir la ecuacin de nuevo, as que en esta era la ecuacin para la
transicin del estado y debido a esto Transicin obtenemos esas dos ecuaciones que juntas son el
paso de prediccin del filtro comn y como veis esto es lineal de modo que la transicin del estado
antiguo usa una multiplicacin lineal por una matriz y el control tambin se multiplica por una
matriz sin embargo en nuestra descripcin De nuestro robot descubrimos que xt es alguna funcin
de xt menos 1 y UT y vimos que esta funcin tiene variables en el denominador y tambin tiene
funcin seno y coseno por lo que es no lineal y por lo que una forma de proceder sera linealizar
esta funcin Para que obtengamos esto y entonces podemos usar esto en un filtro de Kalman y
este es de hecho el filtro de Kalman estndar ahora vamos a tener un vistazo a lo que se llama el
filtro de Kalman extendido por lo que en lugar de linealizar esto use la funcin no lineal para
calcular la prediccin De nuestro nuevo bistec as que de la misma manera sv se movi aqu desde
nuestra ecuacin de transicin a la ecuacin que calcul el estado predicho desde el viejo estado
utilizando slo un reemplazo de ax t menos 1 Por mu t menos 1 ahora nos movemos aqu desde
nuestra ecuacin no lineal que usa xt menos 1 y usted a la ecuacin de prediccin que usa mu t
menos 1 y as que procedemos exactamente de la misma manera y luego para la covariacin
pronosticada hacemos lo siguiente ahora No tenemos esto una matriz ms porque lo
reemplazamos por una funcin no lineal y lo que tenemos que poner aqu es la matriz Jacobiana
de barato por lo que es todas las derivadas de g con respecto a todas las variables en el estado o
tambin podra Escriben esto como todas las derivadas parciales de g con respecto al estado y esta
matriz se calcula a nuevo t menos 1 y UT por lo que este es el paso de prediccin fuera del filtro
ampliado de Kalman y dar un ejemplo por lo que si nuestro estado es XY y theta entonces La matriz
G sera la derivada parcial del primer componente de g con respecto a X con respecto a Y y con
respecto a theta y lo haramos para los tres componentes de nuestra funcin G, por lo que esta es
la matriz cobiana del t en Caso nuestro tres-dim Xx + theta ahora vamos a averiguar trucos y el
caso que tenamos g de xy theta ln r que fue dado por xy theta ms estos trminos y por lo que es
g1 g2 y g3 por lo que tenemos la derivada parcial de chivonne con respecto a X que es fcil porque
no hay X en esta parte detrs de aqu es slo aqu por lo que es uno y luego las derivadas parciales
de la primera componente con respecto a Y todo en el primer componente no hay y en absoluto
por lo que es cero y es ms complicado Para theta por lo que no hay theta en esta parte por lo que
es slo una habitacin constante, pero hay theta aqu y aqu por lo que tiene que formar la
derivada de los dos trminos por lo que la derivada de la sine es la cuerda de tender la ropa por lo
que tenemos ahora el segundo componente No tenemos hacha en el segundo componente por lo
que es 0 tenemos un vino y para los derivados de tres spec 2 theta lo obtenemos y as para el
tercer componente que es slo theta ms alfa por lo que las derivadas con respecto a xey son 0 Y
con respecto a theta Es uno por lo que estos son los nueve elementos de nuestra matriz g ahora
que es un problema con esas frmulas porque aqu esa es la expresin r ms w tienen pero
nuestro es l dividido por alfa y por lo que si R es igual a L alfa es cero y tendramos un Divisin por
cero cuando se calcula esto as que tenemos que pensar en el caso R es igual a L vamos a echar un
vistazo a este componente aqu permtanme volver a escribir este componente ahora vamos a
pensar en lo que sucede si alfa va a cero ahora lo multiplicamos y obtenemos Ahora si alfa va a 0
entonces esto va al coseno de theta menos coseno de TIA por lo que esto ir a 0 multiplicado con
un factor constante todo el plazo pasar a 0 ahora aqu es un poco ms complicado porque esto ir
a 0 Pero esto es extrao es dividido por alfa para que esto vaya al infinito y por lo que necesitamos
tener una mirada ms cercana, as que esto es l veces y estamos interesados en el lmite cuando
alfa va a 0 ahora que usamos la regla de lupita a Matemtico francs que se enter de que
nosotros tipo encontrar este lmite b Y la formacin de la derivada en el numerador y el
denominador por lo que tenemos la derivada con respecto a alfa aqu abajo que es uno y la
derivada en el numerador que es menos seno de theta ms alfa y la derivada con respecto a alfa
de este trmino es cero lo que Permanece menos L veces seno de theta y as similarmente
podemos hacer el segundo trmino y as en general para el caso R igual a L obtenemos que T es
igual a 11 en la diagonal principal entonces menos L sinus de theta este es el trmino que
acabamos de derivar La ltima diapositiva y L veces el coseno de theta, por lo que ahora
obtenemos g que es derivada de g con respecto al estado para ambos casos para r igual a l es esto
y para no son iguales a L ver dos diapositivas hace ahora podemos Programa esto as que prepar
este cdigo para usted si usted quiere hacerlo fuera de lnea es salmn del golpe ser pregunta del
derivado del estado y ste es el cdigo que tenamos tan previamente en nuestro filtro extendido
de Kalman esto es el G que Calcula la prediccin de un estado antiguo y el control y aqu esta es la
funcin que tendr que implementar por lo que es todas las derivadas de g con respecto a todas
las variables en el estado y por lo que vimos este constructor resuelto en una matriz de 3 x 3 Con el
fin de mostrar cmo construir esto he puesto aqu este constructor de matriz que construy 3
veces 3 matriz slo con los elementos 1 2 3 en la primera fila 4 5 6 en la segunda fila y siete ocho
nueve en la tercera fila y Por lo que lo est haciendo poniendo cada fila en su lista y haciendo una
lista de todas las filas y luego dar esa lista al constructor de matriz por lo que necesitar la theta
que es el tercer componente, contando desde cero su nmero de ndice dos de la Estado y
necesitars izquierda y derecha que son los dos componentes despus del control y necesitars
distinguir esos dos casos si R no es igual a L as que el robot hace un giro o si R iguala L donde el
robot va derecho y as Con el fin de averiguar si su solucin es correcta la ma En funcin aqu hace
la derivada numrica de Qi y lo compara con sus derivadas analticas as que aqu definimos un
pequeo Delta y llamamos a la funcin original G con el estado modificado en X por este Delta con
el estado modificado en blanco y modificado en lder y Dividimos esto por Delta por lo que esto
nos da la diferenciaquotients que son si Delta es lo suficientemente pequeo cerca del cociente
diferencial que se est calculando aqu abajo y por lo que al final de salida el cociente diferencia
que se llama los parientes numricos la corrosin diferencial que es Lo que se calcula la diferencia
entre los dos y luego llamamos a la funcin de Python numrico que prueba si todos los valores en
estas dos matrices son similares y por lo que si ejecuta esto idealmente debera ver algo as que
esto es el resultado de la diferencia cociente Este es el resultado del cociente diferencial y se ve el
de la diagonal principal y aqu las derivadas con respecto a theta y se ve la diferencia Ference es
muy pequea y por lo que la prueba final te dice que parece ser correcto porque los valores en
esta matriz son muy pequeos ahora antes de empezar permtanme explicar brevemente este
truco con el cociente de diferencia por lo que estamos interesados en la derivada parcial de G 1
con respecto a X y como se sabe por clculo diferencial este es el lmite de chivonne de X ms
Delta Y en theta menos Chi 1 de XY y theta dividido por Delta y lo que hace la funcin principal es
que calcula este cociente de diferencia Para Delta muy pequeo como 10 elevado a la potencia de
menos 7, entonces imprime este valor y tambin imprime este valor que resulta de su
diferenciacin analtica de Chi 1 con respecto a X y luego imprime esos dos valores y comprueba si
Son aproximadamente los mismos, as que ahora vamos a programar esto.

VIDEO SLAM D14:

Ahora vamos a echar un vistazo a donde estamos as que nuestro paso de prediccin fue como
sigue mu fue calculado por chi y nuestra prediccin sigma fue calculado por chi sigma chi becas
post ms nuestro ruido del sistema son y por lo que hemos establecido esta ecuacin y tambin
hemos calculado Chi que fue la derivada con respecto al estado por lo que sabemos cmo hacer
esto y esto y este es el Sigma anterior por lo que hemos hecho con lo que queda es esto ahora esta
es una matriz 3 veces 3 que resulta del ruido de Nuestro control y tan similar para or el RT de la
computadora del ruido en nuestro control dado por la matriz de la covarianza de nuestro control lo
multiplicamos de la izquierda y la derecha por la matriz V donde V es la derivada de G con respecto
al control as que vea la analoga Entre este y ese pozo esta construccin transporta la varianza del
viejo estado a la varianza del nuevo estado y esto cuenta camin transporta la varianza del conroe
al nuevo estado y para escribir eso vamos a tener nuestro t est libre t Veces la var Iance del
control de izquierda la varianza del control de disturbios x el puesto de transporte de ser as que
esas variaciones capturar la exactitud de nuestro movimiento de la izquierda y la derecha pista del
robot asumiendo que no hay correlacin entre los dos as que si usted mira Las dimensiones esto
es tres veces tres ahora este spc es dos veces dos as que esto debe ser 3 veces 2 y esta es la matriz
de transposicin por lo que es 2 veces 3 y por lo tanto esta matriz V parece as por lo que es en
efecto tres veces a la primera Siendo la columna las derivadas parciales con respecto a L el
segundo siendo con respecto a nuestro ahora calculemos esas derivadas de modo que nuestra
funcin Qi slo g1 g2 g3 esta ex-esposa theta ms esos trminos y ahora tenemos que computar la
derivada parcial de chivonne con respecto A L ahora no hay L en esas ecuaciones porque est
oculto en nuestro as que nuestro pozo como L dividido por alfa y alfa era el mo es l dividido por W
as que vemos que son iguales a lw dividido por r menos l y as el trmino R ms w la mitad de esto
es lwd Dividido por r menos l ms w mitad que es lo mismo que w medias veces R + L dividido por r
menos L entonces vamos a seguir aqu tenemos que calcular la derivada parcial con respecto a L
fuera de la primera componente que es X ms este trmino Aqu cual que se calcula veces el seno
de theta prime donde vamos a establecer theta prime es theta ms alfa menos el seno de theta y
por lo que la derivada de X con respecto a L es cero mientras que aqu tenemos dl en este trmino
y tambin Tiene el L en el Alpha oculto aqu, por lo que tenemos que aplicar la regla de producto
por lo que la derivada del primer factor es 1 dividido por el denominador al cuadrado veces R
menos L ms R + L veces esta parte sin modificar ms la primera parte y modificado veces la
Derivado de la segunda parte que es el coseno x la derivada de theta prime con respecto a L, por lo
que esta es la derivada de alfa con respecto a L, que es menos 1 dividido por W, de modo que
globalmente obtenemos WR / r menos l cuadrado veces el seno Theta principal menos seno de
theta menos este com Es de aqu r ms L dividido por 2 R menos L veces el coseno de theta Prime
por lo que esta es nuestra derivada parcial de la primera componente de G con respecto a L y
tendremos que hacer seis de ellos permtanme anotar todos los Por lo que esto es lo que
acabamos de computar el segundo componente que tenemos para el tercer componente esto es
bastante simple menos uno dividido por el ancho ahora el mismo para las derivadas con respecto a
R para obtener un menos aqu y un plus aqu y finalmente la derivada Del tercer componente con
respecto a R es uno dividido por el ancho y as que recuerde que esto es para no son iguales a L y el
theta prime que se utiliza aqu en lugar de ms alfa nfortunately tendremos que hacer todo esto
para el caso R Igual a L as y as para cada componente tendremos que averiguar qu pasa si alfa va
a 0 y Sofia Brian va a theta as que djame darte esas ecuaciones as que en el caso R igual a L
tendrs que usar esto Conjunto de ecuaciones con una derivada parcial de la tercera componente
De g con respecto a ln r como el mismo que en la diapositiva anterior as que ahora vamos a
programar esto as que he preparado este archivo de pregunta de derivado de control de limo 7c
para usted y as esencialmente su tarea es la misma que en el caso anterior slo que la derivada de
G que tiene que tomar es ahora con respecto a controlar el significado de ln r y no con respecto al
estado y por lo que esta es la funcin que tendr que rellenar de nuevo hay dos casos para no son
iguales a L y cuatro son iguales a L y tendrs que devolver una matriz que sea tres filas veces dos
columnas y aqu abajo en la funcin principal todo est configurado como en el caso anterior esta
vez la diferenciacin numrica es con respecto a Ln R y se comparar esto con Su solucin analtica
y vamos a imprimir la diferencia, as que ahora por favor programe esto

VIDEO SLAM D15:

Ahora tenemos todo lo que necesitamos para implementar el paso de prediccin as que una vez
ms estas son las dos ecuaciones y RT definido como el tiempo Sigma control x regiones post con
un control de seal wats la varianza en el control de izquierda y la varianza en el control de la
derecha Poner en una matriz diagonal y ahora todava tenemos que definir esos dos y aqu est
nuestro enfoque estamos diciendo la varianza para el control de izquierda bien esto depende del
movimiento total con usted con nuestro truco de izquierda veces un factor por lo que este factor
puede, por ejemplo Ser 0 punto 3 por lo que diramos que hacemos un error de treinta por ciento
al mover nuestra pista izquierda y luego viendo el robot que hemos observado que hay sobre todo
mucho se desliz de las pistas en el suelo cuando el robot trminos y por lo que vamos a
multiplicar La diferencia entre izquierda y derecha, pero otro factor esto puede ser, por ejemplo,
sesenta por ciento y por lo que las variaciones aadir uP cuadraticamente tenemos que escribir
esto como sigue y haremos lo mismo para la derecha que estar aqu, pero t Aqu no habr
diferencia en el segundo triP ahora cuando se implementa el chi bien que es la tecla de funcin en
nuestra tela de filtro extendido comn y as que recuerde para los mtodos estticos que usted
puede llamar filtro extendido de Kalman punto g significa que tomar el nombre de la clase y no El
nombre de la variable de una instancia o puede tomar un nombre de variable como KF o auto
punto g ahora tambin necesitamos barato pero ella es la funcin DG d estaca ahora esta varianza
que es auto covariance punto ahora el V es d obtener el control y el Alfa-1 y alfa-2 se llaman el
factor del movimiento del control del punto del uno mismo y el factor de vuelta del control
autodidacta y todo lo que usted tiene que hacer primero es fijar esas dos ecuaciones en segundo
lugar pngalos en esta matriz diagonal entonces llame esta funcin calcular V y computar Este
trmino llama a esta funcin para calcular G y calcular este trmino y luego agregar esto hasta
obtener la covariacin pronosticada y luego al final actualizar el estado que se llama estado de
punto de auto para que el nuevo estado refleja la prediccin as que aqu est el archivo i
Preparado para ti es lmparas md comn predecir la pregunta por lo que ahora el constructor ha
crecido un poco que ahora toma el estado inicial de la covarianza inicial y tambin los robots con
toma de este factor de control de movimiento y control de factor de vuelta y almacena todo eso en
variables miembro, La funcin G que se implementan para yOu y aqu tienes que poner tu cdigo
ms valiente para la derivada parcial con respecto al estado y la Derivada parcial con respecto al
control entonces aqu hay otro cdigo que pongo aqu que hace eigen valor eigen vector
descomposicin Y convierte a dos por dos sub-matriz de la covarianza en una representacin de la
elipse de error consistente en un ngulo del primer eje y la desviacin estndar a lo largo del
primer y segundo eje que son las races cuadradas de 2 valores propios ahora no tienen que usar
Esto el cdigo abajo lo usar para dar salida a la elipse de error y ahora aqu est el cdigo que
tendrs que escribir esta vez es el paso de la prediccin y como yo solo he Hown que en primer
lugar calcular la nueva covarianza y poner este cdigo aqu y el clculo del nuevo estado poner este
cdigo aqu y eso es todo lo que hay que hacer y poner algunas sugerencias adicionales en esta
seccin de comentarios ahora vamos a echar un vistazo al cdigo principal Aqu otra vez hay esta
constante aqu estn nuestras constantes del filtro con respecto a nuestro control all para el
deslizamiento del treinta y cinco por ciento al ir derecho y un deslizamiento del sesenta por ciento
cuando giramos aqu definimos nuestro estado inicial y nuestra covariance inicial as que en el
principio decimos que sabemos nuestra posicin Con una desviacin estndar de 100 milmetros,
de modo que es de 10 centmetros en xey, y una desviacin estndar de 10 grados, entonces
inicializamos el filtro de Kalman aqu leemos el archivo de registro como lo hicimos anteriormente
y luego aqu este es nuestro bucle de filtro comn que por ahora Slo consiste en las predicciones
all y es ms o menos como el cdigo anterior para releer el control mediante la lectura de los tics
motor y multiplicar con el factor de dos milmetros de texto y luego nos Que se acaba de llamar un
filtro comn predecir utilizando ese control y esto reemplazar el estado antiguo que se almacena
en una variable miembro de la instancia de filtro comn por nuestro nuevo estado predicho y en
este loopy tambin almacenar el estado y la covarianza en listas adicionales en orden Para
emitirlos de nuevo aqu agarrar el centro del escner y no el centro del robot mediante este
desplazamiento y adems de la posicin veena salida la elipse de error que se calcula a partir de la
covarianza y tambin la desviacin estndar de nuestro encabezado como calculado Desde el
elemento 22 de nuestra matriz de covarianza tomando la raz cuadrada as que ahora por favor
implementar la funcin de prediccin

VIDEO SLAM D16:

Por lo que al ejecutar su script Python generar esta prediccin comn txt punto ahora abrir esto
en el visor de archivos de registro y ver nuestra trayectoria bien conocida que no es ninguna
sorpresa, porque ahora lo implementamos slo el paso de prediccin que no es diferente de lo que
Lo hicimos antes sin embargo, adems de computar el estado que ahora calcular la matriz de
covarianza y por lo que dos veces dos sub matriz para la posicin se visualiza aqu como un error
elipse entonces nuestra posicin inicial esto es un crculo esto es porque hemos establecido el
estndar Desviacin a 100 milmetros en ambos ejes y tambin hemos establecido la desviacin
estndar en el ngulo de rumbo a 10 grados que es eflected por este ms menos 10 grados de arco
circular representado aqu ahora a medida que avanzamos se puede ver que los labios de error se
Ms grande y tambin nuestra incertidumbre en hatting rpidamente crece y lo rpido que esto
crece tiene que ver con nuestros parmetros que record vector de movimiento de control y el
factor de cambio de control y la forma en que los labios de error ex Pense est de acuerdo con lo
que esperamos, a saber, despus de conducir por un tiempo nuestra incertidumbre a lo largo de la
trayectoria es menor que nuestra incertidumbre perpendicular a la trayectoria porque pequeas
diferencias en el control de izquierda y derecha se refieren a una curva en lugar de una lnea recta
y por lo que la Robot puede estar muy lejos de la trayectoria, aunque el error en el control de
izquierda y derecha es relativamente pequeo y me figura alrededor de esta curva aqu nuestra
elipse de error crece an ms rpido y cambia la orientacin de su eje principal y al final la elipse
de error es realmente Grande que refleja el hecho de que despus de conducir por un tiempo sin
tener ninguna medicin de los errores de posicin se acumulan esto tambin se sostiene para el
ngulo de rumbo que termina siendo ms menos 135 grados y lo que necesitamos ahora es
mediciones adicionales ahora echemos un vistazo a El paso de la correccin slo recordar nuestra
hiptesis fue que nuestra medicin CT tiene una relacin funcional con el estado que se puede
expresar como un lineal La ecuacin y desde que hemos desarrollado las ecuaciones que implica la
creacin de ganancia comn y la computacin de nuestro nuevo miu y sigma de la siguiente
manera que el brazo despus de la incorporacin de la medicin se obtiene por la visin prevista
ms los tiempos de ganancia comn la innovacin muy agria covarianza es la identidad Matriz
veces nuestra covariacin predicha menos los tiempos de ganancia comn c veces la covariacin
prevista por lo que es ms pequeo que la covariacin prevista por lo que ahora en general no
tenemos una ecuacin lineal aqu por lo que esta ecuacin ser ms bien CT es una funcin que no
es lineal de XT Y as en el filtro extendido de Kalman manejamos este caso similar a la funcin de
transicin no lineal as que nuestro nuevo mew se define como el mu predicho ms la ganancia de
Kalman y esto sigue siendo exactamente la misma frmula x nuestra medida menos y aqu est la
funcin de la diferencia H de la belleza y as en vez de sustraer la transformacin lineal mu t e i
resta la funcin no-lineal h aplicada a mi mu protegida y as wha T pasa a la matriz de covarianza
bien esto es lo mismo sin embargo aqu no tengo una matriz e ms tan similar al caso de la matriz
de transicin I multiplicar aqu por el Jacobiano de mi funcin de medicin y multiplicar esto con la
matriz de covarianza prevista tan Las diferencias son en primer lugar mi ecuacin de medicin
ahora utiliza una funcin posiblemente no lineal y todas las ocurrencias de C tienen que ser
reemplazadas por la edad y as H es el Jacobiano de la funcin H que es todas las derivadas
parciales de la edad con respecto al estado ahora Echemos un vistazo a esta funcin que define la
relacin entre nuestro estado y nuestras mediciones ahora vamos a echar un vistazo a esta relacin
por lo que nuestro robot est en algn lugar en el plano XY global con un ttulo de theta y ahora
por desgracia nuestro escner lser no coincide con El centro del robot as que aqu tenemos un
desplazamiento D si usted notar que como XL o ms adelante yl y as que es fcil ver que X lyl es
igual a la posicin XY despus del robot ms el desplazamiento Nt x unidad de vector en la
direccin de la partida por lo que ahora asumir que hay un hito que es visto por otro escner lser
de nuestro robot y lo que el tipo de lser nos da es un ngulo alfa en relacin con la orientacin del
lser escner que asumimos es Montado de tal manera que esta moneda se asienta con la
orientacin de nuestro robot y por lo tanto, aparte del ngulo de los cordones puede, por
supuesto, tambin nos da la distancia, digamos que este punto tiene las coordenadas xm ym
formato es fcil ver que r se da Por la distancia entre estos dos puntos por lo que esta XM menos
posicin XL del escner lser al cuadrado ms YN menos y el y tendremos que tomar la raz
cuadrada mientras que alfa que es el arco tangente de YM menos yl / XM menos XL ahora si Usted
tiene una mirada aqu esto le da realmente este ngulo y as que tendremos que restar este ngulo
que es Theta y as que esto es las ecuaciones que buscbamos esto es ver nuestra medida que es
un regular que consiste en nuestro y alfa mientras que esto Es nuestra funcin H Ahora tenemos
que calcular la matriz Jacobiana H de manera similar a lo que hicimos para Chi y ahora vamos a la
primera las derivadas parciales de R con respecto al estado que voy a escribir que de nuevo aqu
para que para calcular la derivada de R con respecto A X tenemos que tomar la derivada de la raz
cuadrada y sta es 1/2 veces la raz cuadrada x la derivada dentro de la raz cuadrada as que
necesitamos la derivada con respecto a X esto es 0 pero la derivada de esto es no cero as Es dos x
XM menos XL veces la derivada de esto con respecto a X ahora recuerda XL es X ms D veces el
coseno de theta y por lo que este XL menos nos da un menos X la derivada es menos 1 por lo que
cancela y vamos a Tambin introducimos Q como el trmino debajo de la raz cuadrada para poder
escribir esto en una notacin ms compacta de menos XM menos XL dividido por la raz cuadrada
de Q y similarmente la derivada con respecto a Y es menos yn menos yl dividida por la raz
cuadrada De Q y as se vuelve ms complicado con el deri Vativa con respecto a theta primero para
obtener la derivada de la raz cuadrada veces la derivada parcial de esto con respecto a theta es
dos x XM menos XL veces la derivada de menos xl que es menos pero entonces aqu la derivada del
coseno es signo menos As que obtendremos un menos menos por lo que es d sine-theta y por la
otra parte tendremos 2 veces ym menos y el ahora aqu tenemos yl es igual a y ms el seno de
theta de modo que la derivada del seno es 2 coseno pero Puesto que formamos la derivada de
menos y el menos permanece as en una forma ms compacta esto cancela esto es d dividido por
la raz cuadrada de Q veces XM menos xl sine theta menos ym menos y coseno theta y entonces
usted lo implementa Puede utilizar algo como delta x es igual a XM menos XL y delta y ym menos y
el so: puede expresar fcilmente esas ecuaciones como menos Delta X dividido por la raz cuadrada
de Q menos Delta Y y Delta x veces sine theta menos Delta Y veces coseno theta Por lo que fue la
primera parte ahora vamos a hacer th E segunda parte ahora necesitamos la derivada parcial de
alfa con respecto al estado y recuerda que alfa era arco tangente as que la derivada de alfa con
respecto a X es la derivada de esto y recuerda que la derivada del arctangente es una dividida por 1
ms x Cuadrado por lo que este es 1 dividido por 1 ms este cuadrado veces la derivada de que esa
es la derivada de un cociente por lo que vamos a utilizar la regla de cociente que es menos 1
dividido por el denominador cuadrado veces el numerador donde la derivada del numerador es 0
Porque es con respecto a X, pero todava tenemos que utilizar la regla de la cadena para la
derivada de menos XL hasta que obtengamos un adicional menos 1 y por lo que cancela y esta x
que es XM y es XL al cuadrado ms YN menos yl cuadrado tan Esta es la razn por la cual m menos
yl / q o delta y dividido por Q y similarmente obtenemos para la derivada con respecto a Y menos
XM menos XL dividido por Q que menos Delta X dividido por Q y as finalmente la derivada con
respecto a theta de nuevo esta Es mor E complicado y es fcil olvidar esto menos uno aqu que
resulta de la menos theta por lo que es lo mismo que menos D dividido por Q veces Delta X coseno
theta ms Delta Y sine theta menos uno por lo que ahora en las dos ltimas diapositivas que
recogimos todos Las seis expresiones que necesitamos para calcular la derivada de H con respecto
al estado por lo que una vez ms tendremos que calcular la derivada de este tiempo para la
funcin de medicin H y I preparado slam 7e medida derivada pregunta para usted y sigue el
patrn que Usted ha visto antes cuando hicimos los derivados para la transicin del estado as que
aqu est la funcin H as que calcula DX dy y de all el radio y el ngulo donde hay dos
especialidades primero de todos que utilizamos para armar la tangente a la funcin del delta Y y
del delta X y luego, despus de restar la theta, normalizamos todo para que est en el rango menos
PI a ms PI y aqu est la funcin que tendrs que implementar para que no tengas que calcular
esos seis derivados y ponerlos en Su matriz 2 x 3 y aqu abajo hacemos el equipo habitual
utilizando H la funcin que calculamos la derivada numrica entonces aqu llamamos a la funcin
que acaba de programa y de nuevo la reforma de la diferencia y la salida de verdad o falso
dependiendo de si la solucin Est suficientemente cerca de la solucin numrica para ahora por
favor programe esto.

VIDEO SLAM D17:

Ahora es el momento de producir nuestro ltimo esult el filtro extendido de Kalman y has visto
todas las ecuaciones y varias partes de esta unidad, pero djame ponerlas juntas en una
diapositiva as que tenemos el paso de prediccin donde calculamos nuestro mu predicho y
nuestro predicho Sigma de modo que este es el estado predicho y la matriz de covarianza predicha
donde en nuestro caso calculamos el ruido del sistema de la covarianza de nuestro ruido de control
y con el paso de correccin nos referimos a tener que calcular nuestra ganancia comn pero
tenemos que reemplazar cada aparicin de La matriz C que usamos anteriormente por nuestra
matriz jacobea de la funcin de medicin y de la que obtenemos nuestro nuevo estado del sistema
y nuestra nueva covarianza y stas son las tres ecuaciones para el paso de correccin por lo que ya
implement el paso de prediccin y la ltima cosa que ' Lo que hay que hacer es implementar este
paso de correccin hay una cosa final que quiero darle con respecto a la correccin paso ahora en
este paso se va a calcular el nuevo estado como se muestra en t La diapositiva anterior por lo que
aqu es la innovacin que es un 2 vector de la medida de alcance menos la medicin del rango
previsto y el ngulo medido menos el ngulo previsto ahora diferencia en nuestro trabajo como
usted esperara, pero la diferencia de ngulo podra darle Resultados errneos que conducen a la
caja que son realmente difciles de arrastrar ahora dicen que la medicin le dice que su ngulo es
este as que C es casi PI menos un pequeo Delta mientras que la posicin prevista es muy
estrecha por lo que H alfa es menos PI ms algunos pequeos epsilon Y tan slo restando los
valores que obtiene 2 pi menos Delta ms epsilon y que no es lo mismo que menos Delta plus
epsilon que se esperara y lo que tiene que hacer aqu es asegurarse de que el rango final del
componente Alpha est dentro de menos Pi k pi aunque en nuestro caso si normaliza ver
correctamente y si normaliza la edad correctamente entonces este caso probablemente no se
produzca sin embargo en general es bueno tratar este caso correctamente as que siempre ten
cuidado Cuando se toman las diferencias de aang que puede introducir algunos adicionales ms
menos 2 pi offset y an una cosa ms cuando se calcula la ganancia de Kalman esta frmula
contiene Q que es la matriz de covarianza de medicin ahora vamos a suponer que Q contiene la
varianza de la medicin de rango Y la varianza de la medicin de ngulos que estn sin correlacin
y por lo tanto en el programa Sigma R se denomina distancia de medida desviacin estndar y
sigma alfa se llama desviacin estndar del ngulo de medicin y tenga en cuenta que q contiene
las variaciones que es el cuadrado de las desviaciones estndar As que aqu est la asignacin de
programacin final, por lo que este es el familiar ampliado filtro de Kalman con su constructor que
ha crecido por dos elementos adicionales la distancia de medicin desviacin estndar en el
ngulo de medicin desviacin estndar aqu es Chi nuestra funcin de transicin de estado y aqu
tienes que poner En las derivadas con respecto al estado y al control aqu tendrs Para poner en tu
funcin de prediccin aqu tienes que poner en la derivada de la funcin de medicin con respecto
al estado en cada caso te di una pista donde encontrar esto y aqu esta es la parte nueva este es el
paso de correccin de la Kalman Filtro y no tenga miedo su funcin final ser mucho ms corto que
los comunes que he puesto aqu que esos comentarios harn su vida ms fcil y tan abajo en la
funcin principal que tenemos nuestras constantes familiares de robot ahora tambin tenemos las
constantes que utilizamos En nuestra unidad B para extraer los cilindros de la exploracin y para
emparejarlos a los cilindros en el mapa las constantes del filtro de la oblea son movimiento y factor
de la vuelta y est bien ahora aqu fijamos nuestro error de la medida en gama y en ngulo y usted
ve I don ' T confiar en mi corazn estamos mucho as que digo que el error de medicin es de 20
centmetros y el error de ngulo es de 15 grados comenzamos con una posicin inicial y ttulo y
una matriz de covarianza inicial de 10 centmetros en xey y 10 grados en el encabezado Y Despus
de utilizar nuestro pao de filtro que leer todos los datos por lo que ahora necesitamos los
motores, sino tambin el escaneo en los puntos de referencia que se convierten aqu a la lista de
cilindros de referencia y aqu este es nuestro conjunto de filtro comn de bucle de modo que es la
prediccin que es exactamente el mismo Paso que utilizamos anteriormente y es la correccin
ahora desafortunadamente en la correccin hay muchas cosas que hacer por lo que tenemos que
evaluar nuestros datos de escaneo encontrar los cilindros de proyectos que encontrar ms cercano
cilindros de nuestra Mapa y luego devolver las medidas de nuestro escner y las posiciones de los
cilindros del reverendo y esto se devuelve en observaciones de modo observaciones es una lista de
pares donde el primer elemento es el rango medido y el ngulo y el segundo elemento es el XY
coordenadas de la correspondiente Punto de referencia ya que estamos en una cierta posicin que
puede ver ms de un hito y esto es suave como sigue slo presentamos los hitos uno tras otro a la
etapa de correccin de nuestro filtro comn para cada paso daremos una sola observacin a
nuestro Paso de correccin y por lo que nuestro paso de correccin vamos a obtener un rango de
medicin en alfa y el punto de referencia xey, vamos a actualizar el estado actual y la covarianza y
despus de haber presentado 0 o ms observaciones al filtro nos detenemos de nuevo y al El
prximo paso de prediccin tambin para propsitos de registro ponen todos los estados en una
lista de todas las covarianzas en otra lista y para la visualizacin tambin aadimos todas las cartas
de referencia emparejadas de nosotros Ed observaciones y ms adelante utilizamos este cdigo
para emitir todas las posiciones todas las elipses de error y desviaciones estndar en el
encabezado y todos los cilindros de coincidencia ahora por favor programe esta funcin arriba y no
olvide reemplazar todos estos otros poner su mtodo aqu por Su cdigo real.

VIDEO SLAM D18:

Tan enhorabuena si usted hizo tan lejos tan usted realmente desarroll un cierto cdigo no trivial
as que comenzando de un robot real usted model el movimiento y la medida y deriv todo el

Derivadas que eran necesarias y luego se implement la prediccin y el paso de correccin de un

filtro completo de Kalman y por lo tanto cada vez que tiene que hacer esto para otro robot u otro
sistema en general, slo puede seguir xactly los mismos pasos que configurar el odel Para la
transicin del sistema que establezca
Sube el modelo para la medicin y entonces se derivan todas las matrices Jacobianas que luego se
requieren para la prediccin y las medidas de la etiqueta fuera del filtro comn y luego la prctica

Despus de haber configurado todo el problema es por lo general para encontrar buenos valores
para las diferentes matrices de covarianza y el filtro ahora cuando ejecuta su cdigo que producir
este comn de prediccin y correccin de archivo de texto y luego se carga que se dar cuenta de
varias cosas interesantes por lo que Por ejemplo en el robot de comienzo se sienta aqu y no se
mueve para los primeros pasos pero ve

Esos seis puntos de referencia por lo que no se mueve el ruido del sistema es cero y por lo que al
principio se acumular todas las mediciones una vez ms lo que a continuacin, resultar en un
rea ms pequea labios como nos movemos en usted puede ver tenemos ms o menos hitos
Hasta que solo tengamos dos aqu o incluso ninguno en posicin para que puedas

Ver en esta posicin de nuestra incertidumbre en el encabezado y la posicin es relativamente

grande esto se vuelve ms pequeo, ya que tenemos ms puntos de referencia en nuestra vista y
como ves es una trayectoria bastante suave y globalmente correcta ahora vamos a hacer la
siguiente modificacin a nuestro cdigo en el Kalman filtro de bucle que he puesto en este

Line que significa que para una cierta posicin donde puedo obtener cero una o ms
observaciones slo voy a mantener la primera observacin en la lista y despus de ejecutar esto,
recargar la trayectoria y

Ahora obtendremos lo siguiente la trayectoria parece ms irregular y mientras avanzamos vemos

que todos tomamos una sola observacin si tenemos al menos una y esto da como resultado que
nuestros labios de error sean ms grandes que en el caso anterior pero sin embargo nuestra
trayectoria

Parece asombrosamente bueno y ciertamente mejor que lo que hemos logrado en la unidad B
usando nuestra correccin de trayectoria basada en una estimacin de la transformacin de
similitud ahora si tomamos la ltima observacin en lugar de la primera vamos a obtener este
resultado que no parece tan bueno Ms especialmente en el principio tenemos este movimiento
ya sea debido a que este hito est relativamente lejos y nuestro escner tiene quizs algn error
sistemtico o debido al hecho de que esta posicin de referencia histrica no es realmente
correcta, as que globalmente no lo estamos haciendo Tan bueno como en el caso anterior y
tambin nota por ejemplo aqu como nos movemos a lo largo de cmo la medida a este punto de
referencia influye en cmo mi elipse est orientada por lo que una vez ms esta es nuestra
solucin final y felicitaciones si lo hizo con xito a travs de esta unidad de nuestro slam
Conferencia as que te veo en la siguiente unidad
VIDEO SLAM E 01:

Ahora bienvenidos a la unidad que ser sobre el filtro de partculas ahora vamos a primero echar
un vistazo a nuestro filtro de base de nuevo que

Toma nuestra vieja creencia el control y nuestra medida y de eso primero calcula una prediccin
que voy a dar ahora en un

Forma continua y luego la correccin que es nuestra creencia predicha multiplicada por la
probabilidad de nuestra medida bajo la condicin de que estamos en el estado XT y luego
devolvemos nuestro nuevo

Creencia ahora hemos visto dos formas diferentes de implementar esto el primero estaba usando
aproximaciones discretas de distribuciones as que en lugar de tener una distribucin continua
tuvimos algo as y la convolucin se calcul usando una suma en lugar de una integral, as que si
esta es la prediccin Y esta es la correccin de estos dos pasos se llevaron a cabo de la misma
manera, pero slo la sustitucin de la integral por su hijo en la primera ecuacin y luego
observamos que si se inicia en algn lugar y nos movemos y por la convolucin de nuestra
distribucin se ampliar hasta en Nuestro caso se aproxim a una distribucin en forma de
campana ahora subdividir el espacio de esa manera conduce a un filtro de histograma donde es
nuestra otra representacin fue paramtrico por lo que dijo que nuestra creencia es normal
distribuido de modo que la distribucin entera puede ser representada por el primer y segundo
momento mu y Sigma cuadrado y esto ha llevado a las ecuaciones de filtro de Kalman que estaban
fuera de la forma mu T y Sigma T la matriz de covarianza que Calculado el paso de prediccin y
luego una ganancia de Kalman y nuestro nuevo mu T ahora sin una sobre-lnea y la varianza Sigma
T y esas tres frmulas donde el paso de correccin ahora si comparamos esos dos vemos que aqu
cualquier distribucin es posible especialmente las distribuciones que tienen mltiples modos Lo
que significa varias fotos por otro lado tengo que definir una lista discreta y ya hemos discutido
que tal vez quiera hacerlos pequeos para una mejor precisin, pero a su vez tendremos muchos
de ellos haciendo el clculo ineficiente por lo que hay un trade-off Entre la aproximacin y el coste,
por otro lado mi filtro comn es

Muy eficiente necesita lidiar con el primer y segundo momentos tambin si nuestras distribuciones
son de hecho distribuciones normales, entonces la representacin es exacta en el otro y asumimos
la distribucin normal por lo que slo tenemos un pico ahora echemos un vistazo a esta modalidad
uni ahora Cuando introdujimos la posicin de los robots como una distribucin dijimos que no
sabemos exactamente dnde est el robot en lugar de pretender que est en una determinada
posicin que decidimos representar su posicin por esta distribucin y por lo que despus de
retirado el robot tendr una posicin predicha como Eso y entonces si integramos una medida esto
conducir a nuevo posterior como que sin embargo puede haber situaciones donde el robot podra
estar aqu

O aqu y probablemente usted pens acerca de que antes cuando se encontraron con el caso de
ensueo comenz en esta esquina y

Mover hacia abajo aqu y actualizamos nuestra posicin utilizando una coincidencia de las paredes
de la arena con nuestros lser candida y que funcion muy bien sin embargo la arena es una plaza
y si hubieran colocado nuestro

Robot aqu nada sera diferente en trminos de la observacin que nuestro lidar ve y por lo que de
hecho tambin podra ser colocado aqu o aqu y por lo que si no sabes nuestra posicin de inicio
se podra modelar esto como distribucin con cuatro picos que significa un 2d Versin de esto ella
parece que ahora decir esto es mi arena y no tengo ninguna informacin inicial de donde mi robot
es ahora qu crees que esto significa para el filtro de Bayes significa que no puede manejar esto
Caso o se puede manejar el caso y la distribucin de alguna manera tiene que ser decir un pico
muy muy plana o ver que puede manejar el caso, pero ya que no s dnde estoy la distribucin es
slo una constante por lo que es plana lo que hacer Usted piensa que un B o C

VIDEO SLAM E 02:

Por supuesto en general si no hago nada acerca de la posicin de mi robot, entonces una
distribucin uniforme ser la representacin adecuada y algunos creo que ser constante tiempo
constante se elige de tal manera que integral sobre toda mi arena ser uno ahora vamos a
introducir tres importantes Clases de problemas de localizacin la primera y ms fcil es el
seguimiento de posicin y este es un problema que hemos trabajado hasta el momento por lo que
hay un puesto inicial conocido y por lo general una distribucin unimodal, por ejemplo, una
distribucin gaussiana y, a continuacin, hay un problema de localizacin global y esto significa No
tengo ninguna indicacin de mi post inicial y en general con el fin de resolver que una distribucin
unimodal no es til ahora similar a que es un problema llamado robot secuestrado que somos un
problema de localizacin global se supone ms habr La posibilidad de que alguien secuestra al
robot y lo traslada a otro lugar donde el robot entonces tiene que recuperarse y determinar su
nueva posicin global, de modo que ahora en realidad robots un No son secuestrados con tanta
frecuencia al menos hasta ahora, pero la importancia prctica de este problema es que cualquier
algoritmo de localizacin global podra fallar eventualmente y si lo hace necesita recuperarse de
este

Fallo significa que necesita la capacidad de descubrir que la posicin que podra haber arrastrado
por un tiempo est completamente equivocado y que por lo tanto, necesita para determinar su
posicin global de nuevo por lo que la importancia prctica es que el robot es capaz de recuperarse
de los fallos de localizacin por lo que estos Son tres problemas de localizacin diferentes que a
menudo se mencionan en la literatura de robtica estndar ahora vamos a pensar en la
localizacin global de nuevo por lo que acaba de aprender que si no s dnde estoy entonces una
distribucin uniforme sera la mejor opcin posible, sin embargo, si estoy Interesado en modelar
esto con una Gaussiana otra posibilidad sera colocar un gaussiano en el centro con la varianza
muy grande as que en 1d esto significara que esta es mi creencia que se centra aqu, pero eso no
importa porque voy a establecer la varianza Para ser muy grande y por lo tanto durante el filtrado
si integrar esto con mi medida mi creencia posterior ser casi la misma que la probabilidad de mi
medicin y As que aunque no pude representar mi creencia inicial exactamente tuve que
reemplazar la distribucin uniforme por este Gaussian muy plano que finalmente despus de mi
primera medicin en una buena suposicin para mi estado del sistema ahora qu crees que sera
posible modelar La creencia por una distribucin normal muy amplia y posteriormente se basan en
las mediciones para determinar nuestro estado del sistema s o no