# 1

$_{5}^1T = \left(\begin{array}{cccc} \cos\left(t_{2}+t_{3}+t_{4}\right)\,\cos\left(t_{5}\right) & -\cos\left(t_{2}+t_{3}+t_{4}\right)\,\sin\left(t_{5}\right) & \sin\left(t_{2}+t_{3}+t_{4}\right) & 251\,\sin\left(t_{2}+t_{3}+t_{4}\right)+350\,\cos\left(t_{2}+t_{3}\right)+300\,\cos\left(t_{2}\right)+50\\ -\sin\left(t_{5}\right) & -\cos\left(t_{5}\right) & 0 & \frac{353}{10}\\ \sin\left(t_{2}+t_{3}+t_{4}\right)\,\cos\left(t_{5}\right) & -\sin\left(t_{2}+t_{3}+t_{4}\right)\,\sin\left(t_{5}\right) & -\cos\left(t_{2}+t_{3}+t_{4}\right) & 350\,\sin\left(t_{2}+t_{3}\right)-251\,\cos\left(t_{2}+t_{3}+t_{4}\right)+300\,\sin\left(t_{2}\right)\\ 0 & 0 & 0 & 1 \end{array}\right)$

It can be observed that $T_{15}(2,4)=-d_2$.
$T_{05}=T_{01}T_{15}$
$T_{15} = T_{01}^{-1}T_{05}=\left(\begin{array}{cccc} \cos\left(t_{1}\right) & \sin\left(t_{1}\right) & 0 & 0\\ -\sin\left(t_{1}\right) & \cos\left(t_{1}\right) & 0 & 0\\ 0 & 0 & 1 & -\frac{717}{2}\\ 0 & 0 & 0 & 1 \end{array}\right)\left(\begin{array}{cccc}r_{11}&r_{12}&r_{13}&p_x\\r_{21}&r_{22}&r_{23}&p_y\\r_{31}&r_{32}&r_{33}&p_z\\0&0&0&1\end{array}\right)$

$-d_2=-\sin(t_1)p_x+\cos(t_1)p_y$
From lecture slide:
==$t_1=Atan2(p_y,p_x)-Atan2(-d_2,\pm\sqrt{p_x^2+p_y^2-d_2^2})$== Two possible solutions for $t_1$.

From the above calculation, all elements of $T_{15}$ are known.
==$t_5=Atan2(-T_{15}(2,1), -T_{15}(2, 2))$==

Because all elements of $T_{15}$ are known.
$_{5}^1T(1,4)=d_5 {}_{5}^1T(1,3)+a_3\cos(t_2+t_3)+a_2\cos(t_2)+a_1$
$_{5}^1T(3,4)=d_5 {} _{5}^1T(3,3)+a_3\sin(t_2+t_3)+a_2\sin(t_2)$
$K_1=_{5}^1T(1,4)-d_5{}_{5}^1T(1,3)-a_1=a_3\cos(t_2+t_3)+a_2\cos(t_2)$
$K_2=_{5}^1T(3,4)-d_5{}_{5}^1T(3,3)=a_3\sin(t_2+t_3)+a_2\sin(t_2)$

$\begin{split}K_1^2+K_2^2&=a_3^2+a_2^2+2a_3a_2\cos(t_2+t_3)\cos(t_2)+2a_3a_2\sin(t_2+t_3)\sin(t_2)\\&=a_3^2+a_2^2+2a_3a_2(\cos(t_2)(\cos(t_2)\cos(t_3)-\sin(t_2)\sin(t_3))+\sin(t_2)(\sin(t_2)\cos(t_3)+\cos(t_2)\sin(t_3)))\\&=a_3^2+a_2^2+2a_3a_2\cos(t_3)\end{split}$
==$t_3=\pm Acos(\frac{K_1^2+K_2^2-a_3^2-a_2^2}{2a_3a_2})$==

$\begin{split}K_1&=a_3(\cos(t_2)\cos(t_3)-\sin(t_2)\sin(t_3))+a_2\cos(t_2)\\&=a_3\cos(t_3)\cos(t_2)-a_3\sin(t_3)\sin(t_2)+a_2\cos(t_2)\\&=(a_3\cos(t_3)+a_2)\cos(t_2)+(-a_3\sin(t_3))\sin(t_2)\\&=c_1\cos(t_2)+c_2\sin(t_2)\end{split}$

$\begin{split}K_2&=a_3(\sin(t_2)\cos(t_3)+\cos(t_2)\sin(t_3))+a_2\sin(t_2)\\&=a_3\sin(t_2)\cos(t_3)+a_3\cos(t_2)\sin(t_3)+a_2\sin(t_2)\\&=(a_3\cos(t_3)+a_2)\sin(t_2)+(a_3\sin(t_3))\cos(t_2)\\&=c_1\sin(t_2)-c_2\cos(t_2)\end{split}$

From either one of the above equations:
In this case:
$-c_1\sin(t_2)+c_2\cos(t_2)=-K_2$
$-\sin(t_2)c_1+\cos(t_2)c_2=-K_2$
From lecture slide:
==$t_2=Atan2(c_2,c_1)-Atan2(-K_2,\pm\sqrt{c_1^2+c_2^2-K_2^2})$== Two possible solutions for $t_2$.

Because all elements of $T_{15}$ are known.
$T_{15}(1,3)=\sin(t_2+t_3+t_4)$
$T_{15}(3,3)=-\cos(t_2+t_3+t_4)$
$t_2+t_3+t_4=Atan2(T_{15}(1,3),-T_{15}(3,3))$
Because $t_2$ and $t_3$ are known, ==$t_4=Atan2(T_{15}(1,3),-T_{15}(3,3
))-t_2-t_3$==.

Life is so HARD!
It seems that $t_{11},t_{21},t_{311},t_{41},t_{51}$ is a set of acceptable solution.

==Please refer to the code for better understanding of the notation.==

$\hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1}$

$P_{k}^{-}=AP_{k-1}A^{T}+Q$

$K_{k}=P_{k}^{-}H^{T}(HP_{k}^{-1}H^{T}+R)^{-1}$

$\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}(z_{k}-H\hat{x}_{k}^{-})$

$P_{k}=(I-K_{k}H)P_{k}^{-}$

State vector $x=[P_x\ P_y\ P_z\ V_x\ V_y\ V_z]^{T}$
$P_x,\ P_y,\ P_z\ :$ coordinates in $\mathbb{R}^{3}$
$V_x,\ V_y,\ V_z\ :$ velocity along $x,\ y,\ z$ direction

$P_x^{+}=P_x^{-}+V_x^{-}\Delta{t}$
$P_y^{+}=P_y^{-}+V_y^{-}\Delta{t}+0.5g\Delta{t}^{2}$
$P_z^{+}=P_z^{-}+V_z^{-}\Delta{t}$
$V_x^{+}=V_x^{-}$
$V_y^{+}=V_y^{-}+g\Delta{t}$
$V_z^{+}=V_z^{-}$

With initial state $[P_x\ P_y\ P_z\ V_x\ V_y\ V_z]^{T}=[1\ 1\ 1\ 1\ 20\ 1]^{T}$,
covariance of the process noise $=0.1I$,
covariance of the observation noise $=0.01I$,
and measurement subject to nosies which are random number from the normal distribution with mean $0$ and standard deviation $0.1$ .

$T_{camera}^{arm}=\left[\begin{array}{ccc} T_{00} & T_{01} & T_{02}\\ T_{10} & T_{11} & T_{12}\\ T_{20} & T_{21} & T_{22} \end{array}\right]_{3 \times 3}$

$P_{Arm}=\left[\begin{array}{c} p_{Arm, x_0} & p_{Arm, x_1} & p_{Arm, x_2} & . & . & . & p_{Arm, x_n} \\ p_{Arm, y_0} & p_{Arm, y_1} & p_{Arm, y_2} & . & . & . & p_{Arm, y_n} \\ p_{Arm, z_0} & p_{Arm, z_1} & p_{Arm, z_2} & . & . & . & p_{Arm, z_n} \end{array}\right]_{3 \times n}$

$P_{Cam}=\left[\begin{array}{c} p_{Cam, x_0} & p_{Cam, x_1} & p_{Cam, x_2} & . & . & . & p_{Cam, x_n} \\ p_{Cam, y_0} & p_{Cam, y_1} & p_{Cam, y_2} & . & . & . & p_{Cam, y_n} \\ p_{Cam, z_0} & p_{Cam, z_1} & p_{Cam, z_2} & . & . & . & p_{Cam, z_n} \end{array}\right]_{3 \times n}$

$T:3 \times 3, \ P_{Arm}:3 \times n, \ P_{Cam}:3 \times n$

==MATLAB可以輸出latex格式!!!!!!!!!!!==
==Mathtype其實也不錯用XD 不一定要用hackmd打~==

$\hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1}$