# Plant-Inspired Soft Growing Robots: A Control Approach Using Nonlinear Model Predictive Techniques
## 3 Moving Horizon Estimation (MHE)
### MHE Optimization Problem  
The MHE problem is formulated as follows: At time \(t_{k}\), \(N_{E}\) system measurements represented as \(\mathbf{\tilde{y}}_{k-N_{E}+1},\ldots,\mathbf{\tilde{y}}_{k}\in\mathbb{R}^{n_{y}}\) corresponding to the past instants \(t_{k-N_{E}+1},\ldots,t_{k}\) along with the nominal system inputs \(\tilde{\mathbf{n}}_{k-N_{E}+1},\ldots,\tilde{\mathbf{n}}_{k-1}\in\mathbb{R}^{n_{u}}\) corresponding to the past instants \(t_{k-N_{E}+1},\ldots,t_{k-1}\) have to be available to estimate the full system state \(\mathbf{\tilde{x}}_{i}\in\mathbb{R}^{n_{x}}\) for all \(i=0,1,\ldots,k\). Thus, the MHE recursively solves the following nonlinear constrained optimization problem along the \(N_{E}\) estimation horizon:  
\[\begin{split}\min_{\mathbf{x}(.),\mathbf{u}(.)}&\sum_{k=i-N _{E}}^{i}||\mathbf{\tilde{y}}_{k}-h(\mathbf{x}_{k},\mathbf{u}_{k})||_{\mathbb{V}}^{2}\\ &+\sum_{k=i-N_{E}}^{i-1}||\mathbf{\tilde{u}}_{k}-\mathbf{u}_{k}||_{ \mathbb{W}}^{2}\\ \text{s.t.}&\dot{\mathbf{x}}(k+1)=f(\mathbf{x}(k),\mathbf{u}(k) )\\ &\mathbf{x}_{\min}\leqslant\mathbf{x}(k)\leqslant\mathbf{x}_{\max}\\ &\mathbf{u}_{\min}\leqslant\mathbf{u}(k)\leqslant\mathbf{u}_{\max}\end{split} \tag{5}\]  
where \(h(.):\mathbb{R}^{n_{x}+n_{u}}\rightarrow\mathbb{R}^{n_{y}}\) is the measurement model that characterizes the sensor transfer function with respect to the system state \(\mathbf{x}\) and input \(\mathbf{u}\) while \(f(.):\mathbb{R}^{n_{x}+n_{u}}\rightarrow\mathbb{R}^{n_{x}}\) indicates the system dynamics. As noted, in (5), the control inputs are included as well in the optimization problem to consider the deviation that could exist between the nominal controls applied and the controls received by the robot due to actuator noise and/or inaccuracy [23].  
The matrices \(\mathbb{V}\succeq\ \mathbf{0}\) and \(\mathbb{W}\succeq\mathbf{0}\) are semi-positive weighting matrices assumed to be constant over the estimation horizon and are inversely proportional to the co-variances of the noise added to the sensor and the control inputs as follows:  
\[\begin{split}\mathbb{V}&=[\text{diag}(\sigma_{s}, \sigma_{\theta},\sigma_{\phi})]^{-1},\\ \mathbb{W}&=[\text{diag}(\sigma_{s},\sigma_{\theta},\sigma_{\phi})]^{-1}\end{split} \tag{6}\]In vine robots, we assumed that the joint-space values, i.e., the robot length \(s_{k}\), the angle of curvature \(\theta_{k}\), and the plane angle \(\phi_{k}\), are measured at each \(k\) sample. As discussed in [15], the robot length is indirectly sensed using an incremental encoder placed on the shaft of the motor that holds the robot material. In addition, according to what has been proposed in [24], the robot angle of curvature \(\theta\) and the plane angle \(\phi\) could be anticipated from the tip quaternion that is measured using an Inertial Measurement Unit (IMU) place on the robot's tip. Thus, to find the measurement model \(h(.)\) in (5), we assume that the robot's output \(\mathbf{y}_{k}=[s_{k},\theta_{k},\phi_{k}]^{T}\) is measured at a sampled time with a \(T\) interval. Hence, with the help of the closed-form Inverse kinematic (IK) solution for one-section continuum robots proposed in [25], the measurement model is mainly depending on the tip Cartesian position \((x,y,z)\) and could be formulated as:  
\[\begin{bmatrix}s\\ \theta\\ \phi\end{bmatrix}_{k}=\begin{bmatrix}\cos^{-1}\Bigl{(}1-\frac{2(x^{2}+y^{2})}{ r^{2}}\Bigr{)}\Bigl{(}\frac{r^{2}}{2\sqrt{x^{2}+y^{2}}}\Bigr{)}\\ \cos^{-1}\Bigl{(}1-\frac{2(x^{2}+y^{2})}{r^{2}}\Bigr{)}\\ \tan^{-1}\Bigl{(}\frac{y}{x}\Bigr{)}\end{bmatrix}_{k} \tag{7}\]  
where \(r=\sqrt{x^{2}+y^{2}+z^{2}}\) while the subscript \(k\) means the measurements and state components at sample \(k\). It is worth mentioning that increasing the estimation horizon \(N_{H}\) will increase the state estimation accuracy. However, the computational time will increase on the other hand. Thus, choosing the value of \(N_{H}\) is a trade-off between speed and accuracy.