state estimation scratch work

This isn't too neat (yet). The plan is to use an Extended Kalman Filter (EKF) to estimate the state of the robot based on a variety of different sensors. This requires having a mathematical model of how the robot works. A good reference for the kind of thing we're trying to do is Optimal State Estimation by Dan Simon. There are free pdf's of it floating around the usual sites.

State

The state of the drive system consists of the following:

• The position on the field $\vec r = (r_x, r_y)$ (in meters)
• The heading $\theta_{bot}$ (in radians)
• The velocity $\vec v = (v_x, v_y)$ (in meters per second)
• The angular velocity $\omega_{bot}$ (in radians per second)
• The current through each motor $(i_{fl}, i_{fr}, i_{bl}, i_{br})$ (in amps)

Control Input

We control the voltage to each motor, $(e_{fl}, e_{fr}, e_{bl}, e_{br})$.

Time evolution

Position and heading change according to their derivatives, which are already part of the state.

$$\dot r_x = v_x$$ $$\dot r_y = v_y$$ $$\dot \theta_{bot} = \omega_{bot}$$

The velocities change according to the forces on the robot. The primary contribution to this is the torque from each wheel, which is proportional to current. We also have to consider friction. We can assume that the force felt at each wheel is proportional to the current in that wheel, so

$$F_{fl} = K_{fl}i_{fl}$$ $$F_{fr} = K_{fr}i_{fr}$$ $$F_{bl} = K_{bl}i_{bl}$$ $$F_{br} = K_{br}i_{br}$$

where each $K_\bullet$ is the ratio of force (felt at the wheel, pushing perpendicular to the rollers), to current. These constants depend on the torque constants of the motors, as well as the geometry of the wheels.

The acceleration on the robot is then

$$\begin{bmatrix} \dot v_x \\\ \dot v_y \end{bmatrix} = \frac{1}{m_{bot}} \begin{bmatrix} \cos \theta & -\sin \theta \\\ \sin \theta & \cos \theta \end{bmatrix} \frac{1}{\sqrt 2} \begin{bmatrix} 1 & -1 & -1 & 1 \\\ 1 & 1 & 1 & 1 \end{bmatrix} \begin{bmatrix} K_{fl} i_{fl} \\ K_{fr} i_{fr} \\ K_{bl} i_{bl} \\ K_{br} i_{br} \end{bmatrix} + \mathrm{slip},$$

Where slip is some error term we need to quantify. Notice that both $\theta$ and the currents $i_\bullet$ are part of the state, so this system is non-linear.

The angular acceleration is computed similarly, but it depends also on the placement of the wheels and the robot's moment of inertia. Luckily, that one's actually linear (since it doesn't depend on the heading). I'm not writing it down explicitly, but it's

$$\dot \omega_{bot} = A \begin{bmatrix} K_{fl} i_{fl} \\ K_{fr} i_{fr} \\ K_{bl} i_{bl} \\ K_{br} i_{br} \end{bmatrix} + \mathrm{slip}$$

for some 1-by-4 matrix $A$ depending on the geometry of the robot.

TODO: model the motors. This needs to be done empirically before we can expect the rest to work.

Measurements

We can measure the following things:

• the absolute position of the drive wheels
• the absolute position of dead wheels
• the current through each motor
• the 3-axis acceleration felt by the IMU
• the angle measured by the gyroscope on the IMU
• the angles from the robot to various points on the field, as measured by a camera

TODO: model some of these, both their nominal values (computed from state) and their variances