How do I apply a disturbance observer to a mobile robot?

Question

I have to design a observer to estimate disturbances which are applied to a unicycle model. So, the unicycle model with disturbances is:

$\dot{x}=v cos\theta + d_x$

$\dot{y}=v sin\theta + d_y$

$\dot{\theta}=\omega$

where $d_x$ and $d_y$ are the disturbances, which can be thought as velocities which I cannot access. I would like to define a disturbance observer to estimate these velocities, and use them in the controller for the control law.

The problem is that I am not practicle with disturbance observers, since I have never studied them. Searching online I have understood that I have to define a copy of the system of the type:

$\dot{\xi_x}=v cos\theta + k(x-\xi_x)$

$\dot{\xi_y}=v cos\theta + k(y-\xi_y)$

where I have understood that : $d_x=k(x-\xi_x)$ and $d_y=k(y-\xi_y)$.

Now, I have to design this on Simulink, but while doing it I have realized that I haave not clear from where I should access $\xi_x$ and $\xi_y$. Moreover, from the observer, the output should be $d_x$ and $d_y$, which are the estimates, since I have to use those in the definition of the controller, but in the equation of the observer I am computing $\dot{\xi_x}$ and $\dot{\xi_y}$, so how do I output them in order to send them to the controller?

I am probaly saying something wrong in the question, but I have not clear how to work with observers.

If needed I can write here the Simulink model and the code insede the blocks. I have not written to avoid making the question too long.

Answer 1

I think the thing to recognize here is that you're not actually going to have an error solely in the x- or y-directions unless the tire is slipping. The errors instead would be better modeled as a linear speed error $v_{err}$ and an angular speed error $\omega_{err}$. Those errors then propagate through your system like you'd expect, with the sine/cosine modifiers, etc.

You mentioned that you're not clear on how to work with observers, so here's a quick tutorial on those.

---

Suppose you have a linear system with dynamics. If you have states $x$, an input $u$, outputs $y$, then the system is typically modeled as:

$$ \dot{x} = Ax + Bu \\ y = Cx \\ $$

You can't know the true states, though; you can only make approximations or estimates. Your estimates take the same form, though -

$$ \dot{\hat{x}} = A\hat{x} + Bu \\ \hat{y} = C\hat{x} \\ $$

What you want is a way to estimate the error in your system and to then drive that error to zero, so you add a term to your model. In the above model, you can include a "control signal" based on the measurement error.

You can measure the actual output of the system $y$ and subtract from that the output of your model $\hat{y}$. Your model should include some gain $L$ that will drive the state error to zero. This leaves you with a new model:

$$ \dot{\hat{x}} = A\hat{x} + Bu + L(y - \hat{y})\\ \hat{y} = C\hat{x} \\ $$

Now, for the state error. If you define it to be the difference between actual and estimated states:

$$ e = x - \hat{x} \\ $$

Then you can describe how the error changes as the difference between how the actual state is changing and how the estimated state is changing:

$$ \dot{e} = \dot{x} - \dot{\hat{x}} \\ $$

But we've got equations for $\dot{x}$ and $\dot{\hat{x}}$! You can say:

$$ \begin{matrix} \dot{e} \\ \dot{e} \end{matrix} = \begin{matrix} \dot{x} & -\dot{\hat{x}} \\ A\hat{x} + Bu & -\left(A\hat{x} + Bu + L(y - \hat{y})\right) \end{matrix} $$

I can expand that and re-group terms:

$$ \dot{e} = A\hat{x} + Bu -\left(A\hat{x} + Bu + L(y - \hat{y})\right) \\ \dot{e} = A\hat{x} + Bu -A\hat{x} - Bu - L(y - \hat{y}) \\ \dot{e} = \left(A\hat{x}-A\hat{x}\right) + \left(Bu-Bu\right) - L(y - \hat{y})\\ $$

The input terms there $Bu$ cancel. Also, we can substitute in the expression for $y = Cx$ and $\hat{y} = C\hat{x}$:

$$ \dot{e} = \left(A\hat{x}-A\hat{x}\right) - L(Cx - C\hat{x}) \\ \dot{e} = A\left(x - \hat{x}\right) - LC\left(x - \hat{x}\right) \\ $$

which, when you substitute back $e = x - \hat{x}$, leaves you with

$$ \boxed{\dot{e} = [A - LC]e} \\ $$

If you are assuming that your state feedback controller is going to use gains of

$$ u = -Gx \\ $$

then your controller winds up looking like:

$$ \dot{x} = Ax + B(-Gx) \\ $$

or

$$ \boxed{\dot{x} = [A-BG]x} \\ $$

That is, the general structure of your state feedback controller and your error dynamics controller are the same. The observer design process is commonly referred to as the "dual" of the controller design process for this reason. For the controller design you're trying to drive the states to zero and for the observer design you're trying to drive the state error to zero.

The error dynamics need to be "faster" than the state dynamics so you drive the error to zero first (and thus your controller acts on a "good estimate" of the states). You can choose whatever you want, but faster error dynamics (higher frequency response) requires more actuator authority. Typically the error dynamics are chosen to be 5 to 10 times faster than the state dynamics.