\documentclass[onecolumn,11pt,a4paper]{article}
\usepackage{amsfonts,times,graphicx,amsmath}
%\usepackage[bookmarks=false,pdfstartview=Fit,dvips]{hyperref}
\usepackage[ruled]{algorithm}
\usepackage{algpseudocode}
\usepackage[noadjust]{cite}
\usepackage{amsmath,amsfonts,amssymb}
\usepackage{graphicx}
\usepackage{subfigure}
%\input{defi}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\textheight 225mm
\textwidth 160mm
\topmargin 10mm
\vskip 3mm
\evensidemargin -2mm
\oddsidemargin -2mm
\parskip .5em
\parindent 0em%ex
%\psdraft
\columnsep 8mm

\newcommand\eps{\varepsilon}
\newcommand*{\QEDB}{\hfill\ensuremath{\square}}%
%\newcommand\Matlab{{\sc Matlab}}

\title{\vskip -3cm {\bf Estimation for control}\\Laboratory guide \vskip -1.5cm}
\date{}

\begin{document}
\maketitle
%\vspace*{-1cm}
\section{Introduction}
This laboratory guide is developed to give a better understanding on linear state estimation problems. All the theoretical concepts are presented through a case study, with the main core, a water tank example. In some cases, to highlight certain aspects other examples are presented as well as some experimental results. We start by presenting the nonlinear model of the tank system, which is followed by a linearization around an equilibrium point. First, a linear state-feedback control is designed for stabilization, then  for reference tracking, under the assumption that all states are measured. Afterwards, an observer is designed to estimate the unmeasured states and the controller will use their estimated state.
%This is followed by state-feedback control design for stabilization and tracking.

To develop our results we will use the following notations: $w_0$ means the equilibrium point of vector $w\in \mathbb{R}^n$ around which we will do the linearization; $w_{k0}\in \mathbb{R}$ is the $k$-th element in vector $w$. $I$ and $0$ mean the identity and zero matrices of appropriate dimensions. $\mathrm{eig}(F)$ means the eigenvalues of $F$, and $\mathrm{rank}(F)$ refers to the rank of $F$. The notation $f(x,u)$ refers to a vector function of the form $f(x,u) = [f_1(x,u),\, f_2(x,u)\,,...,\,f_n(x,u)]^T$, where $n$ is the number of states. For the sake of simplicity we will provide every numerical value with two decimal precision.
\section*{Tank system}

Consider the two tank system shown in Figure~\ref{fig:tanks}. Water is pumped from a reservoir into the upper tank. From this tank, the water flows to the lower tanks and then back to the reservoir. There is another pipe which pumps directly the water from the reservoir to the lower tank. The system has two control inputs $u_1$ and $u_2$, which are the voltages applied to the motors of the pumps. It is assumed that the levels in the tanks ($h_1$ and $h_2$) are measured. The goal is to control the levels in the tanks.
\section{Nonlinear model}
The equations describing the dynamics~are:
\begin{equation}
\begin{aligned}
\label{eq:tanks}
% \nonumber to remove numbering (before each equation)
  \delta_1 \dot{F}_{\textrm{in},1} & =  - F_{\textrm{in},1}+Q_{s,1} \cdot u_1\\
  \dot{h}_1 & = \frac{F_{\textrm{in},1}}{A_1} - \frac{s_1\sqrt{2gh_1}}{A_1} \\
  \delta_2 \dot{F}_{in,2} & =  - F_{\textrm{in},2}+Q_{s,2} \cdot u_2\\
  \dot{h}_2  & =  \frac{s_1 \sqrt{2gh_1}}{A_2} - \frac{s_2 \sqrt{2gh_2}}{A_2} +\frac{F_{\textrm{in},2}}{A_2} \\
\end{aligned}
\end{equation}

\begin{figure}[!htb]
  \centering%
      \includegraphics[width=.50\textwidth]{img/Tanksys.png}%
  \caption{Cascaded tanks system.}%
  \label{fig:tanks}%
\end{figure}

The parameter values are presented in Table \ref{tab:values}.
\begin{table}[!h]
  \centering
  \caption{Parameter values used.}
  \label{tab:values}
\begin{tabular}{llll}
  % after \\: \hline or \cline{col1-col2} \cline{col3-col4} ...
  Parameter& Symbol &Value & Units\\
  \hline
  Acceleration  &$g $& $981$ & $\mathrm{cm/s}^2$ \\
  Area of the tanks &$A_1$, $A_2$,& $25$, $16$ & $\mathrm{cm}^2$ \\
  Outlet area of tank 1&$s_1$ & $1$ & $\mathrm{cm}^2$ \\
  Outlet area of tank 2&$s_2$ & $1.5$ &$\mathrm{cm}^2$\\
  Input to flow gains & $Q_{s,1}$, $Q_{s,2}$ & $5$,\, $10$ &$\mathrm{cm}^3\mathrm{/s/V}$\\
  Motor time constants&  $\delta_1$, $\delta_2$ &$0.5$,\,  $1$& $\mathrm{s}$ \\
  \hline
\end{tabular}
\end{table}
%
We denote the state vector with $x=\begin{bmatrix} x_1 & x_2 & x_3 & x_4 \end{bmatrix}^T $, where $x_1 = F_{in,1}$, $x_2 = h_1$, $x_3=F_{in,2}$ and $x_4=h_2$; and the input vector is $u=[u_1 \,\, u_2]^T$. With these notations the nonlinear state-space model is:
\begin{equation} \begin{aligned}
\dot{x}
=
\begin{bmatrix}
    \dot{x}_1\\
    \\
    \dot{x}_2\\
    \\
    \dot{x}_3\\
    \\
    \dot{x}_4
\end{bmatrix}
=
\begin{bmatrix}
-\frac{1}{\delta_1}x_1 +\frac{Q_{s,1}}{\delta_1}u_1\\
\\
 \frac{x_1}{A_1}-\frac{s_1\sqrt{2gx_2}}{A_1}\\
 \\
 \frac{1}{\delta_2}x_3+\frac{Q_{s,2}}{\delta_2}u_2\\
 \\
 \frac{s_1\sqrt{2gx_2}}{A_2}-\frac{s_2\sqrt{2gx_4}}{A_2}+\frac{x_3}{A_3}
\end{bmatrix}
=
\begin{bmatrix}
    f_1(x_1,u_1)\\
    \\
    f_2(x_1,x_2)\\
    \\
    f_3(x_3,u_2)\\
    \\
    f_4(x_2.x_3,x_4)
\end{bmatrix}
=
f(x,u)
\label{eq:nlstate_space}
\end{aligned}
\end{equation} 
%\newpage
The dynamics described in \eqref{eq:tanks} is nonlinear. To design a linear controller, we need to linearize the system around an equilibrium point. In this context the linearization around $0$, means that the level in the tanks are zero, which is not a normal operating point. Instead of this we consider the scenario where the levels in the tanks vary around $x_{20}=h_{10}=5\,[cm]$ and $x_{40}=h_{20} = 3\,[cm]$.


In order to maintain these levels in the tanks we need to provide constant water inputs, otherwise, if the inputs are $0$ for the tanks, the water will flow out, and eventually after a while the tanks will be empty. So we need to find the constant inputs for which the desired levels are maintained.

Based on \cite{Khalil2000} an equilibrium point for a nonlinear dynamic system of the form $\dot{x} = f(x,u)$ can be found in the point where the dynamic system of equations is $0$, i.e. $f(x_0,u_0)=0$. For \eqref{eq:tanks} we have:
\begin{align}
\label{eq:lin1}
      - x_1+Q_{s,1} u_1=&0\\
\label{eq:lin2}
   \frac{x_1}{A_1} - \frac{s_1\sqrt{2gx_2}}{A_1} =&0\\
\label{eq:lin3}
   - x_3+Q_{s,2}u_2=&0\\
\label{eq:lin4}
   \frac{s_1 \sqrt{2gx_2}}{A_2} - \frac{s_2 \sqrt{2gx_4}}{A_2} +\frac{x_3}{A_2}=&0.
\end{align}
Based on \eqref{eq:lin2}, $x_{10} = s_1\sqrt{2gx_{20}}$, from where using \eqref{eq:lin1} we obtain $u_{10} = \frac{x_{10}}{Q_{s,1}}$. Similarly, based on \eqref{eq:lin4} we have $x_{30} = s_2\sqrt{2gx_{40}}-s_1\sqrt{2gx_{20}}$, and using \eqref{eq:lin3}  we obtain $u_{20} = \frac{x_3}{Q_{s,2}}$. Using the parameter values from Table \ref{tab:values} and the desired levels, $x_{20}=5\,[cm]$ and $x_{40}=3\,[cm]$, we obtain: $x_{10}=99.04$, $u_{10}=19.8$, $x_{30}=16.03$ and $u_{20} =1.6$.

Based on these notation the equilibrium point around which we do the linearization is:
\begin{equation} \begin{aligned}
    u_0 = \begin{bmatrix} 19.8 & 1.6\end{bmatrix}^T, \quad x_0 = \begin{bmatrix} 99.05 & 5 & 16.03 & 3\end{bmatrix}^T
\end{aligned} \end{equation}

\subsection{Linearized model}
Recall that the truncated Taylor series approximation for a function of $n$ variables: $g = f(x_1, x_2, ..., x_n)$ around a point
$x_0 = (x_{10}, x_{20}, ..., x_{n0})$:
\begin{equation} \begin{aligned}s
    g = f(x_{10},\, x_{20},\, ..., \,x_{n0}) + \frac{\partial f}{\partial x_1}|_{x_0}(x_1-x_{10})+\, ...\, +\frac{\partial f}{\partial x_n}|_{x_0}(x_n-x_{n0}).
\end{aligned} \end{equation} s
We evaluate the partial derivatives, $\frac{\partial f}{\partial x_k}|_{x_0}$ for all $k =1,...,n$, which will lead to a constant, and by denoting $\Delta x_k=x_k-x_{k0}$  for all $k =1,...,n$ a linear expression is obtained.

In our case we have two nonlinear equations in \eqref{eq:nlstate_space}, the second and the forth equation, and we put all the terms on the left hand side of the equation to obtain:
\begin{equation} \begin{aligned}
   g_2(x_1,x_2,\dot{x}_2)=&\frac{x_1}{A_1} - \frac{s_1\sqrt{2gx_2}}{A_1}-\dot{x}_2\\
   g_4(x_2,x_3,x_4,\dot{x}_4)=&\frac{s_1 \sqrt{2gx_2}}{A_2} - \frac{s_2 \sqrt{2gx_4}}{A_2} +\frac{x_3}{A_2}-\dot{x}_4.
\end{aligned} \end{equation}
By applying the first order Taylor series approximation the following is obtained:
\begin{equation} \begin{aligned}
    g_2(x_1,x_2,\dot{x}_2) = &\frac{1}{A_1}(x_1-x_{10}) +\begin{pmatrix} -\frac{s_1\sqrt{2q}}{2A_1\sqrt{x_2}}\end{pmatrix}|_{x_0}(x_2-x_{20})-\dot{x}_2\\
    =& 0.04(x_1-x_{10})+(-0.39)(x_2-x_{20})-\dot{x}_2\\
    \Rightarrow &\dot{x}_2 = 0.04\Delta x_1 -0.39\Delta x_2.
\end{aligned} \end{equation}
Similarly, with $g_4$ we obtain:
\begin{equation} \begin{aligned}
    \dot{x}_4 = 0.61\Delta x_2+ 0.06 \Delta x_3-1.2 \Delta x_4.
\end{aligned} \end{equation}
The linear system is written in terms:
\begin{equation} \begin{aligned}
    \Delta x= \begin{bmatrix} \Delta x_1\\ \Delta x_2 \\ \Delta x_3 \\ \Delta x_4 \end{bmatrix} = \begin{bmatrix} F_1-F_{10}\\ h_1-h_{10}\\ F_2-F_{20}\\ h_2-h_{20}\end{bmatrix} = \begin{bmatrix} F_1-99.5\\ h_1-5\\ F_2-16.03 \\ h_3-3 \end{bmatrix},\quad
    \Delta u =  \begin{bmatrix} \Delta u_1\\ \Delta u_2\end{bmatrix} = \begin{bmatrix} u_1-19.8\\ u_2-1.6 \end{bmatrix}.
\end{aligned} \end{equation}
By using the parameter values in Table \ref{tab:values}, we obtain the following linear system:
\begin{equation} \begin{aligned}
    \Delta \dot{x} = A\Delta x + B \Delta u, \quad
    A = \begin{bmatrix} -2 & 0 & 0 & 0\\
            0.04 & -0.39 & 0 & 0\\
            0 & 0 & -1 & 0\\
            0 & 0.62 & 0.06 & -1.2
    \end{bmatrix}, \quad B = \begin{bmatrix} 10 & 0 \\ 0 & 0\\ 0 & 10 \\ 0 & 0 \end{bmatrix}.
    \label{eq:linss}
\end{aligned} \end{equation}
Based on the eigenvalues of $A$, $ \mathrm{eig}(A) = \begin{bmatrix} -2 & -0.39 & -1 & -1.2\end{bmatrix}$, we see that the unforced system is asymptotically stable.

For simulation consider the Simulink model described in Fig. \ref{fig:open_loop}.
\begin{figure}[!htb]
  \centering%
      \includegraphics[width=1.0\textwidth]{img/open_loop_system.png}%
  \caption{Open loop system, linear and nonlinear}%
  \label{fig:open_loop}%
\end{figure}
As it can be seen on Fig \ref{fig:open_loop} the linear model defines the system in form of $Dx$, $Du$, which refers to $\Delta x$ and $\Delta u$. To compare correctly we need to add the linearization point, $x_0$. Then initial condition for the nonlinear model is the linearization point, $x_0$, and also the control input is $u_0$.

The nonlinear model inside contains the nonlinear equations described in \eqref{eq:nlstate_space} and an integrator. This can be seen on Fig. \ref{fig:nonlin_model}. Similar idea is applied for the linear model.
\begin{figure}[!htb]
  \centering%
      \includegraphics[width=1.0\textwidth]{img/nl_model.png}%
  \caption{Nonlinear model}%
  \label{fig:nonlin_model}%
\end{figure}
\pagebreak
%------------------------------------------------------------------
% INTERMEZZO
%------------------------------------------------------------------
\section{State-feedback control}
Consider the linear system:
\begin{equation} \begin{aligned}s
    \dot{x} = Ax+Bu,
\end{aligned} \end{equation} s
where $x\in \mathbb{R}^n$, $u\in \mathbb{R}^m$, and $A,\, B$ are the system matrices. Consider the state feedback control law, $u = -Kx$, which leads to:
\begin{equation} \begin{aligned}
\dot{x} = Ax-BKx = (A-BK)x.
\label{eq:closedloopsf}
\end{aligned} \end{equation}
Based on \cite{Khalil2000} the closed loop system in \eqref{eq:closedloopsf} is stable if the eigenvalues of $(A-BK)$ are negative. For this reason we need to check first if the pair $(A,B)$ is controllable:
\begin{equation} \begin{aligned}
    P_c = \begin{bmatrix} B & AB&...&  A^{n-1}B \end{bmatrix}.
\end{aligned} \end{equation}
If the rank of $P_c$ is equal with the number of states $n$, then the system is fully state controllable, and the gain $K$ can be obtained.
% For a better understanding let us consider the following numerical example:
% \begin{equation} \begin{aligned}s
%     \dot{x}= Ax+Bu,\quad  A = \begin{bmatrix} 0 & 1 \\ 2 & -1 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 3 \end{bmatrix}.
% \end{aligned} \end{equation} s
% We see that the unforced (zero input) system is unstable, since $\mathrm{eig}(A) =[1,\,-2]$.

% It can happen that the unforced system is stable, i.e. all the eigenvalues are negative, but we want to change some of the performances of the system, for instance to reduce the settling time or to reduce the overshoot, in these cases we can also use the state-feedback control law, with poles for which the desired performances are achieved.

% The controllability matrix is $P_c = [B \, AB] = \begin{bmatrix} 0 & 3\\ 3 & -3  \end{bmatrix} $, and $rank(P_c) = 2$, so the system is fully state controllable. We consider $K = [k_1\, k_2]$, so
% \begin{equation} \begin{aligned}s
%     A-BK = \begin{bmatrix} 0 & 1 \\ 2-3k_1 & -3*k_2-1 \end{bmatrix}.
% \end{aligned} \end{equation} s
% To find the eigenvalues, we have characteristic equation:
% \begin{equation} \begin{aligned}
%     \mathrm{det}(\lambda I-A+BK) = \lambda^2+(3k_2+1)\lambda+ 3k_1-2 = 0.
%     \label{eq:chareq}
% \end{aligned} \end{equation}
% We impose that the desired closed loop eigenvalues are $\lambda_{1,2} = [-1, \, -2]$, so the desired characteristic equation is $\lambda^2+3\lambda+2=0$. From the desired characteristic equation and \eqref{eq:chareq}, we obtain that $ 3k_2+1 = 3$ and $3k_1-2=2$, which leads to $k_1 = 1.33$, $k_2 = 0.66$.

% Note that for higher order systems the calculation of the gain is not straight forward. Matlab can be used for these cases with function place, in the following form: \texttt{K = place(A,B,[{eigdesired}])}.

%It is logical, since if we are at the equilibrium point the level in the tanks decrease to zero.
In \eqref{eq:linss} the linear system is stable, but we want to change the eigenvalues of the closed-loop system to $e_d = [-1\, -5\, -2\, -3]$. The system \eqref{eq:linss} is fully state controllable since the rank of the controllability matrix is $4$. We consider the state feedback control law:
\begin{equation} \begin{aligned}
    \Delta u = -K\Delta x,
    \label{eq:contr_law}
\end{aligned} \end{equation}
where $K$ is the state-feedback gain. By using \texttt{place} in Matlab, we obtain:
\begin{equation} \begin{aligned}
    K = \begin{bmatrix}  0.42 & 16.15 & -0.06 & -0.97 \\
             -0.11 &-6.42 &  0.21 & 0.22 \end{bmatrix}
\end{aligned} \end{equation}
The closed loop model, with control law \eqref{eq:contr_law} represented in Simulink can be seen on Fig. \ref{fig:closed_loop}. As it was described in \eqref{eq:contr_law} in the control law the $\Delta x$ is used and the obtained control input is $\Delta u$.
\begin{figure}[!htb]
  \centering%
      \includegraphics[width=1.0\textwidth]{img/closed_loop_system.png}%
  \caption{Closed loop system}%
  \label{fig:closed_loop}%
\end{figure}
%-------------------------------------------------------------------------------
\section{Observer design}
Until this point it was assumed that all the states are measurable and availbe for control. In this section we consider the case where some of the states are unavailable, which need to estimated for control. For the example described in \eqref{eq:tanks} we assume that the levels in the tanks are available, ($x_2$, $x_4$), and we want to estimate the flow rates ($x_1$, $x_3$).
Let us consider again the linear model \eqref{eq:linss} extended with the output matrix $C$:
\begin{equation} \begin{aligned}
    \Delta \dot{x} =& A\Delta x + B \Delta u\\
    \Delta y  = & C \Delta x,
\end{aligned} \end{equation}
and
\begin{equation} \begin{aligned}
    C = \begin{bmatrix} 0 & 1 & 0 & 0\\
             0 & 0 & 0 & 1\end{bmatrix}.
\end{aligned} \end{equation}

The following linear observer is considered:
\begin{equation} \begin{aligned}
    \Delta \dot{\hat{x}} =& A \Delta \hat{x} + B\Delta u + L(\Delta y -\Delta \hat{y})\\
    \Delta \hat{y}    =& C\Delta \hat{x},
\end{aligned} \end{equation}
where $\Delta \hat{x} $ is the estimate of $\Delta x$, $L$ is the observer gain. In order to design the observer first we need to verify if the system is fully observable. For this reason the observability matrix need to be calculated:
\begin{equation} \begin{aligned}
    P_o = \begin{bmatrix}
            C\\
            CA\\
            .\\
            .\\
            .\\
            CA^{n-1}
        \end{bmatrix}
\end{aligned} \end{equation}
As it was in the case of the controllability matrix, also here the rank of the observability matrix, $P_o$ has to be equal with the number of states $n$. For the linear tank system in \eqref{eq:linss} the rank of the observability matrix is $4$.

% \subsection{Intermezzo-setpoint tracking controller}
% State-feedback is a control law which returns the system states to 0. However, we are also interested in tracking a
% reference command $r(t)$, so that the output $y(t)$ follows $r(t)$. We consider the following system:
% \begin{equation} \begin{aligned}
%     \dot{x} = & Ax+Bu\\
%     z = & C_c x,
% \end{aligned} \end{equation}
% where $z$ is the controller output. All the states are available for feedback and the objective is that the controlled output should converge to some reference signal $r$. Note that not all the states can be made to converge to a reference signal.

% The approach is to add a new state vector $x^I$ to the system that integrates the tracking error:
% \begin{equation} \begin{aligned}s
%     \dot{x^I} = e = r-z
% \end{aligned} \end{equation} s
% By using this new dynamics we can write the extended system in the form:
% \begin{equation} \begin{aligned}
%     \begin{bmatrix}\dot{x} \\ \dot{x^I}\end{bmatrix} = \begin{bmatrix} A & 0 \\ C & 0\end{bmatrix}\begin{bmatrix} x\\ x^I\end{bmatrix}+\begin{bmatrix} B\\ 0 \end{bmatrix} u +\begin{bmatrix} 0\\ r \end{bmatrix}.
%     \label{eq:fullss}
% \end{aligned} \end{equation}

% State-feedback control can be designed for \eqref{eq:fullss} if the pair $\begin{pmatrix} \begin{bmatrix} A & 0 \\ C & 0\end{bmatrix}, \begin{bmatrix} B \\0 \end{bmatrix}  \end{pmatrix} $ is controllable. If this condition is fulfilled, the controller can be designed with simple pole placement technique. The control law is:
% \begin{equation} \begin{aligned}
%     u = -K\begin{bmatrix} x \\ x^I \end{bmatrix} = - \begin{bmatrix} K_x & K_{x^I} \end{bmatrix} \begin{bmatrix} x \\ x^I \end{bmatrix}.
% \end{aligned} \end{equation} 
% For a better understanding see Fig \ref{fig:lqr_servo}. If $u$ stabilizes the system, then $z\rightarrow r$, or an intuitive explanation can be to consider that $x^I$ will converge to a constant value, so $\dot{x}^I\rightarrow 0$, which leads to $z\rightarrow r$.
% The effect of the integrator can be understood as a Proportional Integrator (PI) control law, which effect is to obtain $0$ steady state error.
% \begin{figure}[!htb]
%   \centering%
%       \includegraphics[width=.80\textwidth]{img/lqr_servo.png}%
%   \caption{Reference tracking}%
%   \label{fig:lqr_servo}%
% \end{figure}

%In the next iterations we will consider the linear model in \eqref{eq:linss},  we will use the simple notation $x$ and $u$ instead of $\Delta x$, $\Delta u$.


\bibliographystyle{IEEEtranS}
%\IEEEtriggeratref{4}
\bibliography{nagyZ}
\end{document}