Least Squares for Robotics $(16$ points$)$

Robots rely on sensors for understanding their environment and navigating in the real world. These sensors must be calibrated to ensure accurate measurements, which we explore in this problem.

$($a$)(3$ points$)$ Your robot is equipped with two forward$-$facing sensors $-$ a radar and camera. However, the sensors are placed with an offset $($i$.$e$.$ a gap$)$ of l in meters $($m$),$ as depicted in Fig. $20,$ and you want to find its value. The radar returns a range $\backslash$rho in meters $($ m $)$ and heading angle $\backslash$theta in radians $($rad$)$ with respect to the object. In contrast, the camera only returns an angle, $\backslash$phi in radians $($rad$),$ with respect to the object.

Figure $20$: Sensor Placement and Offset l$.$

These relationships are summarized by the following sensor model, where x$\_($r$)$ and y$\_($r$)$ are the Cartesian coordinates of the object with respect to the radar:

x$\_($r$)=\backslash$rho cos$(\backslash$theta $),$

y$\_($r$)=\backslash$rho sin$(\backslash$theta $),$

tan$(\backslash$phi $)=($y$\_($r$))/($x$\_($r$)+$l$).$

Assuming $\backslash$phi $!=0,$ use equations $(4),(5),(6)$ to express l in terms of $\backslash$rho $,\backslash$theta $,$ and $\backslash$phi $.$

$($b$)(5$ points$)$ Often it is difficult to precisely identify the value of l$.$ To learn the value of l you decide to

take a series of measurements. In particular, you take N measurements and get the equations:

al$+$e$\_($i$)=$b$\_($i$)$

for iN$.$ Here a$!=0$ is a fixed and known constant. Each b$\_($i$)$ represents your i$^($th $)$ measurement and

e$\_($i$)$ represents the error in your measurement. While you know all of the b$\_($i$)$ values, you do not know the

error values e$\_($i$).$

We can write this equation in a vector format as:

Al$+$vec$($e$)=$vec$($b$),$

where A$=[[$a$],[$vdots$],[$a$]],$vec$($e$)=[[$e$\_(1)],[$vdots$],[$e$\_($N$)]],$vec$($b$)=[[$b$\_(1)],[$vdots$],[$b$\_($N$)]].$

In this simple $1-$D case, the least squares solution is a scaled version of the average of $\{$b$\_($i$)\}\_($i$)=1^($N$).$

Find the best estimate for l$,$ denoted as hat$($l$),$ using least squares. Simplify your expression and

express hat$($l$)$ in terms of a$,$b$\_($i$),$ and N$.$ Your answer may not include any vector notation.

Note: A is a vector and not a matrix.

$($c$)(8$ points$)$ Now we turn to the task of controlling the robot's velocity and acceleration, which is a key

requirement for navigation.

We use the following model for the robot, which describes how the velocity and acceleration of the

robot changes with timestep k :

$[[$v$[$k$+1]],[$a$[$k$+1]]]=[[1,1],[0,1]][[$v$[$k$]],[$a$[$k$]]]+[[0],[1]]$j$[$k$],$

where

k is the timestep;

v$[$k$]$ is the velocity state at timestep k;

a$[$k$]$ is the acceleration state at timestep k;

j$[$k$]$ is the jerk $($derivative of acceleration$)$ control input at timestep k$.$

We start at a known initial state $[[$v$[0]],[$a$[0]]],$ and we want to find j$[0]$ to set $[[$v$[1]],[$a$[1]]]$ as close to $[[0],[0]]$ as

possible. For this, we minimize:

E$=||[[$v$[1]],[$a$[1]]]||^(2).$

Find the best estimate for the optimal choice of jerk, hat$($j$)[0],$ by using least squares method to

minimize E$.$ Express your solution in terms of v$[0]$ and a$[0].$ Show your work.

Hint: Rewrite E in terms of j$[0]$ and other relevant terms.