Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

65

description

This thesis shows you how to make a Kalman filter for tracking by centroid

Transcript of Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

Page 1: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

��������� ������� ����������������� ����� ���� � � �� ����! � ��"�#�

$&%�'(%*)+-,/. % , )0-1 %�2 03�4 0 26587 +/. 7 +-9;: 7<3 + '=2 +-,?> %A@B3�4 + 'C5 0 @ 1D0-,

EGFIH

4�JLKNMIHPOFQ'(FIRLMISTF 1 FISVUXW

3�7Y'(%�'Z FI[\W^]�JLOM�KNFI[\F�HPWN_a`J Z Jb]PF�cTMIHPK^JLMIRdM�RLM

> MeK^`Rb]PMeOfOW�2*JLW^STK^JLM Z OW-RLM=26FI[Cc`]PMeK^J )FIS

EGMIHPMgFIh]PW^STW^H�W^RjiIHPMeOFCOW

@jJLKNW^STK^JLMeOF�W^Sk2*JLW^STK^JLM Z OWlRLM=26FI[Cc`]PMeK^J )FIS

0 Z W Z FIHNm . HNn<@dWNFIcoFIRLOF 0 Rb]PMI[CJbHPMISTFC4pFIhRLW Z

'(W^c]�JLW^[�hHPW�OW^R�qIrerBs

Page 2: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

Page 3: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� �

Page 4: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� ������������

��������������������! #"%$�& '(���)*����+,������$ �.-/10 #"%��$2�43(�5���76$8 /95:.9<;�=7>�?5@A=7BC=�D*D*D*D*DED*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D 995: FHGJILKM> �ON ��P?Q>SR�IUT2V�@W?YXQI NLZ ?[D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D F95: \H;]@W?5V P?5^ ��Z ?_D*D*D*D*DED*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D \95: `bac^ed�=7@ � ?5^f=hg�d � IU>C^eIER � @ �Oi IjIU^ Z IkV�@W?YXQI NLZ ?lD*D*D*D*D*D*D*D*D*D \95: mHno?!R�IUTO?5^4R�I Z @A=7p�=Yqr?5^sV�@WILt � ?5^uD*D*D*D*D*DED*D*D*D*D*D*D*D*D*D `

95: m�:.9 v]^ Z � BC= N �%P?Q>wR�I4Bx?Yt � B � IU> Z ?4p�=7^y=7R�?sIU>zBx?!R�IUTO?Q^�=7R�=7V!{Z � tQ?5^|D*D*D*DED*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D `

95: m�: F }�@A= Ny~ IU?s@W?5p�d�^ Z ?�R�I4V#?5^ �ON ��P?Q>�XJtQIUTO? N � R�=�R N ?5>E�*=7TOBC=�>^e>�= ~ IU^�D*D*DED*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D m

95: m�: \ }�@A= Ny~ IU? N ?5>CBx?!R�IUTO?Q^ N ?5>�IL� � ?5> � ^ Z =�^�D*D*D*D*D*D*D*D m95: m�: ` �s?5BxV�=7@A= N �YP?Q>o@W?5p�d�^ Z =�X Z @A= Ny~ IU?�R�I�?5p�qrI Z ?5^*=7@ Z � {

N d�T�=7R�?5^�d�^y=7>�R�?�@WIUV�@WIU^eIU> Z = N � ?Q>�IU^4p�=7^y=7R�=7^fIU>Ct � ^ Z =7^ m��� ��$��M6� ��&Y3(�2�#���5�! #"%� �F�:.9<��IUV�=7@A= N ��P?Q>SR�IUT2?5p�qrI Z ?�R�IUT2@WIU^ Z ?�R�IET�=*IU^ N IU>�=kD*D*D*D*D*D*D*D �

F�:.95:.9 �sT�=7^ � K N = N �YP?Q>SR�IET�=*@WI i ��P?5>SR�I � > Z IU@ PIU^�D*D*D*D*D*D*D*D �F�:.95:.95:.9 �fI N � >�R�=7R�D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D �F�:.95:.95: F �w=7B � >�?�D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D �F�:.95:.95: \ �s?5>�I NLZ � t � R�=�R�D*D*D*D*DED*D*D*D*D*D*D*D*D*D �

F�:.95: F �kV#IU@A=7>�R�?�^e?5p�@WIzT�= � BC= i IL>�D*D*DED*D*D*D*D*D*D*D*D*D �F�: F�}�@A= Ny~ IU?hV#?5@ Z =7BC=��>�?wD*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D�9Y�

F�: F�:.9 }�@A= Ny~ IU?hV#?5@ N IU> Z @W? � R�IcD*D*D*D*D*DED*D*D*D*D*D*D*D*D*D�959� � �

Page 5: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� t ����������� ���

F�: \�}�@A= Ny~ IU? N ?5>CIUT8KMT Z @W?S�*=7TOBC=�>SIL� Z IU>�R � R�?�D*D*D*D*D*D*D*D*D*D�9YFF�: \�:.9 �5d�^ Z � K N = N �YP?Q>lD*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D�9YFF�: \�: F � � T Z @W?��*=7TOBC=�>oD*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D�9Y\F�: \�: \ G � ^eI��>�?ER�IUT2^ � ^ Z IUBC=kR � > P=7B ��N ? D*DED*D*D*D*D*D*D*D*D*D�9U`

F�: \�: \�:.9 � � ^ Z IUBC=7^]X�;]@W? N IU^e?5^ DED*D*D*D*D*D*D*D*D*D�9U`F�: \�: \�: F } � IUBxV#? N ?5> Z � >�d�?�t�^%:]R � ^ N @WI Z ? D*D*D*D*D*D�9YmF�: \�: \�: \ ��=7@ � =7p�T�IL^4R�IEIU^ Z =7R�?�X�tQI NLZ ?5@WIU^SD*D*D*D*D*D�9YmF�: \�: \�: ` no?!R�IUTO?5^�R�IJIU^eV�= N � ?kR�IkIU^ Z =7R�?5^4V�=7@A=E^ � ^W{

Z IUBC=7^fR � > P=7B ��N ?Q^ D*D*DED*D*D*D*D*D*D*D*D*D�9��F�: `bG � ^eI��>�?ER�IUT8KMT Z @W?S�*=7TOBC=�>SIL� Z IU>�R � R�? D*DED*D*D*D*D*D*D*D*D*D�9��

F�: `�:.9 � P=7T N d�T�?zR�I�� D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D�9Y�F�: `�:.95:.9 v N d�= N � ?5>�IU^kR � ^ N @WI Z =7^zR�IUT�KMT Z @W? �*=7TOBC=�>

IL� Z IU>�R � R�? D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D F5�F�: `�: F �kT i ?5@ �.Z BS? DED*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D F5F

� )*�Q3(������&�� �5$8 (����3(&Y��$8 (��& ���\�:.9<v]g�d � V#? D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D F5\

\�:.95:.9 ;]@W? N IU^e?�R�I � >�^ Z @Wd�BxIU> Z = N �%P?Q>_D*DED*D*D*D*D*D*D*D*D*D F7`\�:.95: F ��I N d�IU> N � =7^4XCIU^ Z =7R P� ^ Z �.N =�^lD*D*D*DED*D*D*D*D*D*D*D*D*D F7`\�:.95: \ �s?5> N TOd�^ � ?Q>�IU^ D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D F5�

�� 6$��4����$ �����:.9 ;]@ � BxIU@4tQIU> Z =7>�= � > Z IU@A= NLZ � tQ= D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D \����: F � NLZ d�=7T �"! = N �%P?Q>�R � ^ N @WI Z =ER�IUT8KMT Z @W?��*=7TOBC=Y>ED*D*D*D*D*D*D*D*D*D \5���: \ �kV�T �ON = N �%P?Q>�V�@ � > N � V�=7TMD*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D `��

#���+,�����! (������& ���

Page 6: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� ��������

F�:.9�� P=7T N d�T�?hV�=7@ N � =7T2R�I�T�=�BC= Z @ � ! �k:hGJILKM> �ON ��P?Q>CR�I�T�=�BC= Z @ � !� ����� :]nod�T Z � V�T �.N = N �%P?Q>JR�I � ����� V#?5@(T�=�tQ=7@ � =7p�TOI(R�I � > Z I i @A= N �YP?Q>� :4V�� � � N ?5@W@WIU^eV#?5>�R�IJ=����y:�D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D�9Y�

F�: F � P=7T N d�T�?�R�I������ ��� � ����� � P=7T N d�T�?�R�IfT�= � >5tQIU@W^y=sR�I������ ��� � �����XC?5p Z IU> N ��P?5>�R�IUT���=7V�T�= N � =�>�? � >5tQIU@W^e?�:��� ��!"$#&%('�) � ����� � �*����,+ %(' :-��= Pd�T Z � B�=�T P� >�IY=SB�d�IU^ Z @A=�T�=�^e?5TOd N ��P?Q> N IU@W@A=7R�=�R�IUT^ � ^ Z IUBC=kR�IEI N d�= N � ?5>�IL^4R �/. IU@WIU> N � =�T.IU^4R�IEV�@ � BxIU@�?5@WR�IU> :zD*D*D F5�

\�:.910 BC= i IU> � > �ON � =�T :�v]Ts?5p�qrI Z ? = Z @A= Ny~ IY=7@zIU^�T�=Cp#? Z IUTOT�=!:2� ?5^N T �ONy~ ^�g�d�IcIUTMd�^ed�=7@ � ?3��=ER�=7R�? N ?5@W@WIU^eV#?5>�R�IU> N ?5>STO?5^4=7^ Z IU@r{� ^ N ?5^�^e?5p�@WIzT�=*p#? Z IUTOT�=!:�D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D F5m

\�: F54wIU^ed�T Z =7R�?CR�IC^eI i BxIU> Z =7@JT�= � BC= i IL> � > �ON � =�T : v]^ Z =�@WI i ��P?5>N ?5@W@WIU^eV#?5>�R�Ij=*d�>�=*^e?5T�=J@WI i ��P?5>���{ N ?5>�I NLZ =7R�=!: �kp�^eIU@rtQIcg�d�IT�=Ep#? Z IUTOT�=J>�?6��=h^ � R�?�^eI i BxIU> Z =7R�=kIU>C^ed Z ? Z =7T � R�=�R�R�IUp � R�?= g�d�I T�=od�B�p�@A=7T �"! = N �%P?Q> >�?7��= � > N TOd � R�?�=7T i d�>�=7^ � > Z IU>�^ � {R�=7R�IU^wR�I�V � ��IUT,:�;�=7@A= N =7R�= � BC= i IL>�R�I*T�=h^eI N d�IU> N � =E^eIE@WIL{=7T � ! =z^eI i BxIU> Z = N �%P?Q>SX N�P=7T N d�T�?*R�IUT N IU> Z @W? � R�I5:fv]T N IU> Z @W? � R�IN ?5@W@WIU^eV#?5>�R�IJ=7T8tQ=7TO?5@�?5p�^eIU@rtQ=7R�?�: D*D*D*DED*D*D*D*D*D*D*D*D*D F��

\�: \H;(?5^ �ON � ?Q>�=�B � IU> Z ?4R�I4T�=4tQIU> Z =7>�=sR�I Z @A= Ny~ IU? ) @WI N d�=7R�@W?�p�T�=7> N ?8+IU>�IUT N IU> Z @W? � R�IsR�IcT�= � BC= i IU> � > �ON � =�T : �kp�^eIU@rtQIjT�= N @Wd ! V#IU@r{. I NLZ =7BxIU> Z I N IU> Z @A=7R�=�:kv]T N IU> Z @W?�R�I�T�= N @Wd ! @WIUV�@WIU^eIU> Z =�=7TN IU> Z @W? � R�IcR�IET�=*@WI i ��P?5>S^eI i BxIU> Z =7R�=k>�?x=7T2R�IET�=*p#? Z IUTOT�=!:�D*D F��

t

Page 7: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

t � � ���������

\�: `b��I N d�IU> N � =�R�I � B P= i IL>�IU^�V�@W? N IU^y=7R�=7^%:8�kp�^eIU@rtQI N ?5Bx?sT�= N @Wd !R�IUT N IU> Z @W? R�ICT�=�tQIU> Z =7>�=oR�I Z @A= Ny~ IU?o^eI�B�d�ILtQI5: v]^ Z I�IU^IUT]tQ=7TO?5@cV�@WIUR �ON ��? V#?5@zIUT]KMT Z @W?o�*=7TOBC=�> X IUT N IU> Z @W?�R�I�T�=N @Wd ! � >�R �ON =og�d�I Z =7> TOIrqr?5^h^eIoIU^ Z#P= R�IUT N IU> Z @W? � R�I�IL��= NLZ ?�:�s?5>�^ � R�IU@WIkg�d�I�T�=�tQIU> Z =7>�=�R�I Z @A= Ny~ IU?xIU^Jd�> N d�=7R�@A=7R�?�R�I9Y��V � ��IUTOIU^�V#?5@�T�=7R�?�:kD*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D F��

\�: mH��I N d�IU> N � =zR�I � B P= i IL>�IU^4V�@W? N IU^y=7R�=7^%: D*DED*D*D*D*D*D*D*D*D*D F��\�: �H��I N d�IU> N � =zR�I � B P= i IL>�IU^4V�@W? N IU^y=7R�=7^%: D*DED*D*D*D*D*D*D*D*D*D F5�\�: � PacT Z � B�= � BC= i IU>SR�IET�=*^eI N d�IU> N � =�: D*D*D*DED*D*D*D*D*D*D*D*D*D F5�\�: �HGJIU^ N @ � V N �YP?Q>�R�IUT�Bx?Yt � B � IL> Z ?�R�IUT(?5p�qrI Z ?�IU>oIUT(V�T�=7>�?� ��:

v]T�?5p�qrI Z ?�V�=7@ Z I�=7TO@WIUR�IUR�?5@sR�I�T�= N ?!?5@WR�IU>�=7R�= ) �5\�� `�� +]X�^eIBC=7> Z � IU>�IjT�=*V#?5^ �ON ��P?Q>�� N ?5>�^ Z =7> Z IJV#?5@j=7T ijPd�> Z � IUBxV#?�: D*D*D \5�\�: �54wIU^ed�T Z =7R�?*R�IzKMT Z @A=7@fT�=�^eI��>�=7T N ?5@W@WIU^eV#?5>�R � IL> Z IJ=hT�=h=7p�^ N � ^y=�: \�9\�:.9Y��4wIU^ed�T Z =7R�?�R�I�KMT Z @A=7@zT�=C^eI��>�=7T N ?5@W@WIU^eV#?5>�R � IU> Z Ih=�T�=�?5@WR�IL{

>�=7R�=�:3��=�IU^ N =7T�=*IU> T�=�R � @WI NUN ��P?Q>�� IU^kR�IS\�V � ��IUTOIU^%:jv]^ Z ?^ �Oi > � K N =Cg�d�I IUTj?5p�qrI Z ? >�? ^eI�Bx?Yt ��P?oR�IUBC=7^ � =�R�?oIU> IU^ Z =R � @WI NUN ��P?Q> :[D*D*D*D*DED*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D \5F

\�:.959 v]@W@W?5@cIU>oT�=7^wIU^ Z � BC= N � ?Q>�IL^%:s�kp�^eIU@rtQI*g�d�IhIUT�V�@W? N IU^e? Z � IU>�Id�> Z @A=7>�^ ��Z ?5@ � ?�=7T � > �ON � ?�R�IUT(KMT Z @A=7R�?������������������! "�#��$�%&�' ��(��)�*��+,$��-�(. /#�0$213+4.056%&.�1�.0$,7& ��(�L:xv]^ Z ?oIU^wqrd�^ Z =7BxIU> Z I�TO? g�d�I^ed N IUR�I5: ��= ^eI N d�IU> N � =xR�I . @A=7BxIU^ N ?5@W@WIU^eV#?5>�R�I N ?5>�IUTfIrqrIR�IUT Z � IUBxV#?x:oD*D*D*DED*D*D*D*D*D*D*D*D*D*DED*D*D*D*D*D*D*D*D*D \5\

Page 8: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

t � � � �������������� �����������

Page 9: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� ����� ������

v]> IU^ Z I Z @A=7p�=Yqr?�V�@WIU^eIU> Z ?hTO?5^f@WIU^ed�T Z =7R�?5^4R�IUT8IU^ Z d�R � ?�R�IkKMT Z @W?5^��*=7TOBC=�>hXV�@W? N IU^y=7B � IL> Z ?JR�I � B P= i IU>�IL^�V�=7@A=zT�=z@WIY=7T � ! = N �%P?Q>�R�IUT#V�@W? Z ? Z � V#?kIU>Cn = Z T�=7p� 56%�� +4����zg�d�ISV#IU@WB ��Z IE^eI i d � @j?5p�qrI Z ?5^J@WIUV�@WIU^eIU> Z =7R�?5^jV#?5@kd�>�=�^eI N d�IU> N � =R�I � B P= i IL>�IU^U:CP=7^ �ON =7BSIU> Z I � 56%�� +4����od Z � T � ! = ^eI i BxIU> Z = N �%P?Q> � N ?5BxV#?5>�IU> Z IU^ N ?5>�I N {

Z =7R�=7^�� N IU> Z @W? � R�IU^4XCIU^ Z � BC= N � ?7>�IU^f�*=7TOBC=�> :v]T2d�^ed�=7@ � ?*^eIUTOI NUN � ?Q>�=cIUT2?5p�qrI Z ?�= Z @A= Ny~ IY=7@�V#?5@�BxIUR � ?ER�IktQ=7@ � ?5^ N T �ONy~ ^4R�I@A= Z#P?5>x^e?5p�@WIkIUT2?5p�qrI Z ?�IU> d�>�=htQIU> Z =7>�= i @ P=�K N = � > Z IU@A= NLZ � tQ=!:

��=7^ � B P= i IL>�IU^(g�d�Ik^eIcV�=7^y=7>SIUTM^ � ^ Z IUBC=j^eI ��=7> Z @A=7>�^ . ?5@WBC=7R�?z=E> � tQIUTOIU^R�I i @ � ^sV�=7@A=�d�>�=�@WIUR�d NUN ��P?5>SIU> IUT Z � IUBxV#?*R�IEV�@W? N IU^y=7B � IL> Z ?M:GJI �Oi d�=7T . ?5@WBC=h^eI���=7> IU^ N =7T�=7R�?�=�d�> Z =7BC=��>�?hR�I�9��5���89YF5�h=�BC=7>�IU@A=�R�I= i � T � ! =�@]T�=7^sV�@Wd�IUp�=7^%:;�=7@A=xTO? i @A=7@s^eIUV�=7@A=7@jIUT�?5p�qrI Z ?�R�IUT�@WIU^ Z ?�R�I�T�= � BC= i IL>�^eIh@WIY=7T � ! =*d�>�=

^eI i BxIU> Z = N �%P?Q>*g�d�I Z � IU>�I N ?5Bx?kd�B�p�@A=7T!tQ=7TO?5@WIU^�R�IcV � ��IUTOIU^�B�d�IU^ Z @WIY=7R�?5^�R�ITO?5^ N T �ONy~ ^fg�d�IEIUT2d�^ed�=7@ � ?�@WIY=7T � !�P?M:v]T @WIU^ed�T Z =7R�?*IU^wd�>�=h^eI N d�IU> N � =kR�I � B P= i IL>�IU^4p � >�=7@ � =7^]R�?5>�R�IzIUT2?5p�qrI Z ?

^eIUTOI NUN � ?Q>�=�R�?zIU^j@WIUV�@WIU^eIU> Z =7R�?�IU> p�T�=7> N ?�:��=*V#?5^ �ON �YP?Q> ) �6� X�+sR�IUT N IU> Z @W? � R�Iw^eIkV�@WIUR �ON IjV#?5@sBxIUR � ?zR�IkKMT Z @W?5^s�*=7TOBC=�> �R�I . ?5@WBC=Eg�d�I*IUT2?5p�qrI Z ?h>�?�^eIzV � IU@WR�=*IU> N =7^e?hR�I � @�R�IUBC=7^ � =�R�?z@ P=7V � R�?�:v]T(^eI i d � B � IL> Z ?*^eIEV�@WIU^eIU> Z =�IU>�d�>�=S^eI N d�IU> N � =zR�I*tQIU> Z =7>�=7^wIU>�IUT�=7B�{

p � IU> Z I�n = Z T�=7p�g�d�I�B�d�IU^ Z @A=oIUTw?5p�qrI Z ? IU> Bx?Yt � B � IL> Z ? �fT�=otQIU> Z =7>�= R�IZ @A= Ny~ IU?hX IUT8tQ=7TO?5@fIU^ Z � BC=7R�?zV#?5@wIUT8KMT Z @W?��*=7TOBC=�> N ?5Bx?�d�>�= N @Wd ! R�IU> Z @W?R�IET�=EtQIU> Z =7>�=hR�I Z @A= Ny~ IU?�:v]T V�@W?YXQI NLZ ?�IU^ Z#P=*?5@ i =7> � ! =�R�=JIU>�T�=*^ �Oi d � IU> Z I . ?5@WBC=�:v]T N =7V P� Z d�TO?h9kIU^sd�>�= � > Z @W?!R�d NUN ��P?Q>h^e?5p�@WIcIUT#?5p�qrI Z � tQ?*R�IUT#V�@W?YXQI NLZ ?��5T�=7^

� �

Page 10: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� � � ��� ���� �

R � K N d�T Z =7R�IU^�R�IJ@WIY=7T � ! =Y@�^ � ^ Z IUBC=7^�R�I Z @A= Ny~ IU?*V#?5@ N ?5BxV�d Z =7R�?5@A= ��T�=7^4d Z � T � {R�=7R�IU^�g�d�Ij^eI�TOIsV�d�IUR�IwR�=7@4=kd�>�?zR�I Z =7TOIU^�^ � ^ Z IUBC=�^��%=7^ P� N ?5Bx? N ?5BxIU> Z =7@ � ?Q^^e?5p�@WIE=7T i d�>�?5^ Z @A=7p�=Yqr?5^s@WIY=7T � ! =�R�?Q^4V#?5@ Z IU@ N IU@W?5^%:v]T N =7V P� Z d�TO?*F�V�@WIU^eIU> Z =�T�= Z IU?5@ P� =k^ed�p5X�= N IU> Z I*=7T8R � ^eI��>�?*R�IUT2^ � ^ Z IUBC=�:v]T N =7V P� Z d�TO?J\kV�@WIU^eIU> Z =JTO?5^�@WIU^ed�T Z =7R�?5^ X�^eIj@WIY=7T � ! =Y> N ?5BxIU> Z =7@ � ?Q^ ^e?5p�@WI

TO?5^s@WIU^ed�T Z =7R�?5^�?5p Z IU> � R�?5^s=7^ P� N ?5Bx?�BxIrqr?5@A=7^s=7T8V�@W? Z ? Z � V#?�:

�������� ����� �������������� � ����� ����������� ���� !������ ��"��#$��%�� �&����# ���'��%���� ������� �#(���)���% ���#$�� ���� +*,��"� �����.-/����#(��%10

{ 4E: �8: n : '� $���$�.0+3254 +3617 �*.,1�.��(��. � �(%&�0/ 7 +4�056.98 .' ��(��$;:����� ��#$<=� �?>,@@�A

BDCEGFDHJI�KMLON3FQPRISPRTULVPWEGFDXZY[ L\P]N_^�KMP]NJ`]KMLaISPcbMLd^=eOTgfSHSFQLV`]KWYeONihjP]NkP]blISP]fJLOXDFQLOTUP]N3FQeISPk^�KMP]NJ`]KMLkISP)bMLm^=eOTgfSHSFQLV`]KWYeONnISP]bgopNJEGFDK FDHSFQekq�LV`]KMeONJLOb(ISPdr�EGFDXQeOs�Y[ EGKM`WL Yt fSFDKM`WLvuCwbMPW`]FDXxYeONSKM`WL�y,CwTULOK b{z XQEDeObMLONJe"|�`WEDPW}�y K NJLVe"P]fiy Tg~�h�XQEDeObMLONJe"|�EGK Hiy �SHJLOfiy Tg~�Z^=LOFQPWI�XxYLOFDKM`We1ISP]b�ISP]fJLOXDFQLOTUP]N3FQe�ISP�^�KMP]NJ`]KML1ISP�bML(^=eOTgfSHSFQLV`]KWYeON�ISP]b�o�q�r t C

Page 11: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

���������� ��� �

� ��������� � � ����� �

����� ������������ !�v�� � ^ Z I*d�> � > Z IU@ PIU^wIU>�?5@WBxIzIU> IUT�BxIU@ N =7R�?�IU>oTO? i @A=7@sIUg�d � V#?5^wIUTOI NLZ @ P?5> �.N ?Q^

� > Z IUT �Oi IL> Z IU^��4X R�I�=7V�@WIU>�R � ! =YqrI�=7d Z ?5B P= Z �.N ? ' : � ?5^�^ � ^ Z IUBC=�^hR�I t � ^ ��P?5>� > Z IUT �Oi IL> Z IU^4>�?S^e?5>CT�=�IL� N IUV N ��P?Q> :ac>�?*R�I Z =7TOIU^ N ?5BxV#?5>�IU> Z IU^�B�d!X � BxV#?5@ Z =�> Z IfIU^�T�= ��=7p � T � R�=�R*R�IcV#?!R�IU@

^eI i d � @JX�@WI N ?5>�? N IU@J?5p�qrI Z ?5^EV#?5@*BxIUR � ?CR�I�^ � ^ Z IUBC=7^JR�Ixt � ^ ��P?Q> V#?5@ N ?5B�{V�d Z =7R�?5@A=��5IU> d�> =7B�p � IU> Z IjX�=�^eIY= N ?5> Z @W?5T�=7R�?z?�=7TOIY= Z ?Q@ � ?�:;(IU@W?E^eI i d � @ ?5p�qrI Z ?5^�V#?5@�BxIUR � ?cR�Iwd�>S^ � ^ Z IUBC=sR�Iwt � ^ ��P?5>zV#?5@ N ?5BxV�d Z =�{

R�?5@A=�>�?�IU^ N ?5^y=*^eIU> N � TOT�=�:� ? i @A=7@sIU^ Z = Z =7@WIY= N ?5> Z @A=7@ � ?S=STO?Sg�d�I�^eI N @WI P� =*IU>oTO?5^ � > �ON � ?Q^fR�I�T�= � >!{

Z IUT �Oi IU> N � =�=7@ Z � K N � =�T&��IU^z^ed�BC=7BxIU> Z IzR �/.WP� N � T �!V#IU^eIS=�g�d�ISIU^zd�>�= Z =7@WIY=�g�d�IV�=7^y= � >�=7R!tQIU@ Z � R�=zV#?5@�IUT2^eIU@ ��d�BC=7>�?�:

� ?5^ . = NLZ ?5@WIU^fg�d�IEBC=7^sR�I i @A=7R�=7>C= N d�=7TOg�d � IU@4^ � ^ Z IUBC=JR�I Z @A= Ny~ IU?�IU^wT�=� TOd�B � >�= N �%P?Q>SR�IET�=*IU^ N IU>�=�:;(?5@z? Z @W? T�=7R�?���TO?5^k^ � ^ Z IUBC=�^jg�d�I�^e?5> @W?5p�d�^ Z ?5^E=�T�= � TOd�B � >�= N �YP?Q> Z � IU>�R�IU>=o^eIU@�TOIU> Z ?5^EX V#?5@*IU>�R�IC>�? ^eI�d Z � T � ! =�> IU> =7V�T �ON = N � ?Q>�IL^kR�I Z � IUBxV#? @WIY=7TN @ P� Z �ON =�^U:v]> Z @WIET�=7^�=7V�T �ON = N � ?Q>�IL^4g�d�IE^eIkV�d�IUR�IU> R�IU@ � tQ=7@�R�Iz^ � ^ Z IUBC=7^]g�d�I Z @A= Ny~ {BDCwb�FSYP]XDTgK NJe�LOfSXQP]NJI�K#"WL%$�P�LOHSFQeOTdYLOFDKM`We PWE LV`]H'&NJLVISeUISP]b�K NJ}Ob�YPWE)(9LV`+*SK NJP-, PWLOXDNSK NJ}

9

Page 12: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

F � �E; P� }Ja ��� 95:�0��c}34w�EGzaz�j�&0 P���

IY=7> ?5p�qrI Z ?5^xIU^ Z#P= IUTkR � ^eI��>�? R�I � > Z IU@ . = N IU^ � > Z IUT �Oi IU> Z IL^��4^ � ^ Z IUBC=7^hR�I ^eIL{i d�@ � R�=7R ��^ � ^ Z IUBC=�^ � > Z IU@A= NLZ � tQ?Q^���^ � ^ Z IUBC=7^�R�I R�I . IU>�^y=�B � T � Z =�@���^ � ^ Z IUBC=7^B�d�T Z � B�?�R�=�T.IL^��%^ � ^ Z IUBC=7^�R�IJBxIrqr?5@A=JIUR�d N = Z � tQ=�� � ^eI i d � B � IL> Z ?kR�IJV#IU@W^e?5>�=7^��^eI i d � B � IU> Z ?JR�IJV�d�V � T�=7^�� T�I N Z d�@A=jR�IJT�=7p � ?5^��YTOI NLZ d�@A=JR�IJTOIU> i d�=YqrI�V#?5@f^eI��>�=7^��� > Z IU@WV�@WI Z = N �YP?Q>SR�I i IU^ Z ?5^�� I ZeN :

��= Z =7@WIY=cg�d�Iw@WIU^ed�IUT�tQ?cIU>�IU^ Z I Z @A=7p�=Yqr?JIU^]IUT!R � ^eI��>�?�XhV�@W? i @A=7BC= N �%P?Q>kR�Id�>�V�@W? Z ? Z � V#?sIU>hn = Z T�=7pJg�d�Is^ �Oi d�I]?5p�qrI Z ?5^2R�Isd�>�=w^eI N d�IU> N � =�R�I � B P= i IL>�IL^%:ac>�?�R�IkTO?5^fV�@W?5p�TOIUBC=�^]g�d�Iz^eIkV�@WIU^eIU> Z =7@W?5>�X�^eIk@WIU^e?5T�t � IU@r?Q>�IU^sIUT8@WI . IL{

@WIU> Z I�=hT�=*^eI i BxIU> Z = N �%P?Q>SR�IUT2?5p�qrI Z ?�R�IUT2@WIU^ Z ?�R�IET�=*IU^ N IU>�=�:v]T Z ?!?5TOp#?Y��R�ISV�@W? N IU^y=7B � IL> Z ?SR�I � B P= i IL>�IU^wR�I�BC= Z T�=7p Z � IU>�I�B�d N ��=7^

N =7@WIU> N � =7^ X�^eIcR ��P?zIUTMBxIrqr?5@�d�^e?�R�IcT�=7^4@Wd Z � >�=7^]V�@W?5V#?5@ N � ?5>�=�R�=�^��UV�d�IU^sV�@W?Q{i @A=7BC=7@(@Wd Z � >�=7^4R�IcV�@W? N IU^y=7B � IL> Z ?cR�I � B P= i IU>�IL^(IU>Cn = Z T�=7p*IU^fV�@W? � � p ��Z � tQ?Z IUBxV#?5@A=7T�BSIU> Z IQ:ac>�?JR�IsTO?5^ V�d�> Z ?5^�B P=7^ � BxV#?5@ Z =�> Z IU^#g�d�Ij=7g�d P� ^eIf@WIU^ed�IUT�tQIU>EIU^�T�= . ?5@WBC=

IU> g�d�I*^eIzR�IUp#IzBx?!R�IUT�=�@4d�>�^ � ^ Z IUBC=kR�I N ?5> Z @W?5T,:v]T�^ � ^ Z IUBC=ER�I N ?5> Z @W?5T2g�d�Ih^eI � BxV�TOIUBSIU> Z =E^eI*IU> N =7@ i =�R�I*KMT Z @A=7@wT�= � > . ?5@r{BC= N ��P?Q>x?5p�^eIU@rtQ=7R�=�X�R�I�IU^ Z = . ?5@WBC=�BC=7> Z � IU>�IJd�> ^eI i d � B � IL> Z ?�= N IUV Z =7p�TOIR�IUT2?5p�qrI Z ?�:ac>�?oR�I�TO?5^z?5p�qrI Z � tQ?5^zg�d�Ix^eIxV�d�^ � IU@W?5> N ?5Bx?C@WIUg�d � ^ ��Z ?5^z=7T � > �ON � ?�R�IUT

V�@W?YXQI NLZ ? . d�IEIUT2d�^e?SR�IEIUg�d � V#? N ?5B Pd�>SIU> d�> =7B�p � IU> Z IcR�I�;f�k:M��IEd Z � T � !�P?d�>�= N ?5BxV�d Z =7R�?5@A=z;f� N ?5BxV�= Z � p�T.I�XCd�>�= N�P=7BC=7@A=���IUp :

����� ��� � �������� ��� ����� �������������v]TM^ � ^ Z IUBC=�R�IUp#IU@ P=cIU^ Z =7@ ��=7p � T �.Z =�R�?wV�=7@A=k^eI i d � @(?5p�qrI Z ?5^�IU>Sd�>�=z^eI N d�IU> N � =R�I � B P= i IL>�IU^4p�=Yqr? N � IU@ Z =7^f@WIU^ Z @ �ONUN � ?Q>�IU^f= N IUV Z =7p�TOIU^U:�c?�^eI ��=h^eI��>�=7T�=�R�?z>�=7R�=�= N IU@ N =ER�IUT Z =7BC=��>�?kX N ?5TO?5@4R�IET�=7^ � B P= i IL>�IL^��

=7^ P� N ?5Bx? Z =7BxV#? N ?�IUTwB P� > � BS?�R�I � B P= i IL>�IU^�= V�@W? N IU^y=7@*V#?5@Sd�> � R�=7RlR�IZ � IUBxV#?�:v]TQ^ � ^ Z IUBC=]g�d�I���I4R�IU^y=7@W@W?5TOT�=�R�?4V#?5^eIUI]=7T i d�>�=7^M@WIU^ Z @ �ONUN � ?7>�IU^�R�I Z =7BC=��>�?

X N ?5TO?5@4V#IU@W?S^e?5TO? N ?5>�IUT(= .QP=7>xR�Ih= i � T � ! =�@]T�=7^sV�@Wd�IUp�=7^%:;]d�IU^ Z ?kg�d�IwIUT�^ � ^ Z IUBC=f>�?zIU^�R�IUBC=7^ � =�R�? i @A=7>�R�I��%TO? ��IwV�=7@ Z �ON � ?Q>�=�R�?sIU>

R�?5^kB P?5R�d�TO?5^�V�@ � > N � V�=7T�IU^��!d�>�?�g�d�I N�P=7T N d�T�=*T�=7^JV�@WIUR �ONUN � ?Q>�IU^ N ?5> IUT�KMT Z @W?�*=7TOBC=�>hXC? Z @W?�g�d�I*TOIUIJT�=7^ � B P= i IL>�IU^]X ��= N Ikd�^e?SR�IUT2B P?5R�d�TO?�=7> Z IU@ � ?5@U:GJI N d�=7TOg�d � IU@ . ?5@WBC=EIU^jV#?5^ � p�TOIw@WIUR � ^eI��>�=7@WT.?*=hIU^ Z =7^j=7T Z d�@A=7^%:

Page 13: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

������� � � ������ �� \���� � ��� � ��� � ���v]T8V�@W?5V P?5^ ��Z ?kR�IUT#V�@W?YXQI NLZ ?�IU^sIUT#R�IkR�IU^y=7@W@W?5TOT�=�@�d�>CV�@W? Z ? Z � V#?zg�d�IzV#IU@WB ��Z =IU> Z IU>�R�IU@�TO?5^ . d�>�R�=7BxIU> Z ?5^fR�IUT Z @A= Ny~ IU?�R�IE?5p�qrI Z ?5^s?�@W?5^ Z @W?5^%:CP=7^ �ON =7BSIU> Z I2T�=fT ��Z IU@A= Z d�@A=�R �ONLZ =�g�d�I�^eI4@WIUg�d � IU@WI�>�?j^e?5TO?�V�@W? N IU^y=7B � IU> Z ?

R�I � B P= i IU>�IL^�^ � >�? Z =7B�p � IU> Z IU?5@ P� =4R�I N ?5> Z @W?5TYV�=7@A=fBC=7> Z IU>�IU@�d�>*^eI i d � B � IL> Z ?= N IUV Z =7p�TOI�R�IUT�?5p�qrI Z ?�:(v]^ Z IsV�@W? Z ? Z � V#?�IU^ . = N � TOBSIU> Z I�IL� Z IU>�R � p�TOI�=cTOIU> i d�=YqrI� ?hT�=ktQ=7@ � =7> Z Ic� ��� �QV�d�IU^sT�=zBC=UXQ?5@ N =7> Z � R�=7R�R�I N�P?5R �Oi ?J@WI N =7IjIU>xT�= � B�{V�TOIUBxIU> Z = N �%P?Q>xR�IUT(KMT Z @W?��*=7TOBC=�> : ��=SV�=7@ Z I*g�d�I N ?5@W@WIU^eV#?5>�R�I*=SIUT N�P?5R �Oi ?R�IxV�@W? N IU^y=7B � IU> Z ?xR�I � B P= i IL>�IU^cIU^ Z ?5BC=7R�=xR�IUT Z ?!?5TOp#?Y��R�IxBC= Z T�=7p�X�IU^=7d Z ?!IL��V�T �ON = Z � tQ=!:v]TzR�IU^y=7@W@W?5TOT�? R�I d�>|V�@W? Z ? Z � V#? IU> BC= Z T�=7p Z � IU>�IoBC=7^�tQIU> Z =Yqe=7^�g�d�I

R�IU^WtQIU> Z =Yqe=7^%:;(?5@�IrqrIUBxV�TO?�� .QP= N � T�BSIU> Z I4^eIjV�d�IUR�IU>�=7R���IU@ � @�BxIrqr?5@A=7^��LXSd Z � T � ! =�@ T�=7^ . = N � T � {R�=7R�IU^�g�d�IwV�@W?YtQIUIU>�TO?5^ Z ?!?5TOp#?Y�c^ � >*>�I N IU^ � R�=7RzR�I�V�@W? i @A=7BC=�@8TO?5^ N ?5> N IUV Z ?5^R�I�=7T Z ?�> � tQIUT8>�I N IU^y=7@ � ?5^U:��= Pd�> �ON =zR�IU^WtQIU> Z =Yqe=�g�d�I*V�d�R�IE?5p�^eIU@rtQ=7@fIU^wIUT Z � IUBxV#?*g�d�I*TOTOILtQ=kV�@W? N IU^y=7@T�=7^ � B P= i IL>�IU^��QR�I . ?5@WBC=*g�d�I�IU>�BC= Z T�=7p�IUT ^ � ^ Z IUBC=z>�?�IU^cR�I Z � IUBxV#?�@WIY=7T �V#IU@W? PIU^ Z IJV�@W? Z ? Z � V#?*IU^wV#?5^ � p�TOIjIrqrI N d Z =7@WTO?EIU> Z � IUBxV#?*@WIY=7T�:

����� ������ � � �� ����� ��� �� � � � � ��� � �� � � � �������������v]^ Z IjV�@W? Z ? Z � V#?c^eIjR � @ �Oi I�= Z ?!R�?�=7g�d�IUT�IU^ Z d�R � =7> Z IfR�Iz� � IU> N � =�^(R�IjT�=E�s?5B�{V�d Z = N ��P?5>hg�d�IkR�IU^eIUIJIL� Z IU>�R�IU@f^ed�^ N ?5>�? N � B � IU> Z ?Q^�R�IkV�@W? N IU^y=7B � IL> Z ?kIU^eV�=�{N � =7T�XCIUBxV�@WIU>�R�=�d�> N =7B � >�?EIU>�IUT P=7B�p ��Z ?*R�IUT�=7V�@WIU>�R � ! =YqrIJ=7d Z ?5B P= Z �.N ?EXt � ^ ��P?5>SV#?5@ N ?5BxV�d Z =7R�?5@A=��Y:v]T V�@W? i @A=7BC=kIU^ � > Z IU@A= NLZ � tQ?zX .QP= N � T�R�IEIrqrI N d Z =7@U:

}4=7B�p �UPIL>�IU^ .QP= N � T�R�IcBxIrqr?5@A=7@��Y>�?�?5p�^ Z =7> Z I��7T�= Z IU?5@ P� =jR�IcKMT Z @W?5^��*=7TOBC=�>*IU^d�> V#? N ?*V#IU^y=7R�=�:�v]>CIU^ Z Ik@WIUV#?5@ Z Ic>�?�^eI ��= N IJd�>�=�R�IU^ N @ � V N ��P?Q>hR�I Z ?!R�?hIUTZ @A=7^ . ?5>�R�?�R�IzKMT Z @A=7R�?S�*=7TOBC=�> :��I�IL��V�T �ON =xR�ICBC=7>�IU@A= i TO?5p�=7T�IU>lg�d�I N ?5>�^ � ^ Z IhT�=CtQIU@W^ ��P?5> IL� Z IU>�R � R�=CR�IUTKMT Z @W?�X N ?5Bx?�^eI ��= � BxV�TOIUBSIU> Z =�R�?zIU> BC= Z T�=7p :��= N ?5BxV�@WIU>�^ �YP?Q> Z ? Z =7T2R�IUT2KMT Z @A=7R�?��*=7TOBC=�>�@WIUg�d � IU@WI*=7T BxIU>�?5^�T�=�TOI NLZ d�@A=� (9LOFDbMLO�1FDKMP]NJPHSNJL�EDeO�SXQP=`WLOXQ}VL+ISP LOb3TUP]NJeVE��3z �Oh ujbMeVE,XQPWEGHSb FQLVISeVE�eO�SFQP]NSKMISeVE K NJI�KM`WLON

! HJPjEDP�fSHJPWISP�fSXQe"`WPWEDLOX P]b�"SH $�egISP$#3KMISPWe1ISP&%(' s XQLOTUPWE=f�eOX+EDPW}OHSNJISe) (9LV`+*SK NJP$#3KMEGKMeONiy

Page 14: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

` � �E; P� }Ja ��� 95:�0��c}34w�EGzaz�j�&0 P���

R�ICm N =7V P� Z d�TO?Q^cR�IxT�=�T ��Z IU@A= Z d�@A= N ��Z =7R�=�X�TO?5^*=7@ ZYP� N d�T�?Q^ �Oi d�=7TOBSIL> Z I�BxIU>!{N � ?5>�=7R�?5^%:;(IU@W?EIU^eV#IU@W?kg�d�IjT�= . ?5@WBC=wIU>Sg�d�IcR�IU^ i TO? ! ?wIUT!^ � ^ Z IUBC=�R � > P=7B ��N ?wg�d�IcBx?!R!{IUT�= =7T�Bx?Yt � B � IL> Z ?�R�I�?5p�qrI Z ?5^S=UX�d�R�I = d�> IU> Z IU>�R � B � IU> Z ?o=7d Z ?!R � R�= NLZ =N d�=7>�R�?�IUT2TOI NLZ ?5@�^eIzIU> . @WIU> Z I*=�IU^e?5^wm N =7V P� Z d�TO?Q^4R�IET�= N ��Z =�:

����� � � � ��� �� � � ������� ���-�� � � ��� � ��

v]^ Z =x^eI NUN ��P?Q> Z � IU>�I N ?5Bx?S?5p�qrI Z � tQ?xR�=7@cd�> V�=7>�?5@A=7BC=�R�IhT�= N ?5@W@ � IU> Z I*= N {Z d�=7T2g�d�IE^eIzIUBxV�TOIY=zIU> IUT Z @A= Ny~ IU?�R�IE?5p�qrI Z ?5^%:� Irqr?5^*R�I�@WIUV�@WIU^eIU> Z =7@EIUT N =7B � >�? N ?5@W@WI NLZ ? V�=7@A=�@WIU^e?5T�tQIU@zIUTsV�@W?5p�TOIUBC=�R�IZ @A= Ny~ IU?ER�IJ?5p�qrI Z ?5^�� Z � IU>�I N ?5Bx?k?5p�qrI Z � tQ?EV�@WIU^eIU> Z =7@�=7TMTOI NLZ ?5@�R � ^ Z � > Z ?5^�IU>!{. ?!g�d�IkXCT�= N ?5BxV�TOIrq � R�=�RSg�d�I N ?5>�TOTOILtQ=kIUT(=7V#I i =7@W^eIJ=S=7T i d�>�?*R�I PIU^ Z ?5^%:v]>oTO?�V#IU@W^e?5>�=7T N @WIU?Sg�d�I*d�>�=�V�@ � BxIU@A=�=7V�@W?Y� � BC= N �%P?Q>�=7R�I N d�=7R�=*IU^jT�=�g�d�I^ �Oi ?�IU> IU^ Z IkV�@W?YXQI NLZ ?�V#?5@�R � tQIU@W^y=7^s@A= ! ?5>�IU^%:;]@ � BxIU@W?*BC=7> Z � IU>�IJ=7T2V�@W?YXQI NLZ ?hXC?5p�qrI Z � tQ?�BC=7>�Irqe=7p�TOIL^%:��= Z IU?5@ P� =Sg�d�I�^ed�p5X�= N IxIU^�BC=7>�Irqe=7p�T�I*IU> d�> ^eIUBxIU^ Z @WIhR�I Z =7T . ?5@WBC=�g�d�I>�?S^eIzV � IU@WR�IJT�=Et � ^ ��P?5>SR�IUT8?5p�qrI Z � tQ?h^ � IUT2V�@W?YXQI NLZ ?hIU^wR�IU^y=7@W@W?5TOT�=�R�?zV#?5@�d�>IU^ Z d�R � =7> Z IcIUBxV�@WIU>�R�IUR�?5@U:��I i d�>�R�?�� N ?5Bx? N ?5>�^eI N d�IU> N � =jR�IkTO?h=7> Z IU@ � ?5@���TO?5^4IU> . ?!g�d�IU^fg�d�Ik^eI N ?5BxIU>!{Z =7> = N ?5> Z � >�d�= N �YP?Q>x@WIUg�d � IU@WIU> R�I N ?5>�? N � B � IL> Z ?Q^sBC= Z IUB P= Z �.N ?7^sg�d�Ih>�?�^eI=7R�g�d � IU@WIU>�IU>�d�>�=zT �ON IU> N � = Z d�@A=sd�> � R � ^ N � V�T � >�=�@ � = �L^ � >�?Eg�d�IjIU^4IUT!@WIU^ed�T Z =7R�?R�I Z @A=7p�=Yqr?hIU> IUg�d � V#?5^sB�d�T Z � R � ^ N � V�T � >�=�@ � ?7^%:

� ��� ������������������! #"�$ � �&% ���'� $( � �*) ���+� "��,$( � �.-"�$(/0� �1� " ��2���� %�� �

v]^ Z I Z @A=7p�=Yqr?sV�@WIU^eIU> Z =fd�>EBC=7@ N ?fR�I Z @A=7p�=Yqr?�B�d�T Z � R � BSIU>�^ � ?Q>�=�T&�eR � ^ N @WI Z � ! =�R�?Xl=7R�=7V Z � tQ? V�=7@A= IU^ Z � BC=7@cIUTfBx?Yt � B � IL> Z ?�R�I�?5p�qrI Z ?5^ER�Ixd�>�=oV�@W?YXQI NUN ��P?5>^ed N IU^ � tQ=zR�I N =7@A= NLZ IU@ P� ^ Z �.N =�^4IL� Z @A= P� R�=7^4R�I � B P= i IL>�IU^U:v]TjBx?!R�IUTO? R�IUT�Bx?Yt � B � IU> Z ? N ?5>�^ � ^ Z ISR�I d�> N ?5>�qrd�> Z ? R�I I N d�= N � ?5>�IU^

R �/. IU@WIU> N � =�T.IU^ N ?5>kV�=7@ P=7BxI Z @W?Q^�g�d�I�^eI]IU^ Z � BC=7>w@WI N d�@W^ � tQ=7B�IU> Z I2R�I4T�=fI N d�= N ��P?Q>R�Io?5p�^eIU@rtQ= N ��P?Q>�>�?�T � >�IY=7T :���= R � BxIU>�^ ��P?Q>�R�IUTjBx?!R�IUTO? N ?5@W@WIU^eV#?5>�R�I N ?5>T�= ?5@ �Oi � >�=�T4R�IUTjBx?!R�IUTO?�IU^eV�= N � =7Tf= Pd�>l>�?�V�@W?YXQI NLZ =7R�?��4R�I Z =7T . ?5@WBC=�g�d�IV#IU@WB ��Z IzT�= N ?5BxV#IU>�^y= N �%P?Q>CR�I N =7@A= NLZ IU@ P� ^ Z �.N =�^�R�I�V�@W?YXQI NUN ��P?5> N ?5Bx?�^e?5> IUTBx?Yt � B � IU> Z ?zR�IUT N ?5> Z @W?5T�R�I*T�= N�P=7BC=7@A=!:

Page 15: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

��������� ������� � ��� ������������ a� ���� � m� �� � ��� ����� $�� � ��)�� ��� � "�$ 2 � � �0������! �� %�$(/0� ��� " � " � �!

� � / ��� � ��� $ �v]>EIU^ Z I Z @A=7p�=Yqr? � �%Bx?!R�IUTO?Q^�R�I N ?5> Z ?5@W>�?w= NLZ � tQ?5^�p�=7^y=7R�?5^�IU>zKMT Z @W?5^2�*=7TOBC=�>^eI�d Z � T � ! =Y>oV�=7@A= Z @A= Ny~ IU? R�Ix?5p�qrI Z ?5^z>�?o@ P� i � R�?Q^jIU>�d�>�R�?5B � > � ?SIU^eV�= N � ?Q{Z IUBxV#?5@A=7T N ?5B�p � >�=7R�?M:4v]^ Z =x=7V�@W?Y� � BC= N �%P?Q>�IUBxV�TOIY=EBx?!R�IUTO?Q^fR�I�BxIUR �ON �YP?Q>R�I i @A=7R � IU> Z IU^ V#? Z IU> N � =7T�IL^ IU>�T�=7^ � B P= i IL>�IU^8X�R�I��Md�qr? P?5V Z �ON ?]qrd�> Z ? N ?5>�TO?5^N ?5> Z ?5@W>�?5^ N ?5Bx?SBxIUR �ON � ?Q>�IL^sR�IUT�^ � ^ Z IUBC=!:���Ihd Z � T � ! =EIUT��Md�qr? P?5V Z �ON ?hV�=7@A=TO? i @A=7@�@W?5p�d�^ Z IU^ N ?5> Z @A=E? N TOd�^ � ?7>�IU^�X�@Wd � R�?�:�GJIkIU^ Z = . ?5@WBC=zIUT8^ � ^ Z IUBC=cR�IL{Z I NLZ =�X @WI N ��= ! =SBxIUR �ON � ?Q>�IL^wg�d�I�>�?�^eIY=7> N ?5>�^ � ^ Z IU> Z IU^ N ?5> IU^ Z � BC= N � ?Q>�IU^V�@WILt � =7^sR�IUT2Bx?Yt � B � IU> Z ?zIU> T�=�IU^ N IU>�=�:

� ���� ��� ����� $�� � �! � � "�$(/0� � � �! �$�� � �! �0��� ���v]>�IU^ Z IcIU> . ?!g�d�Ic^eIcV�@W?5V#?5>�Ijd�>�Bx?!R�IUTO? N ?5>�IL� � ?5> � ^ Z =jV�=7@A=z^eI i d � @]@W?5^ Z @W?5^g�d�I�BC=7> � V�d�T�=hT�=7^J?5@ � IU> Z = N � ?Q>�IU^wR�I�d�>�= N�P=7BC=7@A=�X @WIY=7T � ! =S= N IU@ N =7B � IU> Z ?^eI ijPd�>c^eIY=s>�I N IU^y=7@ � ?��eR�I Z =7T . ?5@WBC=�g�d�I�^eI�BC=7> Z IU> i =4IUT7@W?5^ Z @W?�R�I�T�=sV#IU@W^e?5>�=TO? N =7T � ! =�R�?EIU> N d�=7TOg�d � IU@4Bx?5BxIU> Z ?EIU> g�d�I*^eIz@WI N � p#IJIUT��Md�qr?�R�Izt � R�IU?�:v]^ Z I�^ � ^ Z IUBC=�?5V#IU@A=xIU> Z � IUBxV#?�@WIY=7T�X ^eI�=7R�=7V Z =C=CR �/. IU@WIU> Z IU^ N ?5>�R � {

N � ?5>�IU^fR�IETOd ! ��d�> . = NLZ ?5@fg�d�Ih= . I NLZ =�=hT�=�BC=UXQ?5@ P� =JR�IETO?5^s^ � ^ Z IUBC=�^%:v]^ Z IE^ � ^ Z IUBC=kIU^wV�=7@ Z IzR�IEd�>�^ � ^ Z IUBC=kB�d�T Z � B�?�R�=�T N ?5BxV�TOIrqr?M:

� �� � ! � ��2���� ��������! � ��)�� ��� � � �"� ����� $�� "�$ ��)$# $ � � ����+���0� ��/ � "�� � � �+� �"�� � $ 2%� $ � $( � ����� �! �$ � ) ���+� " ���$( % �0��� ���

v]^ Z I Z @A=7p�=Yqr?kIUBxV�TOIY=wd�>�=*=7V�@W?Y� � BC= N �YP?Q>Ep�=7^y=7R�=cIU>St � ^ Z =7^�V�=7@A=JIUT Z @A= Ny~ IU?R�I ?5p�qrI Z ?5^�@ P� i � R�?Q^*X =7@ Z �ON d�T�=�R�?Q^%: v]T Z @A=7p�=Yqr?�^eI N ?5>�^ Z @Wd!XQI�^e?5p�@WI Z @A=�{p�=Yqr?5^EV�@WILt � ?5^ER�IxIU^eV�= N � ?5^JV�@W?5V � ?5^ � Z�PI N > �ON =7^JR�IxIU^ Z � BC= N �%P?Q>o@W?5p�d�^ Z =7^zXIU^ Z � BC= N �%P?Q>SR�I�Md�qr? P?5V Z �ON ?*V�=7@A=7BxI Z @ �"! =�R�?�:v]^ Z Ih^ � ^ Z IUBC=E@WI N ?5>�^ Z @Wd!XQI*T�=7^ � B P= i IU>�IL^sIU>oIUT�^ � ^ Z IUBC=*V�@W?5V � ?Sp�=7^y=7R�=

IU> B P� > � BS?Q^ N d�=7R�@A=7R�?5^%:v]^ Z IEIU^wd�>�^ � ^ Z IUBC=kg�d�IE>�?�^eIzIrqrI N d Z =�IU> Z � IUBxV#?E@WIY=7T,:

&('3P *JL�fSXQP]s P]XDKMISelTULON3FQP]NJP]XwbMLVE=s XxYLVEDPWE=P]NUbMP]NJ}OHJL�K NJ}ObMPWEDL5`]HJLONJISe�bML�FDXQLVI�HJ`W`]KWYeONUTdYLVE! HJPlK NSs eOXDTULOFDK #OL EDPWL5`WeONSs HJEDL�y

Page 16: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� � �E; P� }Ja ��� 95:�0��c}34w�EGzaz�j�&0 P���

Page 17: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

���������� ��� �

� ��� � �� � ������ ��� �������

v]>xIU^ Z =k^eI NUN ��P?Q>�=7>�=7T � ! ?jTO?5^4=7^eV#I NLZ ?5^ Z I P?5@ �ON ?Q^(>�I N IU^y=7@ � ?Q^(V�=7@A=kR�IU^y=7@W@W?5TOTO=�@IUT2V�@W? Z ? Z � V#?*R�I Z @A= Ny~ IU?�:��I ��= N IU> N ?5BxIU> Z =7@ � ?7^�= N IU@ N =jR�IjT�=7^�^y=7T � R�=7^ g�d�I N =7R�=JB P?5R�d�TO?wR�IUp#IU@ P� =@WIY=7T � ! =Y@4V�=7@A=�g�d�I*IUT2^ � ^ Z IUBC=J^eIY=hIU> ^ed Z ? Z =7T � R�=�RSTO?�BC=7^sIU^ Z =7p�TOIjV#?5^ � p�TOI5:��IztQIU@ P=hV#?5@�g�d�IEIUT Z @A= Ny~ IU?�@WIUg�d � IU@WIcKMT Z @A=7R�?S�*=7TOBC=Y>�XC^eIktQIU@ P= N ?5Bx?

IU> V�@ � > N � V � ?E^eIzV�d�IUR�IE@WIY=7T � ! =�@ Z @A= Ny~ IU?hV#?5@ Z =7BC=��>�?ER�IUT2?5p�qrI Z ?�:

� ��� � ��� � ��� ������ � � ��� � � � ����� � ��� � �� ��� � � � ��� � � ���

v]T Z @A= Ny~ IU?�@WIUg�d � IU@WIJR�IET�=*^eIUV�=7@A= N ��P?Q>SR�IUT2?5p�qrI Z ?�R�IUT . ?5>�R�?�g�d�I*TO?h@W?!R�IY=�:v]T�?5p�qrI Z ?x^eI�IU> N d�IU> Z @A=S@WIUV�@WIU^eIU> Z =7R�?�IU> d�>�=�@WI i ��P?5>�g�d�IhIU^kV�=7@ Z I*R�I

T�= � BC= i IU>�F5Gh:v]^ N T�=7@W? g�d�I�T�= � BC= i IU> ^eI V�d�IUR�I�Bx?!R�IUT�=�@ N ?5Bx? d�>�= . d�> N ��P?5>� " )��� � + �MR�?5>�R�I � X � N ?5@W@WIU^eV#?5>�R�IU>�=xT�=7^ N ?!?5@WR�IU>�=7R�=7^sIU^eV�= N � =7T�IL^fX��xIU^

T�= � > Z IU>�^ � R�=7R�R�IUT2V � ��IUT8IU> N d�IU^ Z ��P?5> :v]>lIU^ Z I�^eIU> Z � R�?��(=���^eIxTOISV�d�IUR�IU>�=7V�T �ON =�@ N d�=7TOg�d � IU@cV�@W?5V � IUR�=7R g�d�I

^eIY=�=7R�B � ^ � p�T.IwIU> d�>�R�?5B � > � ?zR � ^ N @WI Z ?�:��=c?5V#IU@A= N ��P?Q>zR�Iw^eI i BxIU> Z = N �%P?Q>k^eIwp�=7^y=jR � @WI NLZ =7BxIU> Z I]IU>�T�= � > Z IU>�^ � R�=7R

R�I N =7R�=�V � ��IUT�XCIU^ Z#P=*R�Ih= N d�IU@WR�?S=hd�>�d�B�p�@A=7T#IU^ Z =7p�TOI N � R�?M:�s?5> N @WI Z =7BxIU> Z I��yV�=7@A=w^eIUV�=7@A=7@8IUT�?5p�qrI Z ?cR�IUT�@WIU^ Z ?jR�IsT�=�IU^ N IU>�=�IU^ Z =7p�TOIL{

Page 18: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� � �E; P� }Ja ��� F�:�}Jvf� 4 P� � ��a �� �k�wv �c}JvN IUBx?5^fd�>�V�@WIUR �ON =7R�?*R�IET�= . ?5@WBC=

��������� )��� � + "� 9 ^ �� �������� � �� ���� �� IU> ? Z @W? N =7^e?

v]>�IU^ Z I N =7^e?h@WI i ��P?5> N ?5> Z � IU>�Ik=7T8?5p�qrI Z ?�R�I � > Z IU@ PIU^%:GJISBC=7>�IU@A= N @Wd�R�=�V#?!R�IUBx?5^jR�I N � @Jg�d�ISIU^ Z =�@WI i ��P?Q> IU^EBxIU@A=7BxIU> Z IzIUT4?5p!{qrI Z ?�R�I � > Z IU@ PIU^ �2V#IU@W? =x> � tQIUT�V � ��IUT�V�d�IUR�I�>�?C^eIU@WTO? N ?5Bx?StQIU@WIUBx?5^jIU> T�=^ �Oi d � IU> Z Iw^eI NUN ��P?5> :v]Tjd�B�p�@A=7TsIU^Sd�>�= tQ=7TO?5@*g�d�I�IU> >�d�IU^ Z @W? V�@W? Z ? Z � V#?�^eI ��= N =7T N d�T�=�R�?

@WIY=7T � ! =Y>�R�?�d�> B�d�IU^ Z @WIU?�R�I�T�= � > Z IU>�^ � R�=7R�R�I N ?5TO?5@sIU>oV � ��IUTOIU^�g�d�Ih^eI�IU>!{N d�IU> Z @A=7>C^e?5p�@WIkIUT2?5p�qrI Z ?�R�I � > Z IU@ PIU^%:v]^ Z IJB�d�IU^ Z @WIU?EIU^sV�@W?5V#?5@ N � ?Q>�=�R�?JV#?5@fT�=z^eIU@ � IwR�I N T �ONy~ ^]g�d�IkIUT#d�^ed�=7@ � ?

R�=S=7T2?5p�qrI Z ?�R�I � > Z IU@ PIU^fd�>�=htQI ! g�d�I PIU^ Z Ik^eIzV�@WIU^eIU> Z =�IU> T�=�V�=7> Z =7TOT�=!:GJIjIU^ Z I N ?5>�qrd�> Z ?zR�I � > Z IU>�^ � R�=7R�IU^(g�d�Ic^eIj?5p Z � IU>�If^eIjIU> N d�IU> Z @A=kIUT�B P� > � BS?XCB P=�� � B�?kXC^eIEd Z � T � ! =�> N ?5Bx? ������ X ���� � @WIU^eV#I NLZ � tQ=7B�IL> Z I5:�kp�^eIU@rtQIkg�d�IEIU^ Z =�= NUN ��P?Q>x=�^eIUV�=7@A=7R�?�IUT2?5p�qrI Z ?hR�Iz>�d�IU^ Z @W? � > Z IU@ PIU^�R�IUT

@WIU^ Z ?cR�IsT�=wIU^ N IU>�=��UR�I ��I N ��?��YT�=�IU^ N IU>�=c=jR�IU^y=7V�=7@WI N � R�?fV�d�IU^�TO?5^2tQ=7TO?5@WIU^8R�IV � ��IUT!g�d�Ic>�?EV#IU@ Z IU>�IU> N IU>S=k>�d�IU^ Z ?E?5p�qrI Z ?zR�I � > Z IU@ PIU^�^eI&��=7>S^eIUTOI NUN � ?Q>�=�R�?= � > Z IU>�^ � R�=7R�� ' :� Z @A= . ?5@WBC=kR�IctQIU@ N d P=7T � > Z IU>�^ � R�=7RhR�IkV � ��IUTMIU^w=7R�I N d�=7R�=kV�=7@A=ET�=z^eI i {BxIU> Z = N ��P?Q> N ?5>�^ � ^ Z IjIU> ?5p�^eIU@rtQ=7@fIUT��'7 $�%&.��0+4�0/#�cR�IEd�>�= � BC= i IU> :v]T(� � ^ Z ? i @A=7Bx=�B�d�IU^ Z @A=JT�=7^ � > Z IU>�^ � R�=7R�IU^(@WIUT�= Z � tQ=�^(?h=7p�^e?5TOd Z =7^(R�IcTO?5^]V � ��{IUTOIU^sR�IET�= � B P= i IU> :v]> d�> � � ^ Z ? i @A=7Bx=�^eI V�d�IUR�IU> ?5p�^eIU@rtQ=7@*V#?5@�IrqrIUBxV�TO?�B P=�� � B�?7^Eg�d�I�R�IL{^ N @ � p#IU> T�= N =7> Z � R�=7RoR�I�V � ��IUTOIU^cg�d�I�V#IU@ Z IU>�I N IU>�= N =7R�=x@WI i ��P?5> :�� � > IUB�{p�=7@ i ?�IU^ Z =w=7V�@W?Y� � BC= N �YP?Q>jIU^(^e?5TO?sV�=7@A= N d�IU^ Z � ?5>�IU^�R�I�IU^ Z d�R � ?sXz>�?c^eIs=7V�T �ON =IU> Z � IUBxV#?*@WIY=7T,:

����� ! / ��� ��� � ��������! "�$ / � � $�� ����! "�$ � � $ ���$ �v]> T�=�^eI NUN ��P?Q>�=7> Z IU@ � ?5@wR�I N�P� =7B�?Q^�g�d�IST�=S@WI i ��P?5>CR�I � > Z IU@ PIU^c>�?C>�I N IU^y=7@ � =Y{BxIU> Z I N ?5@W@WIUV#?5>�R�I N ?5> IUT�?5p�qrI Z ?xR�I � > Z IU@ PIU^%:k;�=7@A=�tQIU@kIU^ Z ?�IU^z>�I N IU^y=7@ � ?� > Z @W?!R�d N � @4IUT N ?5> N IUV Z ?hR�IztQI N � >�R�=7R�X N ?5BxV#?5>�IU> Z IU^ N ?5>�I NLZ =7R�=7^%:BDCwb eO� $�P]FQe1EDPlL EDP]bMPW`W`]KMeONJLVISe�L � ! HJPj`WeOXDXQPWEGf�eONJISP�L! �� P]N$PWED`WLObML5ISPj}OXDKMEDPWE

Page 19: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

��� ��� �j��� ��� � ����� ����� � � ������ ����� ��� �� ��� � � � � ������

��� /�� /�� / � ������ (�����ac>�=�`Q{ tQI N � >�R�=7Rc^eI�R�ILKM>�I N ?5Bx?sTO?5^8V � ��IUTOIU^ ��?5@ � ! ?5> Z =7T.IL^�?jtQIU@ Z �ON =7TOIL^8=7R!X�=�{N IU> Z IU^j=7T2V � ��IUT8R�I � > Z IU@ PIU^%:ac>�= ��{ tQI N � >�R�=7Rl^eI R�ILKM>�I N ?5Bx? TO?5^SV � ��IUTOIU^�=7R!X�= N IU> Z IU^SIU> ^eIU> Z � R�?

��?5@ � ! ?5> Z =7T �stQIU@ Z �ON =7TfX R � = i ?5>�=7T : v]^xR�I N � @��sF�V � ��IUTOIU^�^e?5> ��{ tQI N � >�?5^�^ �

Z � IU>�IU>�IU> N ?5B Pd�>Sd�>�=�IU^eg�d � >�=�:

��� /�� /���� �������� ($ac> N =7B � >�?hR�IUT�V � ��IUT IU>�� ��� � � =7T(V � ��IUT(IU>�� � � � � IU^Jd�>�=x^eI N d�IU> N � =*R�IV � ��IUTOIU^�� ��� � � � � ' � ' D D D�� � ' � � Z =7T�g�d�ISIUT]V � ��IUT�IU> T�=�V#?5^ �ON �YP?Q> � ��� � ��IU^�tQI N � >�?hR�IUT V � ��IUT#IU> T�=�V#?5^ �ON �YP?Q>�� ����� ' � ��� ' V�=7@A=�� ��� � � � 95:v]>�IU^ Z Iz^eIU> Z � R�?��5d�>�`Q{ N =7B � >�?zd Z � T � ! =Jd�>�=�`Q{ tQI N � >�R�=7R :

��� /�� /�� � ��$8 (���Q"���'(�������ac>�= N ?5BxV#?5>�IU> Z I N ?5>�I NLZ =7R�=wIU^4`Q{ N ?5>�I NLZ =7R�=s^ � IUT N ?5>�qrd�> Z ?kR�IjV � ��IUTOIU^(g�d�IT�= . ?5@WBC=7>x^e?5> N ?5>�I NLZ =7R�?5^j:v]^ Z =7^�R�ILKM> �ON � ?5>�IU^�V#IU@WB ��Z IU>�R�I Z IU@WB � >�=7@ N ?5>SV#IU@ . I NLZ = N T�=7@ � R�=7R N d�=7TOIU^

N ?5BxV#?5>�IU> Z IU^ . ?5@WBC=7>C=h>�d�IU^ Z @W?S?5p�qrI Z ?hR�I � > Z IU@ PIU^%:v]^jR�IU^eIY=7p�TOIcg�d�IE^e?5TO?�^eIY=�d�>�= N ?5BxV#?5>�IU> Z IjV#?5@�@A= ! ?5>�IU^f?5p5t � =7^%:� � >�d�IU^ Z @W?x?5p�qrI Z ?�R�I � > Z IU@ PIU^sKM>�=7TOBxIU> Z IkIU^ Z#P= . ?5@WBC=7R�?hV#?5@jtQ=7@ � =7^�@WIL{

i � ?5>�IU^ N ?5>�I NLZ =7R�=7^ N ?5Bx?f@WIU^ed�T Z =7R�?fR�I�T�=s^eI i BxIU> Z = N �%P?Q> �AIUTOI i � BS?Q^MT�=f@WI i ��P?5>N ?5>�I NLZ =7R�=zB P=7^ i @A=7>�R�I5:� � > IUB�p�=7@ i ?��4IU^ Z Io>�?lIU^�>�d�IU^ Z @W? N =7^e?��fV�d�IU^�T�= @WI i ��P?5> R�I � > Z IU@ PIU^Sg�d�I?5p Z IU>�IUBx?5^E^eI p�=7^y= IU> IUT�B�d�IU^ Z @WIU?oXlIU^ Z ? V#IU@WB ��Z I�?5p Z IU>�IU@hd�>�= Pd�> �ON =@WI i ��P?5> :

���� � � 2 $ � � �"�� � ��) � $ / ������� ��$( ��=7^�?5V#IU@A= N � ?Q>�IU^ R�IU^ N @ ��Z =7^�=7> Z IU@ � ?5@WB�IU> Z I4^eIwTOTOILtQ=7>�= N =7p#? . = N � TOBSIU> Z I N ?5>IUT Z ?!?5TOp#?Y�hR�IEBC= Z T�=7p :v]>�V�=7@ Z �ON d�T�=7@���T�=�d�B�p�@A=7T � ! = N �%P?Q>S^eIzTOTOILtQ=*= N =7p#? N ?5>�T�=�T P� >�IY=

��������� " �������� ����� ����� )��� � + �� ���� ����+N fSK ~�P]b������ ��!#"%$'&�(*),+.-%/0-21gEDP I�KM`WP(`WeONJPW`]FQLVISe9L435��� ��!#"%$'&�(*),+.-%/0-21gEGK�*JLWuaHSN

`WLOTgK NJe1ISP6�$L73 ! HJPj`WeONJEGKMEGFQP+EDeObMegISPlfSK ~�P]bMPWE�89�4� ��!#"%$'&�(*),+.-%/0-21]y

Page 20: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

9Y� � �E; P� }Ja ��� F�:�}Jvf� 4 P� � ��a �� �k�wv �c}Jvv]^ Z ?EIU^4V#?5^ � p�TOI]X�=kg�d�IjIU^4d�>�=k?5V#IU@A= N ��P?Q> N ?5B Pd�>EIU>�BC= Z @ �ON IU^2XhV�d�IU^ Z ?

g�d�IfT�= � BC= i IL>cIU^ d�>�=jBC= Z @ � ! IU^ Z ?jIU^ � >�BxIUR � = Z ?�IU>*IUT�=7B�p � IU> Z I�R�I�n = Z T�=7p :GJI ��I N ��?���IU^ Z =�IU^jd�>�=h?5V#IU@A= N ��P?Q>Sg�d�IE>�?�IU^wV�=7@ Z IzR�IUT Z ?!?5TOp#?Y�8:v�� � ^ Z Izd�>�= . d�> N ��P?5>�R � ^eV#?5> � p�T�IwIU> IUT Z ?!?5TOp#?Y��g�d�IE@WIY=7T � ! =JTO?hB � ^eBx?M:��IER�ILKM>�IER�IET�=�^ �Oi d � IU> Z I . ?5@WBC=��

��������� "�� ������� ��) ����� ����� ������ ���� � +� � >lIUB�p�=7@ i ?��(IU^ Z = . d�> N ��P?5>�IU^�B P=7^ N ?5^ Z ?5^y=Cg�d�I T�=o?5V#IU@A= N ��P?Q> BC= Z @ �ON � =�TV�@WILt � =�:v]^ Z =7^CR�?5^�=7V�@W?Y� � BC= N � ?Q>�IL^S?5p Z � IU>�IU> d�>�=l@WI i ��P?5>�g�d�I�>�?�>�I N IU^y=7@ � =Y{

BxIU> Z I N ?5@W@WIU^eV#?5>�R�Ik=hd�>�=�@WI i ��P?5> N ?5>�I NLZ =7R�=�:;�=7@A=h?5p Z IU>�IU@sd�>�=�@WI i ��P?5> N ?5>�I NLZ =7R�=��7^eI ��= N Ikd�^e?SR�IET�= . d�> N ��P?5>����

��������� � � � �� � � � "��� � ��� ����� ) ��������� �� � � +� @ N ?5@W@WIU^eV#?5>�R�Ik=hT�= N ?!?5@WR�IU>�=7R�=J�CR�IETO?5^sV � ��IUTOIU^fR�IEB�d�IU^ Z @WIU?��� NEN ?5@W@WIU^eV#?5>�R�Ik=hT�= N ?!?5@WR�IU>�=7R�=kXCR�IETO?5^sV � ��IUTOIU^�R�IEB�d�IU^ Z @WIU?��� > N ?5@W@WIU^eV#?5>�R�I�=7T Z � V#?�R�I]tQI N � >�R�=7R � � " `��UR�ILKM> � @ P=4d�>�=�`Q{ N ?5>�I NLZ � t � R�=�R �� " ��d�>�=S��{ N ?5>�I NLZ � t � R�=�R :

v]^ Z =7^ ?5V#IU@A= N � ?Q>�IL^SR�=7> N ?5Bx? @WIU^ed�T Z =7R�?�d�>�=�^e?5T�=�@WI i ��P?Q> @WIUV�@WIU^eIU> Z =7R�=N ?5Bx? � B P= i IL>Sp � >�=7@A=Eg�d�I*R�ILKM>�I�=7T8?5p�qrI Z ?�R�I � > Z IU@ PIU^%:�s?5> IU^ Z ?�V#?!R�IUBx?5^w@WIY=7T � ! =�@sd�> Z @A= Ny~ IU? � > i IU>�d�?�V#IU@W?Cg�d�I�IU^zT�=Sp�=7^eI

R�IUT Z @A= Ny~ IU?�g�d�IEd Z � T � ! =cKMT Z @W?5^w�*=7TOBC=�> :

� ��� � ��� ��� � � � ��� ���� !������v]^ Z =hIU^wd�>�= . ?5@WBC=ER�I Z @A= Ny~ IY=7@sd�>�= � BC= i IL>�XCIUT2V�@ � > N � V � ?zIU^ Z @ � t � =7T,:v]> IU^ Z = =7V�@W?Y� � BC= N �%P?Q>�IUT . ? N ? ^eI N IU> Z @A= IU> IUTw?5p�qrI Z ? R�IoT�=oIU^ N IU>�=

N d!XQ? Z =7BC=��>�?C^eI�=7V�@W?Y� � BC= =7TsR�IUT�?5p�qrI Z ? ^eIUTOI NUN � ?Q>�=�R�?CIU>�^ed�p�^eI N d�IU> Z IU^� B P= i IL>�IU^U:v]^ Z ? ^eI�TO? i @A=�^ed�V#?5> � IU>�R�?lg�d�IlIUT*?5p�qrI Z ? >�? ^eI B�d�ILtQI�R�IUBC=7^ � =�R�?

@ P=7V � R�? X g�d�I V#?5@�IU>�R�IoV�d�IUR�IU> =7V�@W?Y� � BC=7@W^eI�T�= >�d�ILtQ= V#?5^ �ON ��P?Q>�R�I TO?5^V � ��IUTOIU^fR�IEB�d�IU^ Z @WIU?�V�=7@A=�V#?!R�IU@sd�B�p�@A=7T � ! =Y@4T�=*>�d�ILtQ= � B P= i IL> :

)��,LOXQL TULWu"eOX XQP]s P]XQP]NJ`]KMLj`WeONJEGHSb FQLOX�� �/eOX ''��

Page 21: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

���.��� ����� ����� � � ��� � ��� ������ 959

v]^ Z =o=7V�@W?Y� � BC= N �%P?Q>�R�=CTOd i =7@z= T�=C=7V�@W?Y� � BC= N �%P?Q>�g�d�Ixd�^y=7@WIUBx?5^cV�=7@A=N ?5@W@W?5p#?5@A=7@4IUT2R�IU^eIUBxV#I��>�?zR�IUT8KMT Z @W?��*=7TOBC=Y> :

�� ��� ��� ����� $�� 2 � � � $( �"� � � "�$}�@A= Ny~ IY=7@�=hd�>�?5p�qrI Z ?�V#?5@�BxIUR � ?*R�IE^ed N IU> Z @W? � R�I Z � IU>�IJR�?5^�tQIU> Z =Yqe=7^%:95: ��=�tQIU> Z =7>�=�R�I Z @A= Ny~ IU?C^eI�BC=7> Z � IU>�I .QP= N � T�BSIL> Z IER�IU> Z @W? R�IUT4?5p�qrI Z ?R�I � > Z IU@ PIU^%:

F�:j��IJV�d�IUR�IJd Z � T � ! =�@�IUT N IU> Z @W? � R�I N ?5Bx?zd�>�V�=7@ P=7BxI Z @W?JV�=7@A=zIU^ Z � BC=7@�T�=N =7T � R�=�RSR�IUT8KMT Z @W?S�*=7TOBC=�> :

� P? Z IU^eI4g�d�IwT�=7^]=7V�@W?Y� � BC= N � ?Q>�IL^2BC=7^(IUT�=7p#?5@A=7R�=�^2d Z � T � ! =�> N ?5@W@WIUT�= N �%P?Q>kV�=7@A=N ?5@W@W?5p#?5@A=7@4g�d�IEIU> @WIY=7T � R�=�RS^eIEIU^ Z#P=*^e?5p�@WIkIUT2?5p�qrI Z ?�R�I � > Z IU@ PIU^%:� � p � IU> IU^ N � IU@ Z ?Sg�d�IhIU^y=7^j^e?5>�^e?5TOd N � ?Q>�IU^fB P=7^wIUT�=7p#?5@A=�R�=�^��5^ed . @WIU>oR�I

T�=7^fB � ^eBC=�^4@A= ! ?5>�IU^fR�IzT�=7^sg�d�IE^ed . @WIU>CR � tQIU@W^y=7^�=7V�@W?Y� � BC= N � ?7>�IU^��5=�^y=7p#IU@��T�=EtQ=7@ � = N �YP?Q>SIU> � > Z IU>�^ � R�=7RSR�I � TOd�B � >�= N �%P?Q> :GJIh? Z @A= . ?5@WBC=h>�?�^eIhIU^ Z =7@ P� =E^eI i d�@W?�R�Ih^ � IUT�^eI i d � B � IL> Z ?S�*=7TOBC=�>x^eI

IU> N d�IU> Z @A=�^e?5p�@WIkIUT2?5p�qrI Z ?�R�I � > Z IU@ PIU^%:v]T N IU> Z @W? � R�ICIU> >�d�IU^ Z @A=�=7V�@W?Y� � BC= N �YP?Q>�^eI ��= N =7T N d�T�=�R�?�R�I BC=7>�IU@A=

p�d�@WR�= � :w;]d�IU^cIUT N =7T N d�TO?E^eI ijPd�>�^eI*R�IU^ N @ � p#I N ?5>�^ed�BxIJR�IUBC=7^ � =�R�? Z � IUBxV#?IU> IUT(=7B�p � IU> Z IJn = Z T�=7p :v]T N�P=7T N d�T�? Z @ � t � =7T�R�IUT N IU> Z @W? � R�IcIU^ Z#P=�R�=7R�?SV#?5@�T�=7^�V�@W?YXQI NUN � ?5>�IU^�R�IET�=

B P� > � Bx=zV#?5^ �ON ��P?Q>S�6�#B P=�� � B�=zV#?5^ �ON ��P?Q>��6��T�=�B P� > � B�=zV#?5^ �ON ��P?Q>�X���B P=�� � B�=V#?5^ �ON ��P?Q>�X5:GJI IU^ Z = . ?5@WBC= IUT N IU> Z @W? � R�I t � IU>�IoR�=7R�?lV#?5@�IUTJV�d�> Z ? BxIUR � ? R�I IU^ Z ?5^^eI i BxIU> Z ?5^%:(� � IU@ Z =7BxIL> Z IwIU^ Z ?h>�?�tQ=�= N ?5@W@WIU^eV#?5>�R�IU@ N ?5>�IUT N IU> Z @W? � R�IjR�IUT?5p�qrI Z ?*R�IcT�= � BC= i IU> �%V#IU@W?*IU^e?Et � IU>�IjR�=7R�?*V#?5@�IUT�B � ^eBx?cIU@W@W?5@ N ?5>�g�d�IJ^eIN =7T N d�T�=�T�=c^eI i BxIU> Z = N �%P?Q>��Y:]��?5>SIU@W@W?5@WIU^]IU>�V�@ � BxIU@A= � >�^ Z =7> N � = � >�ILt ��Z =7p�T�IL^%:

& �+NJLcs eOXDTULRISP$`WLObM`]HSbMLOX P]b `WP]N3FDXQeOKMISPUPWE1eO�SFQP]NSKMP]NJISe9bMLcXQP]fSXQPWEDP]N3FQLV`]KWYeON���� ������ISPUbML�K TULV}VP]Niycr fJLOXDFDK X1ISPUbML�K TULV}VP]N �����������hibMLcLO��`]KMEDL9ISP]b=`WP]N3FDXQeOKMISP1PWEGFxYL9ISLVISef�eOX����������� ���� �"!#�

���� ���� �y$� (&% -('� bMeONJ}OK FDHJIRISP]b "*)VYPWEGK TUe+���,������ ISP 8���!#"%$'&-�/. ),-(�%+ ! % ! h10�(*%

-('�bML/LO��`]KMEDL\ISP]b�`WP]N3FDXQe�ISP]b "*)VYPWEGK TUe2���3�������h �4% -('�aN�YHSTUP]XQeRTdYLO~3K TUe/ISP5���3�������y , LeOXQISP]NJLVISL PWE&67�8�9� �� ����: �";<�

� � � ��� : �y>= (*% -('� bML eOXQISP]NJLVISL ISP]bi`WP]N3FDXQe�ISP]b�"*)VYPWEGK TUe?���@�������y

A]Cw~3KMEGFQP]NlT1HJ`+*Je PWEGs HJP]X "We�ISPwK N #"PWEGFDKM}VLV`]KWYeONlf�eOX XQPWLOb K#"WLOXiLObM}VeOXDK FDTUeVE�ISP�EDPW}OTUP]N3FQLV`]KWYeONK N3FQP]b KM}VP]N3FQPWE]h ISPls eOXDTUL ! HJP�bML EDPW}OTUP]N3FQLV`]KWYeON(EDPWL5TULVE fSXQPW`]KMEDL�y

Page 22: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

9YF � �E; P� }Ja ��� F�:�}Jvf� 4 P� � ��a �� �k�wv �c}JvGJI N d�=7TOg�d � IU@ . ?5@WBC=CTO?og�d�IC>�?5^ � > Z IU@WIU^y=�IU^hd�>�= =7V�@W?Y� � BC= N �%P?Q> V�d�IU^

d�>�=�^e?5TOd N ��P?Q>SB P=7^fIUT�=7p#?5@A=7R�=kV#?5@WR�@ P� =zR�=7@wTOd i =7@�=hd�>oBC=UXQ?5@ N ?5>�^ed�Bx?�R�IZ � IUBxV#?��7TO? N d�=7T8IU^jV�@W? � � p ��Z � tQ?�:�s?5Bx?x^eIhtQIU@ P=xIU^ Z ?C>�? = . I NLZ =SIU> i @A=7> BxIUR � R�=�IUT]R�IU^eIUBxV�=��>�?�R�IUT�KMT�{

Z @A=7R�?��*=7TOBC=�> :

� �� � ��� ��� � � � � � ��� � ������� � �� !��� ��� � � ���� ��� �

���� � � � ������� � ��������! ��=w@A= !�P?5>zV#?5@(T�= N d�=7T5IU^�>�I N IU^y=7@ � ?sd Z � T � ! =�@�IU> . ?!g�d�IU^ N ?5Bx?wIUT5KMT Z @W?J�*=7TOBC=�>^eIJR�IUp#Ik=*g�d�I�� N ?5Bx?z^eIJBxIU> N � ?5>�=�p�=J=7> Z IU@ � ?5@WB�IU> Z I �YIU^f>�I N IU^y=7@ � ?kd Z � T � ! =�@N ?5@W@WIUT�= N �%P?Q>SV�=7@A= N ?5BxV�@W?5p�=7@4g�d�I*^eIzIU^ Z#P=*^e?5p�@WIkIUT2?5p�qrI Z ?�R�I � > Z IU@ PIU^%:v]> T�= =7V�@W?Y� � BC= N �%P?Q>�R�I N ?5@W@WIUT�= N �YP?Q> �(^eI Z � IU>�ISd�>�V�= Z @ P?5> R�IUT�?5p�qrI Z ?

= Z @A= Ny~ IY=7@�X IU^ N ?5> PIU^ Z I N ?5> IUTkg�d�I ^eI N ?5@W@WIUT�= N � ?Q>�=oIU> ^ed�p�^eI N d�IU> Z IU^� B P= i IL>�IU^U:v]^(R�IU^eIY=7p�TOI(g�d�I4IU^ Z I]V�= Z @ P?5>J^eIY=fV#IUg�d�I��>�?sR�I Z =7T . ?5@WBC=�g�d�I4^eI N ?5>�^ed�BC=

IUT2BxIU>�?5@ Z � IUBxV#?*V#?5^ � p�TOIwIU> T�= N ?5BxV�=7@A= N �%P?Q>SR�IEV�= Z @W?5>�IU^%:� � > IUB�p�=7@ i ?���T�=x@WIUR�d NUN ��P?5> R�IUT]V�= Z @ P?5>o^ �Oi > � K N =hg�d�IST�= N ?5BxV�=7@A= N �%P?Q>

R�IUp#I Z IU>�IU@kd�> BC=7@ i IU> R�ISIU@W@W?5@JBxIU>�?5@wX IU^kd�> N ?5BxV�@W?5B � ^e?�g�d�ISV�d�IUR�ITOTOILtQ=7@E=oIU@W@W?5@WIU^EX T�= V PIU@WR � R�=CR�IUT�?5p�qrI Z ? �4V�d�IU^�IUTf?5p�qrI Z ? ^eI�IU> N d�IU> Z @A=^e?5BxI Z � R�?*= N =7B�p � ?Q^4R�I � TOd�B � >�= N �%P?Q> � � � ��� :GJI IU^ Z = . ?5@WBC=�� N ?5> T�= � > N TOd�^ ��P?Q>�R�IUTJKMT Z @W? �*=7TOBC=�> �fIUTz^ � ^ Z IUBC= ^eIBC=7> Z � IU>�IjBC=7^sIU^ Z =7p�TOIjXCBC=7^s@ P=7V � R�?�:;(?5@*d�>�= V�=7@ Z I�T�=xtQIU> Z =7>�=CR�I Z @A= Ny~ IU?�^eI�V�d�IUR�I2��= N IU@zBC=7^zV#IUg�d�I��>�=

BC=7> Z IU> � IU>�R�?Ep�=Yqe=hT�=*V�@W?5p�=7p � T � R�=�RSR�IEg�d�I*IUT2?5p�qrI Z ?h^eIEV � IU@WR�=�:� � ^eI Z � IU>�I�d�>�=�tQIU> Z =7>�=xV#IUg�d�I��>�=�IU> Z ?5> N IU^JIUT N�P=7T N d�T�? N ?5> N ?5@W@WIUT�= N �%P?Q>C?=7T i d�>�=z? Z @A=�=7V�@W?Y� � BC= N �%P?Q>SIU^wBC=7^s@ P=7V � R�?�:ac>�=�R�ICT�=7^ER�IU^WtQIU> Z =Yqe=7^*R�ICg�d�ICIUTs?5p�qrI Z ?o^eI�V � IU@WR�=CIU^hg�d�IC^eI�R�IUp#I

@WIY=7T � ! =Y@jd�>�=Cp Pd�^eg�d�IUR�=�IU> Z ?!R�=�T�= � BC= i IL>�V�=7@A= Z @A= Z =7@kR�I�@WIUTO? N =7T � ! =�@WT.?��TO? N d�=7T � > N @WIUBxIU> Z =*IUT Z � IUBxV#?hR�I�V�@W? N IU^y=7B � IU> Z ?h= � > Z IU@rtQ=7TO?Q^ � @W@WI i d�T�=�@WIU^XCIU^ Z ?�>�?�IU^wR�IU^eIY=7p�TOI5:;(IU@W?�>�?h^ P?5TO?zIU^e?��5IU^ Z =*p Pd�^eg�d�IUR�=zV�d�IUR�IzR�=7@fTOd i =7@f=�R�I Z I NLZ =7@4? Z @W?h?5p�qrI Z ?^ � B � TO=�@fXoIUT Z @A= Ny~ IU?x^eIU@ P=�IU@W@W?5>�IU?�:J� � >oIUB�p�=7@ i ? � N ?5Bx?S^eIhBxIU> N � ?5> P?���IUT

Page 23: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

������� ����� ����� � ����� ��� � ������ � ����� � � � ����������� ��� 9Y\

KMT Z @W?j>�?5^(V#IU@WB ��Z I�@WIUR�d N � @2T�=stQIU> Z =7>�=�XEBC=7> Z IU>�IU@8d�>�^eI i d � B � IU> Z ?fIU^ Z =7p�TOI5:GJIzIU^ Z = . ?5@WBC=*IUT2^ � ^ Z IUBC=JIU^jB P=7^fIU^ Z =7p�TOI5:

���� � � � / �"� � � � / ��� v]>u9Y���5��� 4E: vw:w�*=7TOBC=�>lV�d�p�T �ON�P? ^ed . =7Bx?5^e?�=7@ ZYP� N d�TO? R�IU^ N @ � p � IU>�R�? d�>�=^e?5TOd N ��P?Q>�@WI N d�@W^ � tQ=�=7T2V�@W?5p�TOIUBC=kR�IzKMT Z @A=7R�?�T � >�IY=7T�R�IER�= Z ?5^�R � ^ N @WI Z ?5^%:GJIU^eR�I*IU^eI�Bx?5BxIU> Z ?���IUT KMT Z @W?C�*=7TOBC=Y>�^eI���=St � ^ Z ?�^ed�qrI Z ?C=�d�>�=�IL� Z IU>�^y=

� >5tQIU^ Z �Oi = N �%P?Q>�X =7V�T �ON = N �%P?Q> �4V�=7@ Z �ON d�T�=�@WBSIU> Z IxIU> IUT P=7@WIY= R�I >�=UtQI i = N ��P?Q>=7d Z#P?5>�?5BC=J?�=7^ � ^ Z � R�=!:v]TzKMT Z @W? �*=7TOBC=Y>�IU^ d�> N ?5>�qrd�> Z ?�R�I I N d�= N � ?5>�IU^�BC= Z IUB P= Z �.N =�^�g�d�I

V�@W?YtQIUIU> d�>�=�^e?5TOd N ��P?Q> N ?5BxV�d Z = N � ?Q>�=�T�@WI N d�@W^ � tQ=*ILK N � IU> Z Iz=7T2V�@W?5p�TOIUBC=kR�IB P� > � BS?Q^ N d�=7R�@A=7R�?5^%:v]^ Z I�KMT Z @W?wIU^�B�d!XkV#?!R�IU@W?5^e?�IU>*R � tQIU@W^e?5^ =7^eV#I NLZ ?5^�� V�d�IU^ N ?5> PIU^ Z I4V#?!R�IL{

Bx?5^SIU^ Z � BC=7@*IU^ Z =7R�?5^SV�=7^y=7R�?5^�� ��V�@WIU^eIU> Z IU^���X . d Z d�@W?5^��CR�IUTc^ � ^ Z IUBC=R � > P=7B ��N ?*=�^eIU@�IU^ Z � BC=7R�?��5= Pd�> N d�=7>�R�?�T�=�>�= Z d�@A=7TOI ! =kV�@WI N � ^y=zR�IUT ^ � ^ Z IUBC=Bx?!R�IUT�=�R�?EIU^wR�IU^ N ?5>�? N � R�=!:GJI . ?5@WBC=�IU^eV#I N�P� K N = �8IUT�KMT Z @W? �*=7TOBC=Y> IU^hd�>lIU^ Z � BC=7R�?Q@cd�^y=7R�?oV�=7@A=

IU^ Z � BC=7@fIUT�IU^ Z =7R�?�R�Ihd�> ^ � ^ Z IUBC=*T � >�IY=7T#R � > P=7B ��N ?�V#IU@ Z d�@Wp�=7R�?xV#?5@cBxIUR � {N � ?5>�IU^ N ?5>�@Wd � R�?kp�T�=7> N ?E=7d�^e^ � =7>�?jg�d�Ij^e?5> . d�> N � ?5>�IU^�T � >�IY=7T�IL^ R�IUT�IU^ Z =7R�?R�IUT�^ � ^ Z IUBC=sV#IU@W? N ?5@W@W?5BxV � R�=�^2V#?5@]T�=k=7R �ON ��P?Q>zR�Ij@Wd � R�?kp�T�=7> N ?E=7d�^e^ � =7>�?M:v]T�Bx?!R�IUTO?fBC= Z IUB P= Z �.N ?�d Z � T � ! =YR�?fIU>*T�=sR�IU@ � tQ= N ��P?Q>jR�IUTQKMT Z @W?J�*=7TOBC=�>jIU^

d�>�=�@WIUV�@WIU^eIU> Z = N ��P?Q>w@A= ! ?5>�=7p�TOI2V�=7@A=fB�d N ��?5^8V�@W?5p�TOIUBC=�^MR�I � > Z IU@ PIU^�V�@ P= NLZ �ON ?��g�d�I � > N TOd!XQIU>�V�@W?5p�TOIUBC=7^4R�I N ?5> Z @W?5T2=7^ P� N ?5Bx?*V�@W?5p�TOIUBC=7^4R�IEIU^ Z � BC= N �YP?Q> :}4=7B�p �UPIU> �%IUT�Bx?!R�IUTO?cR�IcKMT Z @W?h�*=7TOBC=Y>�IU^fd Z � T � ! =�R�?cV�=7@A=zIUT8=7> P=7T � ^ � ^(R�I

V�@W?5p�TOIUBC=7^4R�IEBxIUR �ON �YP?Q>�XCIU^ Z � BC= N �%P?Q> :� = N � IU>�R�?o=7TOd�^ ��P?Q>�=7T � > �ON � ?CR�ICIU^ Z =o^eI NUN ��P?5> �2IUT�B PI Z ?!R�? R�I B P� > � B�?Q^

N d�=7R�@A=7R�?5^ . d PIzIUT2V�@ � BxIU@�B PI Z ?!R�?*R�IEIU^ Z � BC= N �%P?Q> P?5V Z � Bx?M:Pv]^ Z I . d PI�R�IU^ N d�p � IU@ Z ?�V#?5@�E=7d�^e^�Xl? Z @W?5^�=7TO@WIUR�IUR�?5@zR�ICKM>�=7TOIU^�R�IUT�^ �Oi TO?R � I N � ? N ��?���XCIU^wB�d!X�d Z � T � ! =YR�?�= Pd�> ��?YX�IU> R P� =�:v]> d�>C^eIU> Z � R�? i IU>�IU@A=7T ��IUT�KMT Z @W?��*=7TOBC=�>� wIU^ Z � BC=Jd�>CV�@W? N IU^e?Ed�^y=7>�R�?d�>�= . ?5@WBC=ER�I N ?5> Z @W?5T#@WI Z @W?�=7T � BSIL> Z =�R�?M:�]CEGFQPjPWEGFDK TULVISeOX PWE=b bMLOTULVISe fSXQPWI�KM`]FQeOXWy� CEGFQPlFDK f�e ISPlPWEGFDK TULVISeOX PWE=b bMLOTULVISe��Sb FDXQe�y�]CEGFQPlFDK f�e ISPlPWEGFDK TULVISeOX PWE=b bMLOTULVISegEGHJL #3KMEDLVISeOX��� K b FDXQe��jLOb TULON/EDP5bMP1EGHJP]bMP�b bMLOTULOXjL `]HJLOb ! HSKMP]XQLUISP5bMeVElFDXQPWE�FDK f�eVElISP PWEGFDK TULVISeOXQPWETUP]NJ`]KMeONJLVISeVE]y

Page 24: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

9U` � �E; P� }Ja ��� F�:�}Jvf� 4 P� � ��a �� �k�wv �c}Jvv]TMKMT Z @W?kIU^ Z � BC=wIUT�IU^ Z =7R�?zR�IUT�V�@W? N IU^e?JIU>�=7T ijPd�>*Bx?5BxIU> Z ?wXSIU>�^eI i d � R�=

^eIh@WI Z @W?�=7T � BSIL> Z =*IU> T�= . ?5@WBC=�R�IhBxIUR �ON � ?7>�IU^ N ?5> @Wd � R�?�:JGJIhIU^ Z = . ?5@WBC=��T�=7^�I N d�= N � ?5>�IL^�R�IUT*KMT Z @W? �*=7TOBC=�> N =7IU> IU> R�?5^ i @Wd�V#?5^�V#IU@ . I NLZ =7BxIU> Z IR�ILKM> � R�?5^��

� ��=7^�I N d�= N � ?5>�IL^4R�I�= NLZ d�=7T �"! = N �%P?Q> Z IUBxV#?5@A=7T ��X� v N d�= N � ?5>�IU^4R�I�= NLZ d�=7T � ! = N �%P?Q>SR�IET�=7^sBxIUR �ON � ?Q>�IL^%:

��=7^2I N d�= N � ?5>�IL^MR�Is= NLZ d�=7T � T ! = N �%P?Q> Z IUBxV#?5@A=7TU^e?5>z@WIU^eV#?5>�^y=7p�TOIU^MR�I�V�@W?YXQI NLZ =7@��= N � =z=7R�IUT�=7> Z I�IU>�IUT Z � IUBxV#?kIUT#IU^ Z =7R�?h= NLZ d�=7T!X�TO?5^�IU@W@W?5@WIU^�R�I N ?YtQ=7@ � =7> ! =IU^ Z � BC=7R�?Q^�V�=7@A=�R�I IU^ Z = . ?5@WBC= ?5p Z IU>�IU@xIU^ Z � BC= N � ?7>�IU^ � 13+,7 .0+,7jV�=7@A= IUT^ �Oi d � IU> Z IwV�=7^e?�:

��=7^�I N d�= N � ?Q>�IU^hR�I�= NLZ d�=7T �"! = N �%P?Q>lR�I T�=�BxIUR �ON ��P?Q>�V#?5@x^ed T�=7R�?��s^e?5>@WIU^eV#?5>�^y=7p�TOIU^hR�I T�=�@WI Z @W?�=7T � BSIU> Z = N �%P?Q> � � : I5: �wR�I � > N ?5@WV#?5@A=7@hd�>�= >�d�ILtQ=BxIUR �ON �YP?Q>ER�IU> Z @W?ER�IjT�=JIU^ Z � BC= N �%P?Q>-� 13+,7 .0+,7�V�=7@A=k?5p Z IU>�IU@�d�>�IU^ Z =7R�?kBxIrqr?Q{@A=7R�? � 1�.0$�%&��+,7 .0+,7�:

��=7^]I N d�= N � ?5>�IL^ R�IJ= NLZ d�=7T �"! = N �%P?Q> Z IUBxV#?5@A=7T Z =7B�p �UPIU>EV�d�IUR�IU>S^eIU@�t � ^ Z =7^N ?5Bx?�I N d�= N � ?5>�IU^�13+4�*�07 ��%&.0+4�0$ ��B � IU> Z @A=7^Eg�d�I T�=7^*I N d�= N � ?5>�IU^zR�I�= NLZ d�=7T�{� ! = N ��P?Q>SR�IET�=�BxIUR �ON �YP?Q> N ?5Bx?�I N d�= N � ?5>�IU^4R�I-�*.0+,+4�*�*��7��.05�:v]>�IU^ Z Iz^eIU> Z � R�?��QIUT(=7T i ?5@ �.Z BS?zIU^ Z � BC=7R�?Q@]^eI*=7^eIUBxIrqe=E=hTO?5^w=7T i ?5@ �.Z B�?Q^

>�d�B PIU@ �ON ?Q^4R�I*T�= . ?5@WBC=�13+4�*�07 ��%&.0+,) �*.0+,+4�*��%&.0+y:

���� �� � �0� $�� �� "�$(/ � �0��� $ ��� " � ����'�0� �v]T�Bx?!R�IUTO?jR�IwIU^eV�= N � ?jR�IjIU^ Z =7R�?5^�R�Iwd�>SV�@W? N IU^e?JR � > P=7B ��N ?wd�^y=JI N d�= N � ?5>�IU^R �/. IU@WIU> N � =�T.IU^�V�=7@A=f@WIUV�@WIU^eIU> Z =7@ Z =7> Z ? . IU> P?5BxIU>�?5^�R�I Z IU@WB � > � ^ Z =�^ N ?5Bx?�=7TOIY= Z ?7{@ � ?5^%:

��=7^�tQ=7@ � =7p�TOIL^�R�IUT#Bx?!R�IUTO?J^e?5>xT�=7^4tQ=7@ � =7p�TOIL^�R�I � > Z IU@ PIU^]X�T�=7^�R�IU@ � tQ=7R�=7^R�I � > Z IU@ PIU^%:��=7^jtQ=7@ � =7p�TOIU^sR�IUV#IU>�R � IU> Z IU^cR�IUT�^ � ^ Z IUBC=*R�I�I N d�= N � ?5>�IL^�R �/. IU@WIU> N � =�T.IU^�^e?5>T�=7^ftQ=7@ � =7p�TOIL^4R�IUT2^ � ^ Z IUBC=kR � > P=7B ��N ?M:Pv]^ Z =7^j@WIUV�@WIU^eIU> Z =7> R�I*BC=7>�IU@A=�IL��V�T P� N �.Z = Z ?!R�=7^wT�=7^ N =7@A= NLZ IU@ P� ^ Z ��N =�^ � BxV#?5@r{Z =7> Z IU^�R�IUT ^ � ^ Z IUBC=kR � > P=7B �.N ?zIU> N d�=7TOg�d � IU@4Bx?5BxIU> Z ?�:��� � � � � / � ��&L"%�!� ��&S� )*��$2�5��&U$�&ac>S^ � ^ Z IUBC=wIU^�d�>�= N ?5TOI NUN �YP?Q>ER�Ic?5p�qrI Z ?5^ � > Z IU@W@WIUT�= N � ?Q>�=�R�?Q^ g�d�IJ^e?5> Z @A= Z =�{R�?5^ N ?5Bx?�d�> Z ?!R�? N ?5>CIUT2V�@W?5V P?5^ ��Z ?ER�IEBx?!R�IUT�=�@4^ed N ?5BxV#?5@ Z =7B � IL> Z ?�:

Page 25: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

������� ����� ����� � ����� ��� � ������ � ����� � � � ����������� ��� 9Ym

��I�R �ON I�g�d�IC^e?5>�R � > P=7B ��N ?Q^J^ � ^ed�^S= Z @ � p�d Z ?5^zR�I � > Z IU@ PIU^ N =7B�p � =�> N ?5> IUTZ � IUBxV#?�:ac> V�@W? N IU^e?�IU^wT�=�ILtQ?5TOd N ��P?Q>SR�IUT2^ � ^ Z IUBC=kIU> IUT Z � IUBxV#?�:�cd�IU^ Z @W?�^ � ^ Z IUBC=*^e?5T�=7@�g�d�I N ?5>�^ � ^ Z IkR�IUT�^e?5T X TO?5^jV�T�=7>�I Z =7^ N ?5>�? N � R�?Q^���IU^d�>�=�IrqrIUBxV�TO? .WP� ^ �ON ?zR�IEd�>�^ � ^ Z IUBC=kR � > P=7B ��N ?M:� Z @W?kIrqrIUBxV�TO?jR�Ij^ � ^ Z IUBC=sR � > P=7B ��N ? � > N TOd!XQI�=7T!@WIU^e?5>�=7R�?5@�=7@WB P?5> ��N ?�T � >�IY=7T :v]T!Bx?!R�IUTO?sR�I��cI�� Z ?5>ER�I�TO?5^(>!{ N d�IU@WV#?5^ IU^]? Z @W?cIrqrIUBxV�TO?�R�I�d�>�^ � ^ Z IUBC=

R � > P=7B ��N ?M:v]T N ?5BxV#?5@ Z =7B � IU> Z ?ER�IET�=*I N ?5>�?5B P� =JIU^wd�>�^ � ^ Z IUBC=kR � > P=7B ��N ?M:

��� � � � ��� � ���!����$ �5$8 #"��� 3($�'2& � �4��&U�5���5"%$�kd�>�g�d�I�=7T i d�>�=7^wtQI N IU^zIU^EV�@WI . IU@ � p�TOIEBx?!R�IUT�=7@wIUT Z � IUBxV#?�R�IU^eR�ISd�>�=CV#IU@r{^eV#I NLZ � tQ= N ?5> Z � >�d�=��5IU^jB P=7^fV�@ P= NLZ �ON ? N ?5>�^ � R�IU@A=7@WT.?zR�IEBC=7>�IU@A=ER � ^ N @WI Z =�:

��� � � � � � � ���Q���������&x���o��&L"%����$�� '����Q"%$�����&v]TMIU^ Z =7R�?zR�Ijd�>x^ � ^ Z IUBC=�R � > P=7B ��N ?jIU>Sd�> Z � IUBxV#?cR�=7R�?E^eI N =7@A= NLZ IU@ �"! =sV#?5@tQ=7TO?5@WIU^ � >�^ Z =7> Z#P=7>�IU?Q^4R�IE^ed�^J= Z @ � p�d Z ?5^fR�I � > Z IU@ PIU^%:v]^ Z ?5^z= Z @ � p�d Z ?5^wR�I � > Z IU@ PIU^jIU> T�=�V�@ P= NLZ �ON =*V�d�IUR�IU> ^eIU@ N =7@A= NLZ IU@ �"! =�R�?Q^

V#?5@4> Pd�BxIU@W?5^(@WIY=7TOIU^�� N ?5Bx?JV�d�IUR�IU>S^eIU@4V#? Z IU> N � =7T.IU^ IUT PI NLZ @ ��N ?7^�� Z IUBxV#IU@A= Z d!{@A=7^���IU^ Z =7R�?hR�IET�=*p#?5TO^y=ER�IztQ=7TO?5@WIU^��7I ZeN :ac>�=�tQ=7@ � =7p�TOIwR�IEIU^ Z =7R�?hR�IEd�>�^ � ^ Z IUBC=kIU^wIUT8tQ=7TO?5@�@WIY=7T =7^e? N � =�R�?M:v]T�tQI NLZ ?5@]R�IjIU^ Z =7R�?kR�Ijd�>�^ � ^ Z IUBC= Z � IU>�I�IU> Z ?5> N IU^�tQ=7@ � =7p�TOIU^2R�IjIU^ Z =7R�?

N ?5Bx?�^ed�^wIUTOIUBxIU> Z ?5^ N ?5BxV#?5>�IU> Z IU^%:ac>�^ � ^ Z IUBC=�IU^ N ?5>�^ � R�IU@A=7R�? N IU@W@A=7R�?k^ � IUT�IU^ Z =7R�? . d Z d�@W?ER�IUTM^ � ^ Z IUBC=�IU>

N d�=7TOg�d � IU@ � >�^ Z =7> Z IhR�I Z � IUBxV#?xIU^*R�I Z IU@WB � >�=7R�?xR�I�BC=7>�IU@A= Pd�> �ON =xV#?5@z^edIU^ Z =7R�?S= NLZ d�=7T,:� � IUT(^ � ^ Z IUBC=*R � > P=7B �.N ?*>�?�IU^ N IU@W@A=7R�?��!IU> Z ?5> N IU^jT�=7^ N =7d�^y=7^wIL� P? i IU>�=7^

N ?5@W@WIU^eV#?5>�R�IU> N ?5>CT�=7^�IU> Z @A=7R�=7^j=7T2^ � ^ Z IUBC=!:�v]^ Z IztQI NLZ ?5@�R�I*IU^ Z =7R�?�R�I*d�>^ � ^ Z IUBC= R�IUp#Io^eIU@ N ?5BxV�TOI Z ?oIU> IUTj^eIU> Z � R�? R�I g�d�IoIUTcIU^ Z =7R�? . d Z d�@W?�R�IUT^ � ^ Z IUBC=CIU^�R�I Z IU@WB � >�=7R�?�R�I�BC=7>�IU@A= Pd�> �ON =�V#?5@�^ed IU^ Z =7R�?�= NLZ d�=7TfX�^ed�^IU> Z @A=7R�=7^ . d Z d�@A=7^%:|;�=7@A= ?5p Z IU>�IU@Sd�> tQI NLZ ?5@SR�IoIU^ Z =7R�?5^ N ?5BxV�TOI Z ?�V�=7@A=d�> ^ � ^ Z IUBC=SR�I Z IU@WB � >�=7R�?���T�=7^ N ?5BxV#?5>�IU>�I Z IL^cR�IxT�=7^ktQ=7@ � =7p�TOIU^jR�IxIU^ Z =7R�?V�d�IUR�IU> � > N TOd � @JR�IU@ � tQ=7R�=7^zR�I�T�=7^ztQ=7@ � =7p�TOIL^JR�ICIU^ Z =7R�?�:�v]>�IU^ Z Ix^eIU> Z � R�?��IU^ Z ?h>�? ��=7p � T ��Z =k=�d�^y=7@fT�=EtQIUTO? N � R�=�Rh?�= N IUTOIU@A= N �YP?Q> N ?5Bx?Ed�>�=�tQ=7@ � =7p�TOIsR�IIU^ Z =7R�?�:

Page 26: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

9�� � �E; P� }Ja ��� F�:�}Jvf� 4 P� � ��a �� �k�wv �c}Jv��� � � � ��� � $2��� ��$�&����z��& ��������$����z��&L"%����$�& ��������&Y��&L"%�!� ��&j�4�� �6�������5$�&;�=7@A=�g�d�I�IUT�IU^ Z =7R�? . d Z d�@W?�R�I�d�> ^ � ^ Z IUBC=*^eIhV�d�IUR�=xR�I Z IU@WB � >�=7@�=xV�=7@ Z � @R�IE^ed IU^ Z =7R�?S= NLZ d�=7T�XCR�IE^ed�^jIU> Z @A=7R�=7^ . d Z d�@A=7^%:v]T N ?5BxV#?5@ Z =7B � IU> Z ? R � > P=7B ��N ?�R�I N =7R�=otQ=7@ � =7p�TOISR�I�IU^ Z =7R�? R�IUTj^ � ^ Z IUBC=R�IUp#IU@ P= ^eIU@�d�>�= . d�> N ��P?5> N ?5>�? N � R�=�R�ICTO?5^EtQ=7TO?5@WIU^ � >�^ Z =7> Z#P=7>�IU?Q^kR�IC? Z @A=7^tQ=7@ � =7p�TOIL^4R�IEIU^ Z =7R�?�XCR�IET�=7^sIU> Z @A=7R�=7^�R�IUT2^ � ^ Z IUBC=!:v]TsBx?!R�IUTO?�R�I�IU^eV�= N � ?�R�IxIU^ Z =7R�?5^zV�=7@A=Cd�> ^ � ^ Z IUBC=SR � > P=7B ��N ?x@WIUV�@WIL{

^eIU> Z =cIU^ Z =7^�R�IUV#IU>�R�IU> N � =7^ . d�> N � ?5>�=7TOIL^2IU> Z�PIU@WB � >�?5^8R�IwI N d�= N � ?5>�IU^2R �/. IU@WIU>!{N � =7TOIU^MR�I�V�@ � BxIU@�?5@WR�IU> :(v]^ Z =7^ I N d�= N � ?5>�IL^M@WIUV�@WIU^eIU> Z =7>JIUT N ?5BxV#?5@ Z =7B � IU> Z ?R�IUT2^ � ^ Z IUBC=kR � > P=7B ��N ?kXC^eIETOIU^sR�IU>�?5B � >�=zI N d�= N � ?Q>�IU^4R�IEIU^ Z =7R�?�:� � IU^ Z =7^kV�d�IUR�IU> ^eIU@k@WIUV�@WIU^eIU> Z =7R�=7^cV#?5@ . d�> N � ?5>�IU^jT � >�IY=7T�IL^wIU> Z ?5> N IU^ Z IU>!{IUBx?5^fd�>�^ � ^ Z IUBC=kR � > P=7B ��N ?ET � >�IY=7T :

� ��� � �� � ���� � ��� � ����� � � �� !��� ��� � � � � ��� �v]T!V�@ � BxIU@2V�=7^e?Jg�d�I�^eIsR�IUp#Is@WIY=7T � ! =�@ N d�=7>�R�?J^eIsR � ^eI��>�=fd�>hKMT Z @W?cIU^]@WIY=7T � ! =�@d�>�Bx?!R�IUTO? ��� � T���� R�IET�=*^eI��>�=7T ' � g�d�IE^eIER�IU^eIY=EKMT Z @A=7@U:v]>�>�d�IU^ Z @W? N =7^e?zT�=z^eI��>�=7T�g�d�Ik^eIcR�IU^eIY=JKMT Z @A=7@4IU^fT�=kV#?5^ �ON ��P?Q>*R�IUT N IU> Z @W? � R�IR�Ixd�> ?5p�qrI Z ?�IU>�Bx?Yt � B � IU> Z ?M:Sv]> p�=7^eI�= T�=�V#?5^ �ON ��P?Q> =7> Z IU@ � ?5@cR�IUT N IU>!{Z @W? � R�I�� IUT5Bx?!R�IUTO?�V�=7@A=�T�=s^ �Oi d � IU> Z I(V#?5^ �ON ��P?Q>cR�IUT N IU> Z @W? � R�I�IU^ Z#P=sR�Is= N d�IU@WR�?=

� : �� : � " �

?SR�IE? Z @A= . ?5@WBC=� : " � ) � � ��� +

g�d�I N ?5@W@WIUV#?5>�R�I = T�= I N d�= N ��P?Q>�R �/. IU@WIU> N � =�T4g�d�I R�IU^ N @ � p#IxIUTjBx?Yt � B � IU> Z ?@WI NLZ � T � >�IL?Ed�> �/. ?5@WBxIUB�IL> Z Ic= N IUTOIU@A=7R�?M:v]^ Z =hI N d�= N ��P?Q>S>�?5^wTOTOILtQ=*=hT�=�^e?5TOd N ��P?Q> � � � ���

��) � + " ��� �� � � � 9F � �]:

B� '3P�TULON3FDKMP]NJP P]b FSYP]XDTgK NJecEDP &NJLOb+f�eOX ! HJP�PWEg`WeOTkYHSNdP]N)bML/b K FQP]XQLOFDHSXQL�h=LOHSN ! HJP9P]NN3HJPWEGFDXQe�`WLVEDe�bMe ! HJPlEDPlISPWEDPWL �Sb FDXQLOX EDeON fSHSN3FQeVE P]N P]bxfSbMLONJe�hJfSHJPWISP+K N3FQP]XDfSXQP]FQLOXQEDP PWEGFQe`WeOTUe5HSNJL EDP &NJLOb{y

Page 27: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

������� ��" � ���� ����� � ������ � ����� � � � ����������� ��� 9��

�s?5Bx? ^y=7p#IUBx?5^Ed�>�= I N d�= N ��P?Q> R �/. IU@WIU> N � =7T]R�I�^eI i d�>�R�? ?5@WR�IU>lV�d�IUR�I�^eIU@IU^ N @ ��Z = N ?5Bx?jd�>�^ � ^ Z IUBC=fR�I�R�?5^�I N d�= N � ?5>�IU^8R �/. IU@WIU> N � =�T.IU^8R�I�V�@ � BxIU@ ?5@WR�IU>

� ' " � � : " ���� � ' " � :

� � : " ) � � ' � : +v]^ Z I�^ � ^ Z IUBC=�^eIsV�d�IUR�I�IU^ N @ � p � @#IU> . ?5@WBC=j= N ?5BxV�=��>�=�R�= ' ' � � �5� N ?5Bx?� � � ' ) � +� � : ) � +�� "� � 9���� �

� � ' ) � +� : ) � +�� �� �� �

GJ?5>�R�I��� "

� � 9� � �

���J^eI�d Z � T � ! =�V�=7@A= N =7T N d�T�=Y@wT�=�BC= Z @ � ! R�I Z @A=7>�^ �ON �YP?Q> R�ISIU^ Z =7R�?5^��MX IU^TOT�=7B�=�R�=fT�=jBC= Z @ � ! R�I N ?!ILK N � IU> Z IU^8R�IUT!^ � ^ Z IUBC=sR�IwI N d�= N � ?5>�IL^2R �/. IU@WIU> N � =�T.IU^R�IEV�@ � BxIU@�?5@WR�IU> :

��) � + " � � ' ) � + � : ) � + �IU^zIUT�tQI NLZ ?5@JR�ISIU^ Z =7R�?5^JR�IUT]^ � ^ Z IUBC=�R � > P=7B ��N ?M: ��=7^ � � ^e?5> T�=7^jtQ=7@ � =7p�TOIU^R�IEIU^ Z =7R�?�: �!#(�� � �9���a� ����%����� ���%k���a������������ IU^2T�=f^e?5TOd N ��P?Q>wR�I�T�=�I N d�= N ��P?Q>BC= Z @ �ON � =�T���?5Bx? i IU>�IY=z=7^e? N � =�R�= N ?5>Cd�>�^ � ^ Z IUBC=kT � >�IY=7T�R � > P=7B ��N ?M:�s?5Bx?k^eIc^y=7p#I��7T�=7^]^e?5TOd N � ?Q>�IU^(R�IUT�^ � ^ Z IUBC=&��?5Bx? i4PIL>�IU?cIU^ Z#P=7>h@WIUT�= N � ?Q>�=�R�=�^N ?5>CT�=7^s^e?5TOd N � ?7>�IU^4R�IEd�>�^ � ^ Z IUBC=kT � >�IY=7T�R � > P=7B ��N ?5��� � T���� :v�� � ^ Z I*d�>�= . ?5@WBC=hR�I N =7T N d�T�=�@fT�=hBC= Z @ � ! R�I Z @A=7>�^ �ON ��P?Q>xR�IhIU^ Z =7R�?5^�IU>N =7^e?hR�IEIL� � ^ Z � @�^e?5TOd N ��P?Q> N IU@W@A=7R�=�:��IY= � T�=�BC= Z @ � ! R�I Z @A=7>�^ �ON ��P?Q>SR�IEIU^ Z =7R�?5^%:v]T N�P=7T N d�T�?zR�I � � � �5� IU^ Z#P=*R�=7R�?SV#?5@

� � ��!"$# %(' ) � : � : � �*� ���,+ %('GJ?5>�R�I #&%(' IU^jIUT���=7V�T�= N � =Y>�? � >5tQIU@W^e? � � : � : IU^jT�=�BC= Z @ � ! � R�IU> Z � R�=7R �5X $fIU^T�=EtQ=7@ � =7p�TOIwR�I � > Z I i @A= N �YP?Q> :� P? Z IU^eIcg�d�IE^eIE?5p Z � IU>�IcT�= � >5tQIU@W^y=*R�IET�=*R �/. IU@WIU> N � =kR�IEBC= Z @ �ON IU^U:

BDB ^=eOTgfJLONSKMeON�s eOXDT

Page 28: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

9Y� � �E; P� }Ja ��� F�:�}Jvf� 4 P� � ��a �� �k�wv �c}Jvv]^ Z =h^e?5TOd N ��P?Q> N =7T N d�T�=JT�=*V#?5^ �ON ��P?Q>�X�tQIUTO? N � R�=�R ' :cIU> T�=*R � @WI NUN ��P?Q> � :v]^�>�I N IU^y=7@ � ?E= i @WI i =7@�T�=zV#?5^ �ON ��P?Q>�X�tQIUTO? N � R�=�R�IU>�T�=zR � @WI NUN ��P?Q> �xR�I Z =7T

. ?5@WBC=Eg�d�I*^eI Z IU> i =�T�=*V#?5^ �ON ��P?Q>SR�IUT N IU> Z @W? � R�I )��� � +v]>�IU^ Z I N =7^e?hIUT2^ � ^ Z IUBC=kIU^ Z#P=*R�=7R�?�V#?5@

������ � ' ) � +� �' ) � +� � : ) � +� �: ) � +

� ���� "

������ � 9 �� � � 9� � ��� � ��

� ���� ������� ' ) � +� ' ) � +� : ) � +� : ) � +

� ���� �

���������

� ����

�s?5> )�� ' � ' +8@WIUV�@WIU^eIU> Z =7>�R�?�IUT N IU> Z @W? � R�I�R�IUTQ?5p�qrI Z ?�X )�� : � : +8T�=ftQIUTO? N � R�=�R :

�� � � !'�� / � ��/0�'"�$���k> Z IU^SR�I�V#?!R�IU@S=7V�T �ON =�@ET�=7^�I N d�= N � ?5>�IU^ER�IUT�KMT Z @W?��*=7TOBC=�> IU^�>�I N IU^y=7@ � ?N =7T N d�T�=�@]T�=*BC= Z @ � ! R�I Z @A=7>�^ �ON ��P?Q>SR�IEIU^ Z =7R�?5^��k:GJIC= N d�IU@WR�? N ?5> � � �5� X�R�IxTO?�IL��V�d�IU^ Z ? =7> Z IU@ � ?5@WBxIL> Z I��#TO?5^z^ �Oi d!{

� IU> Z IU^kV�=7^e?5^zB�d�IU^ Z @A=7> IUT N�P=7T N d�T�?SR�I � d Z � T � ! =Y>�R�?�V�=7@A=CIUTOTO?xIUT4^e? .�Z �j=7@WIR�IEV�@W? N IU^y=7B � IL> Z ?E^ � B�p P?5T �.N ?� ��% � ��/#��% 7 �*�

Bp� , L #"P]bMe"`]KMISLVIREDP *JL$KM}ONJeOXQLVISe�P]N/PWEGFQP�fSXQeOFQeOFDK f�e�h NJe$EDP LV`]FDHJLOb K#"WL(NSKEDP K NSKM`]KMLOb K#"WL`WeON�#OLObMeOXQPWE `WeOXDXQPW`]FQeVE

Page 29: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

������� ��" � ���� ����� � ������ � ����� � � � ����������� ��� 9Y�

� �Oi d�@A=JF�:.9 ��� P=7T N d�T�?wV�=7@ N � =7TQR�IwT�=cBC= Z @ � ! �k:]GJILKM> �ON ��P?Q>ER�IwT�=cBC= Z @ � ! � ����� :nod�T Z � V�T �.N = N �%P?Q>cR�I � ����� V#?5@ T�=ftQ=7@ � =7p�TOI(R�I � > Z I i @A= N �YP?Q> � :�V�� � � N ?5@W@WIU^eV#?5>�R�I= ���y:

Page 30: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

F5� � �E; P� }Ja ��� F�:�}Jvf� 4 P� � ��a �� �k�wv �c}Jv

� �Oi d�@A=cF�: F��(� P=7T N d�T�?�R�I ��� � � � � ����� � P=7T N d�T�?sR�I�T�= � >5tQIU@W^y=wR�I ��� � � � � ����� X?5p Z IU> N ��P?5>�R�IUT ��=7V�T�= N � =�>�? � >5tQIU@W^e?�: � � ��!"$#&%('�) � ����� � � � ��� + %(' :���= Pd�T Z � B�=T P� >�IY=fB�d�IU^ Z @A=�T�=w^e?5TOd N ��P?Q> N IU@W@A=7R�=�R�IUT�^ � ^ Z IUBC=fR�I�I N d�= N � ?5>�IL^8R �/. IU@WIU> N � =�T.IU^R�IEV�@ � BxIU@�?5@WR�IU> :

GJIETO?S=7> Z IU@ � ?5@�^eIz?5p Z � IU>�I

� "�����9 � � �� 9 � �� � 9 �� �� 9

� ����

� ��?5@A=*IU^jV#?5^ � p�TOIc=7V�T �ON =�@4IUT8KMT Z @W?S�*=7TOBC=�> :��= BxIrqr?5@�=7V�@W?Y� � BC= N �%P?Q> N ?5>�^ � R�IU@A=7>�R�?�IUTk@Wd � R�?lR�I�T�=�V�T�=7> Z = X R�I�T�=7^?5p�^eIU@rtQ= N � ?Q>�IL^ N ?5Bx?z@Wd � R�?�p�T�=7> N ? N ?5>xBxIUR � = N IU@W?*IU^ N ?5Bx?E^ �Oi d�I � � �5� :

����� � /�� / � ��3(������$8 (��&S�4��&U�5���5"%��&S��� �����."%��$�������� �� �5-2"%�! (�4����$no?!R�IUTO?*R � > P=7B �.N ?z>�?�T � >�IY=7T

��� " � %(' )���� %(' + � � %(' � ����� ) � � +

Page 31: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

������� ��" � ���� ����� � ������ � ����� � � � ����������� ��� F�9

no?!R�IUTO?*R�IET�=*BxIUR �ON ��P?Q>S>�?�T � >�IY=7T� � "�� ��)���� + �� � � � � � ) � � � +

v N d�= N � ?5>�IU^4R�I � BxV�TOIUBSIL> Z = N �%P?Q>�>�?�T � >�IY=7T �� P=7T N d�T�?zR�IUT2IU^ Z � BC= N �%P?Q>�R�IUT2IU^ Z =7R�?hV�@WIUR �ON ��? �

���� ) � + " � %(' ) ���� %(' ) � + +� P=7T N d�T�?zR�IET�=�BxIUR �ON �YP?Q>SV�@WIUR �ON ��=

�� � "�� � ) ���� ) � + +v N d�= N � ?5>�IU^4R�Ih=7V�@W?Y� � BC= N �%P?Q>ST � >�IY=7T�

��� '��� %(' �� �� �

����� ����� ������ %���s?5>�R �ON � ?Q>�=�B � IU> Z ?z^e?5p�@WIzT�=*BxIUR �ON �YP?Q>SR�IUT2IU^ Z =7R�?hV�@WIUR �ON ��? �

���� ) � + " ���� ) � + ���� � ) � � � �� � +� P=7T N d�T�?zR�IET�=�BC= Z @ � ! R�I N ?YtQ=7@ � =7> ! =#��13+,7 .0+,7

� � ) � + " ��� '��� %(' � � %(' ) � + ��� '�� �� %(' � � %('� P=7T N d�T�?zR�IET�= i =7>�=7> N � =JR�I��*=7TOBC=�> �

�� �3" � � ) � +���� '�� �� � � ��� '��� � � ) � +���� '�� �� � � �� %('� P=7T N d�T�?zR�IET�=�BC= Z @ � ! R�I N ?YtQ=7@ � =7> ! =#��1�.0$�%&��+,7 .0+,7

� � ) � + " � � ���� � ��� '��� � � � ) � + IU^ � > �ON � =�T � ! =�R�= N ?5>xtQ=7TO?5@WIU^ ZYP� V �ON ?5^4BxIU>�?5@WIU^�=S��: � � � : I5: � " �5DO9 � � �����)�� ' � ' +sIU^ � > �ON � =�T � ! =�R�? N ?5>CIUT2V�@ � BxIU@ N IU> Z @W? � R�IcR�IUT2?5p�qrI Z ?�= Z @A= Ny~ IY=7@U:)�� : � : + �!g�d�I N ?5@W@WIU^eV#?5>�R�IJ=�T�=EtQIUTO? N � R�=�RSIU^ � > �ON � =�R�?*= N IU@W?�:

��=�BC= Z @ � ! R�I ^eIU>�^ ��Z � t � R�=�R = T�=7^�BxIUR �ON � ?7>�IU^�� IU^ � > �ON � =�R�= N ?5> T�=BC= Z @ � ! � : � : :4 IU^ � > �ON � =�R�= N ?5>CT�=*BC= Z @ � ! � : � : :

Page 32: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

F5F � �E; P� }Ja ��� F�:�}Jvf� 4 P� � ��a �� �k�wv �c}Jv�� � � � / ��� ��� ��� �v]T#KMT Z @W?�R�IzV�@WIUR �ONUN ��P?Q>h^eIkd Z � T � ! =cV�=7@A=*BxIrqr?5@A=7@�IUT8R�IU^eIUBxV#I��>�?kR�IUT Z @A= Ny~ IU?R�IhTO?5^c?5p�qrI Z ?5^%: � =�g�d�Ih^eIhV#IU@W^ �Oi d�Iz?5p Z IU>�IU@cd�> Z @A= Ny~ IU?xIU> Z � IUBxV#?�@WIY=7T �IUT(=7T i ?5@ � Z B�?z>�I N IU^ ��Z =z^eIU@�@ P=7V � R�?�:v]>�IUT2V�@W? i @A=7BC=kT�=7^ N ?5^y=7^s^e?5> N ?5Bx?*^ �Oi d�I �� v]>�T�=�V�@ � BxIU@A= ��Z IU@A= N ��P?Q>�T�=7^kV#?5^ �ON � ?7>�IU^jV�@WIUR �ON ��=7^k^e?5> T�=7^zB � ^eBC=�^g�d�IhT�=hV#?5^ �ON ��P?Q>�BxIUR � R�= ) ?5p�^eIU@rtQ=7R�= + ��X N ?5@W@WIU^eV#?5>�R�IU> =7T N IU> Z @W? � R�IR�IUT2?5p�qrI Z ?�^eIUTOI NUN � ?Q>�=�R�?M:v]> IU^ Z IkV�d�> Z ?S>�?S^eI Z � IU>�I �*.056.���7 / 7 ��56%&. ��� /#���*��56����.wR�IzT�=7^ N =7@A= N {Z IU@ P� ^ Z �ON =�^4R�IET�=*^eI��>�=7T,:

� ac>�= tQI ! g�d�ICT�= V#?5^ �ON ��P?Q> IU^hBxIUR � R�=�>�d�ILtQ=7BxIU> Z I�� � : I5: �2^eI���������� � �.' $���+�� �0+

� ��I�= NLZ d�=7T �"! =JT�=h= NLZ d�=7T �"! = N �%P?Q>SR�IET�=7^fV�@WIUR �ONUN � ?5>�IL^� v]> Z ?5> N IU^h^eICV�@WIUR �ON I�>�d�ILtQ=7BxIU> Z I�IU>�p�=7^eI =7T N ?5BxV#?5@ Z =�B � IL> Z ?CR�IT�=7^sV�@ � BxIU@A=7^4R�?5^wV#?5^ �ON � ?Q>�IL^

� ��Izt�d�IUT�tQI*=h?5p Z IU>�IU@s? Z @A=�?5p�^eIU@rtQ= N ��P?Q> � D D D �!I ZeN :

Page 33: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

���������� ��� �

� � � � � � � � ��� ����������� �

v]>lIU^ Z IC=7V�=7@ Z =7R�? ^eIxB�d�IU^ Z @A=7> TO?5^E@WIU^ed�T Z =7R�?5^JR�I N ?!R � K N =7@jXl=7V�T �ON =7@cT�=Z IU?5@ P� =JIL��V�d�IU^ Z =h=7> Z IU@ � ?5@WB�IU> Z IQ: �kT i d�>�?5^4R�I Z =7TOTOIU^ Z�PI N > �ON ?5^ N ?5> N IU@W> � IU> Z IU^=hT�=�V�@W? i @A=7BC= N �YP?Q>S^e?5> Z =7B�p �UPIU>SIL��V�d�IU^ Z ?5^%:

� ��� � ��� ��� ��kd�>�g�d�IC^eI�R�IU^eIY= ?5p Z IU>�IU@h=�T�=7@ i ? V�T�= ! ? d�> Z @A= Ny~ IU?oIU> Z � IUBxV#?C@WIY=7T �(IUTV�@W? Z ? Z � V#?*^eIz@WIY=7T � !�P?zIU> BC= Z T�=7pSV#?5@ N d�IU^ Z � ?5>�IU^�V�@ P= NLZ �ON =�^%:��I*d Z � T � !�P?EV�=7@A=hIUTOTO?�d�>�=�;f� N ?5BxV�= Z � p�T.IJ= 9�����n � ! ��X d�>�=x;f� N ?5B�{

V�= Z � p�TOI*=C�Qm5�xn � ! :zv]>�=7B�p�=7^w^eIhV�@W?5p P?xIUT�^ � ^ Z IUBC=�V�=7@A=SR�I Z IU@WB � >�=7@s^edR�IU^eIUBxV#I��>�?�IU> Z � IUBxV#?�: �s?5Bx? IU^xR�IoIU^eV#IU@A=7@W^eI�^eI�?5p Z d!t � IU@W?5>lBxIrqr?5@WIU^Z � IUBxV#?5^4R�IER�IU^eIUBxV#I��>�?EIU> T�=h;f� B P=7^f@ P=7V � R�=�:v]T�IU^ N =7T�=7B � IU> Z ?fR�IsT�=7^ � B P= i IL>�IU^MX*T�= N ?5>5tQIU@W^ ��P?5>JR�I � B P= i IL>�IL^#R�I N ?5TO?5@

=j> � tQIUTOIU^2R�I i @ � ^ ^eIfTO? i @ P?sIU^ N @ � p � IU>�R�?sd�>hV#IUg�d�I��>�?jV�@W? i @A=7BC= ��= N � IU>�R�?wd�^e?R�I�=7T i d�>�=7^ . d�> N � ?5>�IU^fR�ILKM> � R�=7^sIU> IUT Z ?!?5TOp#?Y�8:��I�V�@W? N IU^y=7@W?5> 9Y� � B P= i IL>�IU^ Z ?5BC=7R�=7^ N ?5>�d�>�= N�P=7BC=7@A=SR �Oi ��Z =7T]�j�wG

g�d�I Z ?5BC=JF N d�=7R�@W?5^�V#?5@]^eI i d�>�R�?�:(ac>S^ � ^ Z IUBC=s@WIY=7T�@WI i @WIU^y=7@ P� =w\5� N d�=7R�@W?5^V#?5@�^eI i d�>�R�?�:��=�V�@WI i d�> Z =�g�d�I*^ed�@ i IkIU^j^ � ^eIzV#?!R�@ P=7>xV�@W? N IU^y=7@ Z ?!R�?5^sTO?5^ N d�=7R�@W?5^%:� ?5^�@WIU^ed�T Z =7R�?5^�?5p Z IU> � R�?5^�^eI��>�=7T�=Y> g�d�I IU^�V#?5^ � p�TOI���V�d�IU^�^eI ��=�TO? i @A=7R�?V�@W? N IU^y=7@c9Y� N d�=7R�@W?5^sIU>�95: `S^eI i d�>�R�?5^sIU> IUT(=7B�p � IU> Z I � > Z IU@WV�@WI Z =7R�?�:

��=7^�V�@Wd�IUp�=7^�^eIo@WIY=7T � ! =Y@W?Q>�IU> IUTk=7B�p � IU> Z I�� � >�R�? �c^��5m X � � >�R�? �c^F5�5�5�EV�@W? . IU^ � ?5>�=�T :

F5\

Page 34: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

F7` � �E; P� }Ja ���|\�: ; 4�akv �E� � �j���z�&��ak��0 ���Jv��

;�=7@A=hIUT N�P=7T N d�T�?z^ � B�p P?5T �.N ?z^eIzd Z � T � !�P?zT�=*tQIU@W^ ��P?5>�IU^ Z d�R � =7> Z � T�R�I�n = Z ��IL{BC= Z �ON =!:�kT i d�>�=7^kV�@Wd�IUp�=7^*V�@WILt � =7^�=�T�=�IU^ N @ ��Z d�@A=�R�IUT�V�@W? Z ? Z � V#?C^eIx@WIY=7T � ! =�@W?Q>

IU> IUT �kB�p � IU> Z I � =7T N ?5> ��: � 'v]T @WIUV#?5@ Z Ik^eIzIU^ N @ � p ��P?zIU> ��=7}�I�� p�=Yqr?xac> � �8:

� ��� � � � � $ � � "�$ � ���"� � � $( � ��������! v]TwR�IU^y=7@W@W?5TOT�?CR�IUTwV�@W? Z ? Z � V#? � > N TOd!XQI�IUT�R � ^eI��>�?�R�IUTwBx?!R�IUTO?�X�T�=�V�@W? i @A=�{BC= N ��P?Q> :GJd�@A=7> Z IhIUT�V�@W? N IU^e?�R�I�R�IU^y=7@W@W?5TOT�?hR�ISIU^ Z I Z @A=7p�=Yqr?�T�= � > Z IU@A= NUN �YP?Q>CIU>!{

Z @WI�IUT4R � ^eI��>�?�X�T�=�V�@W? i @A=7BC= N �YP?Q> . d�I N ?5>�^ Z =7> Z I��8IU^ER�I N � @��2^eI�@WIUR � ^eI��>�=�p�=IUTJBx?!R�IUTO? N ?5> . ?5@WBxI�^eI V�@W? i @A=7BC=7p�= �]V�d�IU^ ��=7^ Z =�=7> Z IU^xR�I KM>�=7T � ! =�@�T�=V�@W? i @A=7BC= N �%P?Q> N ?5@W@WI NLZ =��QIL� � ^ Z � =7>SR�I Z =7TOTOIU^4?5p�^ N d�@W?5^%:� � > IUB�p�=7@ i ?o^eI V�d�IUR�I�R�I N � @hg�d�IoR�d�@A=7> Z I Z ?!R�? IUTwV�@W? N IU^e? ^ � IUBxV�@WI

IL� � ^ Z � IU@W?5>jR�?5^(p�TO?!g�d�IU^2R�IfV�@W? i @A=7BC= N �%P?Q>c^eIUV�=7@A=7R�?5^8^e?5p�@WI4TO?5^8g�d�Is^eI�?5V#IU@ P?IU> . ?5@WBC=CV�=7@A=7TOIUT�=SX N ?5@W@WIU^eV#?5>�R�IU>�=7TfV�@W? N IU^y=7B � IL> Z ?CR�I � B P= i IL>�IU^cX�=7TR � ^eI��>�?*R�IUT8KMT Z @W?S�*=7TOBC=�> :

� �� � � $ � ��$( ���0��� � $ ��� � " �� �����0� �����IcV�@WIU^eIU> Z =7>�@WIU^ed�T Z =7R�?5^�IU> . ?5@WBC= i @ P=�K N =wX N ?5BxIU> Z =7@ � ?Q^ IU> . ?5@WBC=cR�IcV � IR�IzK i d�@A=�:

B���LObM`WeON$PWE HSN�EDeOs F�� LOXQPlLObMP]TdYLON$uUbML1fJLObMLO�SXQL NJegEDPlLV`WP]N3FDHJL

Page 35: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

��� ��� ������ � � F5m

� �Oi d�@A=k\�:.9 � 0 BC= i IU> � > �ON � =YT :8v]TM?5p�qrI Z ?E= Z @A= Ny~ IY=7@�IU^4T�=cp#? Z IUTOT�=!: � ?5^ N T �ONy~ ^g�d�IJIUT�d�^ed�=7@ � ? ��=ER�=7R�? N ?5@W@WIU^eV#?5>�R�IU> N ?5>STO?5^f=7^ Z IU@ � ^ N ?Q^�^e?5p�@WIjT�=kp#? Z IUTOT�=!:

Page 36: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

F�� � �E; P� }Ja ���|\�: ; 4�akv �E� � �j���z�&��ak��0 ���Jv��

� �Oi d�@A=l\�: F�� 4wIU^ed�T Z =7R�?�R�I ^eI i BxIU> Z =7@ST�= � BC= i IL> � > �ON � =�T :uv]^ Z =l@WI i ��P?5>N ?5@W@WIU^eV#?5>�R�I*=�d�>�=x^e?5T�=h@WI i ��P?5>C��{ N ?5>�I NLZ =7R�=!:j�kp�^eIU@rtQI*g�d�I�T�=Sp#? Z IUTOT�=E>�?��=�^ � R�?x^eI i BxIU> Z =7R�=�IU> ^ed Z ? Z =7T � R�=�RCR�IUp � R�? =xg�d�IST�=�d�B�p�@A=7T � ! = N �%P?Q>C>�?��= � > N TOd � R�?�=7T i d�>�=7^ � > Z IU>�^ � R�=7R�IU^�R�I�V � ��IUT,:�;�=7@A= N =7R�= � BC= i IL>�R�I�T�=^eI N d�IU> N � =E^eIE@WIY=7T � ! =k^eI i BxIU> Z = N �%P?Q>�X N�P=7T N d�T�?ER�IUT N IU> Z @W? � R�I5:4v]T N IU> Z @W? � R�IN ?5@W@WIU^eV#?5>�R�Ik=7T#tQ=7TO?5@�?5p�^eIU@rtQ=7R�?�:

� �Oi d�@A=c\�: \��(;(?5^ �ON � ?Q>�=�B � IL> Z ?�R�I�T�=wtQIU> Z =7>�=cR�I Z @A= Ny~ IU? ) @WI N d�=7R�@W?Jp�T�=7> N ?8+IU> IUT N IU> Z @W? � R�ICR�I T�= � BC= i IL> � > �ON � =�T : �kp�^eIU@rtQIoT�= N @Wd ! V#IU@ . I NLZ =7BxIU> Z IN IU> Z @A=7R�=�:�v]T N IU> Z @W?lR�I T�= N @Wd ! @WIUV�@WIU^eIU> Z =l=7T N IU> Z @W? � R�I�R�I T�=�@WI i ��P?5>^eI i BxIU> Z =7R�=k>�?x=7T2R�IET�=*p#? Z IUTOT�=!:

Page 37: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

��� ��� ������ � � F��

� �Oi d�@A=x\�: ` �J��I N d�IU> N � =�R�I � B P= i IL>�IU^�V�@W? N IU^y=7R�=7^%:z�kp�^eIU@rtQI N ?5Bx?�T�= N @Wd !R�IUT N IU> Z @W?hR�IzT�=ztQIU> Z =7>�=*R�I Z @A= Ny~ IU?�^eIkB�d�ILtQI5:4v]^ Z IkIU^�IUT#tQ=7TO?5@4V�@WIUR �ON ��?V#?5@�IUT5KMT Z @W?k�*=7TOBC=�>cX*IUT N IU> Z @W?JR�IsT�= N @Wd ! � >�R �ON =sg�d�I Z =7>�TOIrqr?5^2^eIsIU^ Z#P=�R�IUTN IU> Z @W? � R�IJIL��= NLZ ?�:s�s?5>�^ � R�IU@WIJg�d�I�T�=*tQIU> Z =7>�=SR�I Z @A= Ny~ IU?SIU^cd�> N d�=7R�@A=7R�?R�I�9Y��V � ��IUTOIU^fV#?5@�T�=7R�?�:

� �Oi d�@A=h\�: m��]��I N d�IU> N � =zR�I � B P= i IU>�IL^4V�@W? N IU^y=7R�=7^%:

Page 38: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

F5� � �E; P� }Ja ���|\�: ; 4�akv �E� � �j���z�&��ak��0 ���Jv��

� �Oi d�@A=h\�: ���]��I N d�IU> N � =zR�I � B P= i IU>�IL^4V�@W? N IU^y=7R�=7^%:

� �Oi d�@A=h\�: � � PacT Z � BC= � BC= i IL>SR�IET�=�^eI N d�IU> N � =�:

Page 39: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

��� ��� ������ � � F5�� ���� ! �! � / � � � �! �$ ���=7^ N ?5> N TOd�^ � ?Q>�IU^4R�IUp#IU@ P� =7> N IU> Z @A=7@W^eIJIU> R�?5^wV�d�> Z ?5^jV�@ � > N � V�=7T�BSIL> Z I5:

� ��=*K#=7p � T � R�=�R�R�IUT8KMT Z @W?�V�=7@A=�>�d�IU^ Z @A= Z =7@WIY=*R�I Z @A= Ny~ IU?� ��=�@WIU> Z =7p � T � R�=�RSR�IUT2d�^e?SR�IUT8KMT Z @W?�V�=7@A=�V�@W? N IU^y=7@�IU> Z � IUBxV#?*@WIY=7T,:��=hK#=7p � T � R�=�R�R�IUT KMT Z @W?�V�d�IUR�I*^eIU@jR�I Z IU@WB � >�=7R�=zR�Ih=7T BxIU>�?5^ Z @WIU^ . ?5@r{

BC=7^%:��� d�IzIUT8KMT Z @W? ��=UX�=�^ � R�?hR � ^eI��>�=7R�? N ?5@W@WI NLZ =7BxIL> Z I� v]>�p�=7^eIE=�T�= Z IU?5@ P� =k^ed�p5X�= N IU> Z I5:� �s?5>k@WIU^ed�T Z =7R�?5^MIU^ Z =7R P� ^ Z �.N ?7^w:(g�d�I4B�d�IU^ Z @WIU>JIUT7R�IU^eIUBxV#I��>�?4R�IUT Z @A= Ny~ IU?

) KMT Z @A=7R�?�V�@W?5V � =7BxIU> Z IwR �ON ��?8+ :v]> IU^ Z I�@WIUV#?5@ Z ISV�@WIU^eIU> Z ?�T�=�K#=7p � T � R�=�RoR�IUT4KMT Z @W?�V#?5@EBxIUR � ?�R�I i @ P=�K N =7^g�d�I�B�d�IU^ Z @A=7> IUTz^eI i d � B � IU> Z ? R�IUTkKMT Z @W? �*=7TOBC=�> @WIU^eV#I NLZ ? R�I�T�=�^eI��>�=7T?5p�^eIU@rtQ=7R�=�:�s?5Bx?�^eIzV#?!R�@ P=*?5p�^eIU@rtQ=7@��5IUT2R�IU^eIUBxV#I��>�?zIU^J= N IUV Z =7p�TOI5:

��=�@WIU> Z =7p � T � R�=�RCR�IUT�KMT Z @W?xIU^ Z =�R�=7R�=�V#?5@JIUT�> Pd�BxIU@W?SR�I�IU^ Z � BC= N � ?Q>�IU^g�d�I*IUT8KMT Z @W?hV�d�IUR�IER�=7@�V#?5@�d�> � R�=7RCR�I Z � IUBxV#?�:��I � �ON � IU@W?5>zV�@Wd�IUp�=7^�=7T!@WIU^eV#I NLZ ?JXhR�IU> Z @W?ER�IUT�=7B�p � IU> Z I�BC= Z T�=7pzIU^4V#?5^ � p�TOIIU^ Z � BC=7@�\5�5�*V�=7@WIU^ )��� � +sIU> BxIUR � ?*^eI i d�>�R�?�:v]^ Z ? IU^ R�IlIU^eV#IU@A=7@W^eI�V�d�IU^ V�d�IUR�I tQIU@W^eI =7T*KMT Z @W? N ?5Bx? d�>�=7T i ?5@ � Z BS?>�d�B PIU@ �ON ? ���� � � 2 ��=7^zIU^ N =7T�=7^j>�?o>�I N IU^y=7@ � =�B�IL> Z I*^e?5> T�=7^kB � ^eBC=�^jX TO?5^k@WIU^ed�T�{

Z =7R�?5^�^ � >�? ^e?5> ?5p�^eIU@rtQ=7R�?5^ Z IU> � IU>�R�? IU> N d�IU> Z = PIU^ Z ?�V#?!R�@ P� =7> V�=7@WI N IU@IU@W@ P?5>�IU?5^%:

���,eOX�TUPWI�KMe1ISPl}OXxYL �J`WLVE�f�eOX+P $�P]TgfSbMe�y) '"K NcP]T1�JLOXQ}VegP]N�bMLgb K FQP]XQLOFDHSXQL1NJeUEDP�`WeONJEGKMISP]XQL1HSNcLObM}VeOXDK FDTUegN3HST)YP]XDKM`We�hSTULVE��SKMP]N

PWE PWEGFQe"`�YLVEGFDKM`We�y

Page 40: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

\5� � �E; P� }Ja ���|\�: ; 4�akv �E� � �j���z�&��ak��0 ���Jv��

60 70 80 90 100 110 120 130 14044

44.2

44.4

44.6

44.8

45

45.2

45.4

45.6

45.8

46This is the ANT being tracked !....

X position in pixels

Y P

ositi

on in

pix

els

� �Oi d�@A= \�: ����GJIU^ N @ � V N �YP?Q>�R�IUTjBx?Yt � B � IU> Z ?�R�IUTj?5p�qrI Z ? IU> IUTjV�T�=7>�? ��:v]Tw?5p�qrI Z ? V�=7@ Z I =7TO@WIUR�IUR�?5@zR�I T�= N ?!?5@WR�IU>�=7R�= ) �5\�� `�� +JXl^eICBC=7> Z � IU>�IST�=V#?5^ �ON ��P?Q>�� N ?5>�^ Z =7> Z IJV#?5@j=7T ijPd�> Z � IUBxV#?�:

Page 41: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

��� ��� ������ � � \�9

0 2 4 6 8 10 12 14 16 1860

70

80

90

100

110

120

130

140

150

Time (sec)

X P

ositi

on (

pixe

ls)

True Estimated

� �Oi d�@A=h\�: ���*4wIU^ed�T Z =7R�?*R�IEKMT Z @A=7@fT�=*^eI��>�=7T N ?5@W@WIU^eV#?5>�R � IL> Z Ic=�T�=h=7p�^ N � ^y=�:

Page 42: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

\5F � �E; P� }Ja ���|\�: ; 4�akv �E� � �j���z�&��ak��0 ���Jv��

0 2 4 6 8 10 12 14 16 1843.5

44

44.5

45

45.5

46

46.5

Time (sec)

Y P

ositi

on (

pixe

ls)

Pixel position of the object been tracked, it goes as the y component of the centroid

True Estimated

� �Oi d�@A=j\�:.9Y��� 4wIU^ed�T Z =7R�?�R�IfKMT Z @A=7@2T�=�^eI��>�=7T N ?5@W@WIU^eV#?5>�R � IL> Z I]=jT�=�?5@WR�IU>�=7R�=�:��=*IU^ N =7T�=JIU>CT�=ER � @WI NUN ��P?Q> �[IU^�R�I*\�V � ��IUTOIU^%:(v]^ Z ?�^ �Oi > � K N =cg�d�IzIUT8?5p�qrI Z ?>�?S^eIEBx?Yt ��P?ER�IUBC=7^ � =�R�?EIU> IU^ Z =�R � @WI NUN ��P?Q> :

Page 43: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

��� ��� ������ � � \5\

0 2 4 6 8 10 12 14 16 180

1

2

3

4

5

6

7

8

9

10

Time (sec)

Pos

ition

Mean squared estimation uncertainties

� �Oi d�@A=J\�:.959 �(v]@W@W?5@]IU>�T�=7^�IU^ Z � BC= N � ?Q>�IL^%:2�kp�^eIU@rtQI�g�d�IcIUT!V�@W? N IU^e? Z � IU>�Isd�>Z @A=7>�^ ��Z ?5@ � ?*=7T � > �ON � ?kR�IUT8KMT Z @A=7R�?��3��� �������6���! "����$�%&�' ��(�*�*��+,$����(.#/#�0$�13+4.056%&.1�.0$,7& ��(�L: v]^ Z ?lIU^�qrd�^ Z =7BxIU> Z I TO?�g�d�I ^ed N IUR�I5: ��=�^eI N d�IU> N � =oR�I . @A=7BxIU^N ?5@W@WIU^eV#?5>�R�I N ?5>CIUT2IrqrIzR�IUT Z � IUBxV#?x:

Page 44: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

\7` � �E; P� }Ja ���|\�: ; 4�akv �E� � �j���z�&��ak��0 ���Jv��

Page 45: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� � ���� � � � �

� ���� ����

v]>�IU^ Z I*=7V PIU>�R �ON Ic^eIzIU> N d�IU> Z @A=�IUT N�P?5R �Oi ? N ?5BxV�TOI Z ?ER�IET�=�=7V�T �ON = N �%P?Q>SR�IU^W{N @ ��Z =�IU> N =7V P� Z d�T�?Q^#=7> Z IU@ � ?5@WIU^%:#v]T N�P?5R �Oi ?4^eI4IU> N d�IU> Z @A=sR�? N d�BxIU> Z =7R�?�^eI ijPd�>^eI ��= N ?5>�^ � R�IU@A=7R�?zV#IU@ Z � >�IU> Z I5:Pv]^ Z I�R�IUp#I�IrqrI N d Z =7@W^eISIU> IUTw=7B�p � IU> Z I�R�I�n = Z T�=7p�m�: \ X�^eI i d�@A=7BxIU> Z Ih^eIIrqrI N d Z =S^ � >oBx?!R � K N = N � ?Q>�IL^�^ed�p�^ Z =7> N � =7T�IL^�IU> tQIU@W^ � ?5>�IU^J=7> Z IU@ � ?5@WIU^�?CV#?5^W{Z IU@ � ?5@WIU^%:}�IU> i =7^eI IU> N d�IU> Z = g�d�I ^eIl@WIUg�d � IU@WI�IUT Z ?!?5TOp#?Y� R�I�V�@W? N IU^y=7B � IL> Z ?lR�I� B P= i IL>�IU^4R �Oi ��Z#P=�T.IL^ ' :v]> V�@ � > N � V � ? IUTJ^ � ^ Z IUBC= N ?5@W@WI�IU> N d�=7TOg�d � IU@hV�@W? N IU^y=7R�?5@�g�d�I V�d�IUR�=IrqrI N d Z =7@fT�=*tQIU@W^ ��P?5>�V#IU@ Z � >�IU> Z IJR�I�n = Z T�=7p :;�=7@A= N d�=7TOg�d � IU@ N ?5BxIU> Z =7@ � ?zR � @ � qe=7^eIJ=hB � R � @WI NUN ��P?Q>SR�I N ?5@W@WIU?�:

{ 4E: �8: n :@W^e?5T�=7>�?��J^ � d : p�d�=7V : BS�

^eIUV Z � IUB�p�@WIJF5�5��9

B���KM}OK FQLOb�opTULV}VP �wXQe"`WPWEDEGK NJ}�y

\5m

Page 46: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

\�� �) 6����� 0 � � � � 6� � 0�� �� ��� � � �� � � � � � ������� ��� � � ��� ��� ��� ����� ������������� ����� ���� �! ���"�#�$� �%&��'�(�)�*,+-(, �� .0/21

/-$�324

���/-$���/,5-'��6�7.�3�124

'��6�8$,'��6�, 89�� .,:!�;/-<=:�>���/=�-:6?>+�@6<=:!124

8�6A8��B$C�;/,�-(�9�" .!'��6�&124

�-$�DE4

F �;�-",� F,G ��H,(�$6I 4

<6�&��'J$K�8<*,58<6�-9�� .; 8�6A8��&124

'6 896A8(�$�324

'�/&96A�A�$L�;/,�-(�',��H,(�.�<6�&��'M�N'6 896A8(,124

����<,�-(�.0/O1

�;/&'�%&� F .!'�/&96A�A-124

�>� �! ���"�#�$��;/�@O���,(6A�4

/&9�$/&9�� .�" .�P��Q3�1�124

/O��$/O�;�R.�" .�P��Q3�1�124

�-(�<������-$,'�/&96A�ATS�$B/O�VUW'�/&96A�A8X�$/&9M4

��YZ=�Q�;�-"�#[$B* F '�(6A8(6 �� .\�-(�<������=��� �! ��>�O124

���E����#�$����;�-" .�YZO124

Page 47: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� � ���k� ���� � � ���� � ��� ����� ��� ���� �� \��

�/O�;�-$/O�;�R.���124

�/&9���$/&9�� .���124

�/O�;�-$/O�;�R.���124

�/&9���$/&9�� .���124

�� �$������ .�.��/&9������/O�;�O1M?��5&1����/2�;�=4

�� �$������ .�.��/&9������/O�;�O1M?��5&1����/2�;�=4

�,�� ��$��� �� F �;�-",� F,G ��H,(EP��� �� F �;�-",� F&G ��H&(

�,��T+8$��� �� F �;�-",� F,G ��H,(EP��� �� F �;�-",� F&G ��H,(

'�/&96A�AO.��2��+&1�$65�I�I 4

(��-"

(��-"

�,�� ��$��� �� F �;�-",� F,G ��H,(EP��� �� F �;�-",� F&G ��H&(

'�/&96A�AO.��2���� -1�$� 4

(��-"

�,��T+8$��� �� F �;�-",� F,G ��H,(EP��� �� F �;�-",� F&G ��H&(

'�/&96A�AO.��� ���+&1�$� 4

(��-"

�;/&'�%&� F .�YZO1

Page 48: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

\5� �) 6����� 0 � � � � 6� � 0�� �

/-$/ ��324

����<,�-(�.0/O1

�;/&'�%&� F .!'�/&96A�A-124

Page 49: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� �.��� � ��� � ��� � � � ����� ��" ������� � ����� � ������ � ����� � � � \5�� ��� � ��� ��� � ��� � ������ � � �� � � ����� � ���� � ����� � � �� !������� �������� ���%��,( F ����&( F ���%��E���@=�N(�#$�@-",9��,(��&96A�/&9��R.��,�M���%��6�-(E����@,�-(=���E� �M���E���%O���O1

H $B�,��.�3MP>5&124

� $��@,�-(���=:��,�;�� .�.�����@,�-(���=:�����1�1�4

�%��,( F $T�%��6�-( ������.�H�������%��6�-(�124

��&( F $��@,�-( ���������@,�-(E4

� �%O��'��,(�(�@[�%&(J/&9��6�&���B@&��',������,(B",(����;�O���,(

��&( F $ ?>I���.���&( F ����&( F :!124

� @,�-(�����'

�%�� $��%O�������%��,( F 4

�@ $��%O�������&( F ��%O���=: ���M4

(�$,9�*&'�.��%��,( F .�3�1 ���,��.�3�1�1 ���,��.�3�124

Page 50: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

`�� �) 6����� 0 � � � � 6� � 0�� �� �� � � � ��� � ������ � � � ��� ����� � ���� ��������������-�,' Z��� � ��-� G � ������������ ?

� �,����A-�;�&(�9�K(�'����;/&9�������� ���W9 /&���;�-<��*,+-(6 �� F ���% 8���&'��,9��-�C96 � 8(6A8(�-9�������� F ���%� � � ?

� �,'K�;��@�-�L���K�-(6 -��,(�' �%&(K 8(��-�6�-�,��" �.��E����1V@O���,(6A � ���T�%&(T��*,+-(6 ��[�,�� F 9��-�T�,�J�6�-96 �E�

� �%&9��B@O���,(6AJ�-(�@,�-(�'�(��-�,'B�%&(T��*&'�(� ,9��������.��%&(J�6��&(�,96A�&(,1 �,� *&(J���A��,(�-(�" ?

� ��T'����;/&9��,(�" ? )��� 8(J�,��T%&9,(['��@�@�A��

� '���/&(T��*&'�(� ,9��������&96A '�9�/�@�A8(�' �,�� 89�� ' F ���� �%�,�J�,��,� <&(�'�',�;�-<,'K9�*&��-�T�%&(K 8(��-�6�-�,��"

� '��B9�'J�,�T'�@&(�(�"[�%&(J�6�-96 ���;�-<E� �%&(��K�%&��'�(,96A�&(�'J*&(6 8��/&( �%&( �&�,��'��T",9��,9 .�H�1M?

� 9��-"T�%&(W�;��@�-�T�,� �&96A�/&9��K���A��,(�=?

� )�� 8��,�-'�( �,�� 89��L�;�-�,(�, 896A89��,( '���/&(J�6��&( 8(��-�6�-�,��",'K9��-"W'���/&(J<&(�'�'�(�'E?��&� �&��� F 9���O�;�-<E�

� <&(�'�'�(�'[9�-(W�;�&',��",( �%&( F �;�-",� F 9��-" ��� �&���E��&96A�/&9��[@�&'�%&(�' �%&(�/ ?

� ��� 89�8",� G �6A89��&���&���,+-(

� �;�&'������-�,���,96 -�����&96AJ",(��,'��6�-������',� 89W)�@-��� 89J����A8(6 ��6�-���O� 89

� �6�-��<6�-9�/�/&(�" ����-<&'�� ?>5 � �&3

Page 51: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� ����� �U��� � � � ����� � �� ��� ����� `�9

��� (���@&�6A�",� ��A��,9�/O���-9��&���,��*�A8(�'��%�� ?

� ���C��' *&9�'�(�" ���[�%&( *&����

� : �&96A�/&9�� ���A��,(�&�;�-< P � %&(���8� 9��-"��6�-96 ���� 8(M�-:!124

� : *-� � ? G ?��6�-( F 96AB9��-"�� ? � ? ��-"6�-( F 'M�-:!124

� : @��*�A-��'�%&(�"[*-�BZO�A8(��E� 5 � � �=?8:!124

� )�� 8��,�-'�([9�'T96A F 9��,'M� � %&9,(J�,����-" '���/&((���-��-' �;�[�%&(K 8��",(T��[��/O��'�',�����&'

� F %O� �% � %&9,(J�����,(�" 9��-"B@&��'�',�;*�A8( � %&9,(�;�-�6�-��"� 8(�"C'���/&(J��A89 F 'E?

� ��-� F 9��E� ��� F �� �,'��

"���'�@R.,: � � � � � � � � � � � � � � � � � � � � � � � �� � � ������������������������� � � � � � � � � � � � � � � � � � � � �R:!124

"���'�@R.,: � %O��' F ���A�"T*&(B9�� 9�@�@,�-��@,�&��9��,( /&��",(6A �,��R:!124

"���'�@R.,:>�6�-96 ���;�-< ��*,+-(6 ��,' F ���% 9 � ��� 89�/&(�-9 �;�W�-(�96A������;/&( :!124

"���'�@R.,:���� 8��,�-'�( �,��T�&(�(�"[�,�[A8(�9���C9KA-������A8(��� �;/&9�<,( @,�-�6 8(�'�',�;�-<C9��-"��&96A�/&9�� ���A��,(�-' :!124

"���'�@R.,:�'�(�(B�,�� :!124

��� %&(T��*,+-(6 ��B*&(,�;�-<[�6�-96 �,(�"[�,�6A�A8� F ' �%O��' "���&9�/O� B(���&9��������&'

��� .�� ��3�1V$�� � .���1 � F .���1 F�� � . � � �,1

Page 52: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

`�F �) 6����� 0 � � � � 6� � 0�� �

��� .���1V$�� � .���1 �� .���1 � � . � ����1

� 9��-" �;�O������96AB'��,9��,(J� . �&1�$ . � �M� � �M� � � :� � � :!1� .!9�'T9��-�T"������,(�-(��-����96A[(���&9��������O1

��� %&(J�,�6A�A8� F �;�-< (���&9�������� 8���&'��6�-9,�;�&'B�%&(J"���&9�/O� 896A� *&(�%&9�����W���T�%&( /&���;�-< ��*,+-(6 ��

� F ���% 8���&'��,9��-� 96 � 8(6A8(�-9��������=� F %O� �%[/&(�9��&'�%&9��T�,�� 89��[�,�6A�A8� F 9�� ��*,+-(6 �� 9�'K �A8��'�(�" 9�' �,��

� /&��",(6A[���,'T'�@&(�(�" ? ����<%-� ?

� "658����"��658$,9 ?�?�?�?�?�?�?�?�?��%&9��,'W96A�A �

� �,9�-9�/&(��6�-��' ",(6A /&��",(6A8�

�A8(�9�K96A�A��! �A8��'�([96A�A�4

�,���,96A-�;/&9�<,(�'�$�3�� 4

F �;�-",� F,G ��H,(�$6I 4

��D $K(��,(�.�D�124 � (�'��,9 /&9��6�&��HL��",(��-����",9�"'�( -���A-��H,9J@&9�-9T(6AT 896A� ��A8� ",(��

� $C324 � �;�-�,(�-'�9�/�@�A8(B���;/&(W�;�-�,(� ,96A .!',1

�-9��,(�$�� �&324

Page 53: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� ����� �U��� � � � ����� � �� ��� ����� `�\

� $C3��(��,(�.\5&124 � /&(�9�'�,�-(�/&(��-�K�&�,��'�(J�%&(��*&'�(� ,9��������K/&��",(6AB����A��C 8���&',��",(�-' .��E����1

� $ �=?N3� ��DE4 � 8���-���;��&��&' @,�-�6 8(�'�'�&�,��'�(W 8�,9�&��9��� 8(

��$ �N32� � � � � � 4 � �Q32� � � �8#E4 � /&(�9�'�,�-(�/&(��-�'�(��&',������������[/&9��6�&���

� $ �N3 � � � � � � � 4

� �Q3 � � � � � 4

� � � �Q3 � � � 4

� � � � � �Q3 �8#E4 � �%O��'W��'J�%&(K 8����-(6A89��������K/&9��6�&���

� F %O� �% ��'W�;�O��9����A-��H,(�"[*&9�'�(�"W���[�%&(W�;�O������96A � � �M?�(E?��� . �&1

� ���� ����� �#�$����;�-"��-(��-�6�-�,��",(�.N�E���E�N� �! ��0"�1

� �,��.�3�1�$��� �4��,��.\5&1�$��� �4

��� %O��'W�;/�@�A8(�/&(��-�,9��������W&'�(�'J�%&(T(��,96 �� '��6A�-�������K�,��

� �%&(�/T/&9��6�&��� (��@&���&(��-����96A�� (��@R. � � ��1 �,�� �%&(T'��,9��,(

� �6�-9��&',���������[/&9��6�&���E� ",(�&��,(�"T&',�;�-<

� �&9��%&(�/&9���� 89B�,�� G �-",(��-�,'

�%O���T$ �N32� � � � � � 4

Page 54: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

`5` �) 6����� 0 � � � � 6� � 0�� �

� �Q32� � � � 4

� � � �Q32� � 4

� � � � � �Q3�#E4

"���'�@R.,:�. G �;/��A89����;�-< � � � � � � � � � � � � � � � � � � � � � � ��1�:!1�4

��$� 4���3�$� 4��658$� 4����8$� 4���D�$� 4�%O3�$� 4��%,58$� 4��% �8$� 4��%-D�$� 4@O3�3�$� 4>@,5�58$� 4>@ � �8$� 4>@-D�D�$� 4

���

/-$�324

���/-$���/,5-'��6�7.�3�124

'��6�8$,'��6�, 89�� .,:!�;/-<=:�>���/=�-:6?>+�@6<=:!124

8�6A8��B$C�;/,�-(�9�" .!'��6�&124

Page 55: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� ����� �U��� � � � ����� � �� ��� ����� `�m

�-$�� 4

F �;�-",� F,G ��H,(�$6I 4

<6�&��'J$K�8<*,58<6�-9�� .; 8�6A8��&124

'6 896A8(�$�324

'�/&96A�A�$L�;/,�-(�',��H,(�.�<6�&��'M�N'6 896A8(,124

����<,�-(�.�3�1

�;/&'�%&� F .!'�/&96A�A-124

�>� �! ���"�#�$��;/�@O���,(6A�4

/&9�$/&9�� .�" .�P��Q3�1�124

/O��$/O�;�R.�" .�P��Q3�1�124

�-(�<������-$,'�/&96A�ATS�$B/O�VUW'�/&96A�A8X�$/&9M4

��YZ=�Q�;�-"�#[$B* F '�(6A8(6 �� .\�-(�<������=��� �! ��>�O124

���E����#�$����;�-" .�YZO124

�/O�;�-$/O�;�R.���124

�/&9���$/&9�� .���124

�/O�;�-$/O�;�R.���124

�/&9���$/&9�� .���124

�� �$������ .�.��/&9������/O�;�O1M?��5&1����/2�;�=4

�� �$������ .�.��/&9������/O�;�O1M?��5&1����/2�;�=4

Page 56: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

`�� �) 6����� 0 � � � � 6� � 0�� �

�,�� ��$��� �� F �;�-",� F,G ��H,(EP��� �� F �;�-",� F&G ��H&(

�,��T+8$��� �� F �;�-",� F,G ��H,(EP��� �� F �;�-",� F&G ��H,(

'�/&96A�AO.��2��+&1�$65�I�I 4

(��-"

(��-"

�,�� ��$��� �� F �;�-",� F,G ��H,(EP��� �� F �;�-",� F&G ��H&(

'�/&96A�AO.��2���� -1�$� 4

(��-"

�,��T+8$��� �� F �;�-",� F,G ��H,(EP��� �� F �;�-",� F&G ��H&(

'�/&96A�AO.��� ���+&1�$� 4

(��-"

�;/&'�%&� F .�YZO1

����<,�-(�.\5&1

�;/&'�%&� F .!'�/&96A�A-124

�,��.�3�1�$��� �4

�,��.\5&1�$��� �4

�,��. �&1�$�324

Page 57: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� ����� �U��� � � � ����� � �� ��� ����� `!�

�,��.�D�1�$� 4

�,��$��,� :4

�%-$��,�M4

� .�3�1�$ � � � 4

�/R.0/O1 $B� .�3�124

�%O3�.0/O1 $B�%R.�3�124

�%,57.0/O1 $B�%R.\5&124

�% �7.0/O1 $B�%R. �&124

�%-D .0/O1 $B�%R.�D�124

@O3�3�.0/O1 $�� .�32�Q3�124

@,5�57.0/O1 $�� .\5 ��5&124

@ � �7.0/O1 $�� . � � �&124

@-D�D .0/O1 $�� .�DE��D�124

��3�.0/O1 $B�,��.�3�124

�657.0/O1 $B�,��.\5&124

���7.0/O1 $B�,��. �&124

��D .0/O1 $B�,��.�D�124

Page 58: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

`�� �) 6����� 0 � � � � 6� � 0�� �

�%��6�-(�$��%=4

�@,�-(�$��E4

(�$� 4

'�$65 4

�-9��,(�$��,���,96A-�;/&9�<,(�' 4

���

�,�����$65=P>�-9��,(

� .���1 $ � ��.����,3�124

��3�.���1 $B�,��.�3�124

�657.���1 $B�,��.\5&124

���7.���1 $B�,��. �&124

��D .���1 $B�,��.�D�124

� 9 @,�&����&��,96A�&(�'

Page 59: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� ����� �U��� � � � ����� � �� ��� ����� `��

� 9 @&��'��,(�&����&� ,96A�&(�' �,�6A�A8� F �;�-<��*&'�(� ,9��������&96AJ�@-",9��,(

���%=���E���%��6�-(E���@,�-(E�N(�#�$�@-",9��,(��&96A�/&9��R.��,�M���%��6�-(E���@&�-(E���E� � ���E����%O����1�4

�/R.���1 $B� .���124

�%O3�.���1 $B�%R.�3�124

�%,57.���1 $B�%R.\5&124

�% �7.���1 $B�%R. �&124

�%-D .���1 $B�%R.�D�124

@O3�3�.���1 $�� .�32�Q3�124

@,5�57.���1 $�� .\5 ��5&124

@ � �7.���1 $�� . � � �&124

@-D�D .���1 $�� .�DE��D�124

Page 60: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

m5� �) 6����� 0 � � � � 6� � 0�� �

� �@-",9��,(T���T�6��&(['��,9��,( Z�� � �,)�� � �&�,��'�(

� ���� ����� �#�$����;�-"��-(��-�6�-�,��",(�.��E���E���R�! ��0"�1

� �,��.�3�1�$��� �4��,��.\5&1�$��� �4

� � � � � ������������� ��� ��� ��� ��� ��� ��� ��� ��� ��� ��� ��� ��� ��� ����� ���

���/-$���/,5-'��6�7.���124

'��6�8$,'��6�, 89�� .,:!�;/-<=:�>���/=�-:6?>+�@6<=:!124

8�6A8��B$C�;/,�-(�9�" .!'��6�&124

<6�&��'J$K�8<*,58<6�-9�� .; 8�6A8��&124

'6 896A8(�$�324

'�/&96A�A�$L�;/,�-(�',��H,(�.�<6�&��'M�N'6 896A8(,124

� 9���-9�� ���T<6�-9��,'M� �%O��'W��'T9�'B9F 9��W���T�%,�-(�'�%&�6A�"��;�-<

/&9�$/&9�� .�" .�P��Q3�1�124

/O��$/O�;�R.�" .�P��Q3�1�124

�-(�<������K$[�-�,� 8�6A8��7.�<6�&��'M�>/O�2�>/&9,124

Page 61: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� ����� �U��� � � � ����� � �� ��� ����� m�9

�8$6����� 4

�$� ���� 4

��YZ=�Q�;�-"�#[$B* F '�(6A8(6 �� .\�-(�<������=��� �! ��>�O124

���E����#�$����;�-" .�YZO124

�/O�;�-$/O�;�R.���124

�/&9���$/&9�� .���124

�/O�;�-$/O�;�R.���124

�/&9���$/&9�� .���124

�� �$������ .�.��/&9������/O�;�O1M?��5&1����/2�;�=4

�� �$������ .�.��/&9������/O�;�O1M?��5&1����/2�;�=4

�,�� ��$������ .��,��.�3�1�1 � F �;�-",� F,G ��H,(EP��O��� .N�,��.�3-1�1� F �;�-",� F,G ��H,(

�,��T+8$������ .��,��.\5&1�1 � F �;�-",� F,G ��H,(EP��O��� .��&��.\5&1-1� F �;�-",� F,G ��H,(

'�/&96A�AO.��2��+&1�$65�I�I 4

(��-"

(��-"

�,�� ��$������ .��%R.�3�1�1 � F �;�-",� F,G ��H,(EP��O��� .N�%R.�3-1�1� F �;�-",� F,G ��H,(

Page 62: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

m5F �) 6����� 0 � � � � 6� � 0�� �

'�/&96A�AO.��2������� .��%R.\5&1�1����&1�$� 4

(��-"

�,��T+8$������ .��%R.\5&1�1 � F �;�-",� F,G ��H,(EP��O��� .N�%R.\5�1�1� F �;�-",� F,G ��H,(

'�/&96A�AO.������ .��%R.�3�1�1���� ��+&1�$� 4

(��-"

'�$,' ��324

� ����<,�-(�.!',1 � '�9,( ���;/&(

� �;/&'�%&� F .�YZO1

'�$,' ��324

����<,�-(�.!',1

�;/&'�%&� F .!'�/&96A�A-124 � '�9,([���;/&(

�,��.�3�1�$��� �4

�,��.\5&1�$��� �4

� ��� ��� � � � ��� ��� �������������������������������������������

Page 63: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� ����� �U��� � � � ����� � �� ��� ����� m5\

(��-"E4 � �,��

(6A89�@-$��,�6 �4

����<,�-(

@�A8��� .��65 ����3�124

������A8(�.,: � %O��'C��'J�%&( ��� � *&(,�;�-<[�6�-96 �,(�" �?�?�?�?8:!124

��A89�*&(6AO.,: � @&��',��������� �;�T@O���,(6A8' :!124

��A89�*&(6AO.,: � �,��',��������� �;�T@O���,(6A8' :!124

����<,�-(

@�A8��� .��E����32�-: ��R:���/=���%O32�-:6P / :!124

A8(�<,(��-" .,: � ��&( :�-: �,'����;/&9��,(�"=:Q124

��A89�*&(6AO.,: � �;/&( .!'�(6 -1�:!124���A89�*&(6AO.,: � �,��',���������.0@O���,(6A8',1�:!124�������A8(�.,: � ��&(C9��-"W(������;/&9��,(�"C'��,9��,(�'E?.��6�-(�'�'[X ��� � ���,S �,�K 8���-���;��&(E?N1�:!124

������A8(�.,: �����,(6A @&��',��������� ���T�%&(T��*,+-(6 ��*&(�(��[�6�-96 �,(�"E�V���T<,��(�'T9�'J�%&(J� 8��/�@&���&(��-����T�%&(K 8(��-�6�-�,��"=:!124

����<,�-(

@�A8��� .��E���65 �-: ��R:���/=���%,5 �-:6P / :!124

A8(�<,(��-" .,: � ��&( :�-: �,'����;/&9��,(�"=:Q124

Page 64: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

m7` �) 6����� 0 � � � � 6� � 0�� �

��A89�*&(6AO.,: � �;/&( .!'�(6 -1�:!124��A89�*&(6AO.,: � �,��',��������� .0@O���,(6A8',1�:!124

����<,�-(

@�A8��� .��/=�>@O3�3�124

��A89�*&(6AO.,: � �;/&( .!'�(6 -1�:!124���A89�*&(6AO.,: �,��',��������� :!124

������A8(�.,: �&(�9�� '���&9�-(�" (�'����;/&9��������K��� 8(�8�,9,�;�-����(�' :!124

"���'�@R.,: ��A89�@&'�(�"W���;/&(J�,� @,�-�6 8(�'�'.\�-(�9�"E� � �%&9����&(6A ",(6 8��/�@&��'�(M� '�(�</&(��-�E�� ��(�'����;/&9����;�-<E��"���'�@�A89���1C3��W 8(��-�6�-�,��"����6�-9�/&(�' :!124

(6A89�@

"���'�@R.,:[:!124

"���'�@R.,: � � <���,( �,�� 9��-" ��",(�9W����,9�,(�'['���/&(����;/&(�' �=? ��5������['�(6 8' �,�T��@&(��9 F �;�-",� F �;���&9���A89�* � � � ��:!124

"���'�@R.,: %&(�-(C��� ��' :!124

��� � ����<,�-(M� �,�6

"���'�@R.,:6?�?�?�?�?�?�?�?�?�?�?�? � %&9�� ��' 96A�A�� ?�?�?�?G (�( � �� A89��,(�=?�?�?�?�?�?�?�?�?�?�?�?8:!124

Page 65: Visión Dinámica, Trackeo de Objetos con Filtros Kalman, Bachelor Thesis by Ricardo Solano Monje

� ��� ��� ��� �� � � m5m� ��� � � � � ��� �

� � � ��� �: ��:�� � N ? Ny~ I Z =7T,: � 1�1 � 7 �*���,/#������� +4.��*��$,$,7 5 �7: n N k@A= �j{� � TOT � 0 > N : �29Y�5����:

� � � ��� �kTO?5>�^e?kn :�� � >�>�I Z =7T,:�� � $,7 �*�� � �*� ��05�7 �*�7: �kR�R � ^e?5> ��9Y�5����:� � �5� no? � � >�R�IU@(�8: k@WI��j=7TQI Z =7T,:�� ��� /#�05 ��7 �(%&��+,7 5 �i2 � � �*.0+����056�

� +4����% 7 �*�L:�� � TOILX�� 0 > N : ��F5�5�5��:� � ?5@A�5� n = Z ��� ?5@ ~ ^%:��,/#������� +4.��*��$,$,7 5 � ��.�.�� ".��7: n = Z ��� ?5@ ~ ^��0 > N : �MF5�5�5��:��� � T���� GJIU>�> � ^ �: � � TOT,:�� ��������7 .056��$ �07 � ��+4��56��7 ���(��$ �*.05 �,1 � 7 �*����7 .056��$

��� /#.������(����.7:l} ��?5Bx^e?5> �M9Y�5���!: