SPOT velocity estimator ‐‐ Summer 2024 Showdown

Introduction

This wiki page aims to help SRCL converge on a common technique for obtaining velocity and rate estimates for SPOT experiments. This convergence will be achieved through competition.

Competition categories

The competition features two categories: real-time solutions and post-processed solutions.

	Real-time solutions These solutions must run onboard a SPOT platform and will only produce velocity estimates based on past data.
	Post-processed solutions These solutions can use the additional computational power and functionality of a MATLAB analysis environment and can produce velocity estimates based on an entire experimental run.

Measurements

	It is assumed that PhaseSpace measurements are the primary source of position and rotation information for $x$, $y$, and $\theta$.
	Methods which can incorporate additional measurements (e.g., lidar, stereo vision) will also be considered, provided that 1. they are capable of producing estimates without the additional measurements, and 2. the additional measurements consistently improve estimate accuracy.

Candidate methods

Reduced-order observer

contributor(s)	Adam V.
category	real-time
inputs	PhaseSpace measurements commanded forces & torques
status	working

Theory

Design plant for state observer

$$\dot x = A_o x + B_o u, \qquad y = C_o x$$

$$A_o = \begin{bmatrix}0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0\end{bmatrix}, \qquad B_o = \begin{bmatrix}0 \\ 1 \\ 0\end{bmatrix}, \qquad C_o = \begin{bmatrix}1 & 0 & 0\end{bmatrix}.$$

Observability demonstrated since

$$\begin{bmatrix}C_o \\ C_o A_o \\ C_o A_o^2\end{bmatrix}= \begin{bmatrix}1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1\end{bmatrix}$$

has full rank.

For the reduced-order state observer, we introduce $r = [ x_2 \; x_3 ]^\mathsf{T}$ as the vector of states which are not measured and rewrite the state equation as

$$\dot x = \begin{bmatrix}\dot x_1 \\ \dot r\end{bmatrix} = \begin{bmatrix}A_{11} & A_{12} \\ A_{21} & A_{22}\end{bmatrix} \cdot\begin{bmatrix}x_1 \\ r\end{bmatrix} + \begin{bmatrix}B_1 \\ B_2\end{bmatrix} \cdot u$$

where, in our application,

$$A_{11} = 0, \quad A_{12} = \begin{bmatrix}1 & 0\end{bmatrix}, \quad A_{21} = \begin{bmatrix}0 \\ 0\end{bmatrix}, \quad A_{22} =\begin{bmatrix}0 & 1 \\ 0 & 0\end{bmatrix}, \quad B_1 = 0, \quad B_2 = \begin{bmatrix}1 & 0\end{bmatrix},$$

The dynamics of the reduced-order observer are described by the following equations

$$\dot \rho = (A_{22} - L \cdot A_{12}) \rho + (B_2 - L \cdot B_1) u + [ ( A_{22} - L \cdot A_{12} ) \cdot L + A_{21} - L \cdot A_{11} ] y$$

$$\hat r = \rho + L \cdot y$$

where we have introduced the observer state vector $\rho$.

In our application, the first equation reduces to

$$\dot \rho = \begin{bmatrix}-L_1 & 1 \\ -L_2 & 0\end{bmatrix} \cdot \rho + \begin{bmatrix}1 & -L_1^2 + L_2 \\ 0 & -L_1 L_2 \end{bmatrix}\cdot \begin{bmatrix}u \\ y\end{bmatrix}$$

while the second can be stated as

$$\hat r = \begin{bmatrix}1 & 0 \\ 0 & 1\end{bmatrix} \cdot \rho + \begin{bmatrix}0 & L_1 \\ 0 & L_2 \end{bmatrix} \cdot \begin{bmatrix}u \\ y\end{bmatrix}.$$

The observer can be initialized with a first estimate of zero using the initialization

$$\rho_0 = - L \cdot y_0.$$

The error dynamics obey

$$\dot \varepsilon = (A_{22} - L \cdot A_{12}) \cdot \varepsilon, \qquad \varepsilon = r - \hat r$$

Initial results (2024-07-26)