## 6 Controller Designs
### Outer Feedback Control Loop  
In the control system block diagram (Figure 7), the closed-loop linear transfer function is derived in Equation (46), while the eye-in-hand kinematic model is defined as a nonlinear mapping in Equations (24) and (25). When the input to this model is a set of perturbed joint angles \(\vec{q}(t)\), the output corresponds to the estimated image coordinates \(\widehat{p_{image}}\). Revising Equation (24) accordingly, we express the model as:  
\[\widehat{p_{image}}=\mathcal{F}\Big{(}\vec{q}(t),\;Pa,\;P^{O}\Big{)} \tag{50}\]  
Here, \(P^{O}e\)\(\mathbb{R}^{9\times 1}\), denotes the coordinates of fiducial markers in the inertial frame, and \(Pa\) includes fixed parameters such as camera intrinsics, robot kinematics, and the transformation matrix from the end-effector to the camera frame. To linearize this nonlinear model, we select a nominal joint configuration \(\vec{q}^{0}e\)\(\mathbb{R}^{6\times 1}\), and approximate the function \(\mathcal{F}\) using a first-order Taylor expansion, resulting in the following linearized form:  
\[\widehat{p_{image}}\approx J\Big{(}\vec{q}^{0},Pa,P^{O}\Big{)}\vec{q}(t)+ \mathcal{F}\Big{(}\vec{q}^{0},Pa,P^{O}\Big{)} \tag{51}\]  
where \(J\big{(}\vec{q}^{0},Pa,P^{O}\big{)}e\mathbb{R}^{6\times 6}\) is the Jacobian matrix of \(\mathcal{F}\) evaluated at \(\vec{q}^{0}\). Letting \(C_{1}=J\big{(}\vec{q}^{0},Pa,P^{O}\big{)}\), \(C_{2}=\mathcal{F}\big{(}\vec{q}(t),Pa,P^{O}\big{)}\), Equation (51) can be rewritten as:  
\[\widehat{p_{image}}=C_{1}\vec{q}(t)+C_{2} \tag{52}\]  
Defining the offset-corrected output as \(\widehat{p_{image}}^{\prime}=\widehat{p_{image}}-C_{2}\), we obtain a purely linear relation that simplifies analysis. The complete linearized feedback loop is illustrated in Figure 11.  
The resulting linear plant transfer function is given by:  
\[G_{pMIMO\_outer}=\frac{\widehat{P_{image}}^{\prime}}{q_{feedback}}=C_{1}\frac{(3 \tau_{in}s+1)}{\left(\tau_{in}s+1\right)^{3}}\cdot I_{6\times 6} \tag{53}\]  
Since \(C_{1}\) introduces coupling, the first step toward designing an observer for this multivariable system is to derive its Smith-McMillan form [36]. This can be achieved via Singular Value Decomposition (SVD):  
\[G_{pMIMO\_outer}^{linear}=U_{L}M_{p}U_{R} \tag{54}\]  
where \(U_{L}e\)\(\mathbb{R}^{9\times 9}\) and \(U_{R}\)\(e\)\(\mathbb{R}^{6\times 6}\) are the left and right unimodular matrices, and \(M_{p}\)\(e\)\(\mathbb{R}^{9\times 6}\) is the Smith-McMillan form a diagonal matrix of transfer functions. Each nonzero diagonal element takes the form:  
\[M_{p}(i,i)=gain(i)\cdot\frac{(3\tau_{in}s+1)}{\left(\tau_{in}s+1\right)^{3}}, \ ie(1,2,3,4,5,6) \tag{55}\]  
where \(gain\)\(e\)\(\mathbb{R}^{6\times 1}\) is a numerical vector. The Youla controller design for each decoupled entry in \(M_{p}\) is straightforward, as the system is stable (i.e., all poles and zeros lie in the left-half plane). The Youla parameterization allows the shaping of the closed-loop dynamics via a new transfer function \(M_{Y}\), which modifies the system's poles and zeros. A second-order Butterworth filter is selected for the desired closed-loop dynamics:  
\[M_{T}=\frac{{{\omega_{n}}^{2}}}{(s^{2}+2\zeta{\omega_{n}}s+{\omega_{n}}^{2}) }\cdot[ \begin{matrix}I_{6\times 6}&Zero_{3\times 3}\\ Zero_{3\times 3}&Zero_{3\times 3}\end{matrix} ], \tag{56}\]  
Here \(\omega_{n}\) is the natural frequency that determines the bandwidth of the outer loop (ensuring \(1/\omega_{n}>\tau_{in}\) to maintain proper hierarchy with the inner loop), and \(\zeta\) is the damping ratio for tuning response smoothness. The last three coordinates remain uncontrolled in the feedback loop.