User Tools
Differences
This shows you the differences between two versions of the page.
| Both sides previous revision Previous revision Next revision | Previous revision | ||
|
cs:localization:sensors:start [2017/01/25 17:47] Brian Moore [Error Tracking] |
cs:localization:sensors:start [2017/09/03 13:52] (current) Brian Moore [Translational and Rotational Acceleration] |
||
|---|---|---|---|
| Line 271: | Line 271: | ||
| $$ | $$ | ||
| - | a_x = (0 * \dot{\psi}^2) + (p_z * \dot{\phi}^2) + (-p_y * \dot{\theta}^2) \\ | + | a_x = (0 * \ddot{\psi}) + (p_z * \ddot{\phi}) + (-p_y * \ddot{\theta}) \\ |
| - | a_y = (-p_z * \dot{\psi}^2) + (0 * \dot{\phi}^2) + (p_x * \dot{\theta}^2) \\ | + | a_y = (-p_z * \ddot{\psi}) + (0 * \ddot{\phi}) + (p_x * \ddot{\theta}) \\ |
| - | a_z = (p_y * \dot{\psi}^2) + (-p_x * \dot{\phi}^2) + (0 * \dot{\theta}^2) \\ | + | a_z = (p_y * \ddot{\psi}) + (-p_x * \ddot{\phi}) + (0 * \ddot{\theta}) \\ |
| $$ | $$ | ||
| Line 435: | Line 435: | ||
| Thus the system must be calibrated. On Start-up, we need to externally fix the rotational speed of the submarine to $[\dot{\psi},\dot{\phi},\dot{\theta}] = [0,0,0]$ and then take a measurement of what remains. That will be the offset values. | Thus the system must be calibrated. On Start-up, we need to externally fix the rotational speed of the submarine to $[\dot{\psi},\dot{\phi},\dot{\theta}] = [0,0,0]$ and then take a measurement of what remains. That will be the offset values. | ||
| - | This is generally sufficient. However, submarine missions will run for long periods of time. Competition runs take up to 20 minutes, and pool tests see the submarine in the water for potentially hours at a time. | + | This is generally sufficient. However, submarine missions will run for long periods of time. Competition runs take up to 20 minutes, and pool tests see the submarine in the water for potentially hours at a time. The next section covers how to set-up something better, if desired. |
| ==== Error Tracking ==== | ==== Error Tracking ==== | ||
| Line 513: | Line 513: | ||
| If it can be resolved, however, you will have a gyroscopic system that does not need calibration on start-up, nor suffer from drift during long operation. It should be stated however, that calibration is always good to perform when possible. Introducing a temporary 'calibration sensor' that asserts rotational velocity to be 0 with ~0 variance will force the gyroscopes to set their offsets to the current value no matter what. | If it can be resolved, however, you will have a gyroscopic system that does not need calibration on start-up, nor suffer from drift during long operation. It should be stated however, that calibration is always good to perform when possible. Introducing a temporary 'calibration sensor' that asserts rotational velocity to be 0 with ~0 variance will force the gyroscopes to set their offsets to the current value no matter what. | ||
| - | ==== Magnetometer ==== | + | ===== Magnetometer ===== |
| + | ===== Depth Sensor ===== | ||
| + | A Depth Sensor is a pressure traducer that measures pressure (and sometimes temperature) and determines the depth of water sitting atop it. They tend to be very accurate and reliable. | ||
| + | If somebody wants to work out the math for the minute corrections for pressure added or subtracted based on depth sensor orientation and velocity, they're free too. | ||
| + | |||
| + | The absolute depth (z-position) of the submarine has to describe a single point - typically the submarine's center of mass. This point also serves as the origin for the Submarine's intrinsic coordinate system. | ||
| + | |||
| + | A depth sensor placed arbitrarily on the submarine will measure it's own depth, which will be the depth of the submarine, plus the z-component of it's position. Failing to account for this will result in the control system balancing the depth of the sensor, and not the submarine itself. | ||
| + | |||
| + | Additionally, if the submarine pitches or rolls, the z-offset is altered. A depth sensor placed at $[-0.5,0,0]$ will offer the depth of the submarine without adjustment, however, were the submarine to pitch upwards by $90^o$, the control system would hold the depth //sensor// at a fixed depth, and the true submarine would rise to half a meter above its desired depth. | ||
| + | |||
| + | Therefore, we must account for the absolute position of the submarine, the depth sensors position relative to the submarine, and the orientation of the submarine. | ||
| + | |||
| + | $$ | ||
| + | Depth_{xyz} = Sub_{xyz} + R(\psi,\phi,\theta) * pos_{xyz} \\ | ||
| + | D = | ||
| + | \begin{bmatrix} | ||
| + | D_x \\ | ||
| + | D_y \\ | ||
| + | D_z \\ | ||
| + | \end{bmatrix} | ||
| + | = | ||
| + | \begin{bmatrix} | ||
| + | x \\ | ||
| + | y \\ | ||
| + | z \\ | ||
| + | \end{bmatrix} | ||
| + | + | ||
| + | \begin{bmatrix} | ||
| + | R_{11} & R_{12} & R_{13} \\ | ||
| + | R_{21} & R_{22} & R_{23} \\ | ||
| + | R_{31} & R_{32} & R_{33} | ||
| + | \end{bmatrix} | ||
| + | \begin{bmatrix} | ||
| + | p_x \\ | ||
| + | p_y \\ | ||
| + | p_z \\ | ||
| + | \end{bmatrix} | ||
| + | |||
| + | $$ | ||
| + | |||
| + | We can take the transpose of this equation and reorganize this to be an equation with a single term: | ||
| + | |||
| + | $$ | ||
| + | |||
| + | \begin{bmatrix} | ||
| + | p_x & p_y & p_z & 1 \\ | ||
| + | \end{bmatrix} | ||
| + | \begin{bmatrix} | ||
| + | R_{11} & R_{21} & R_{31} \\ | ||
| + | R_{12} & R_{22} & R_{32} \\ | ||
| + | R_{13} & R_{23} & R_{33} \\ | ||
| + | x & y & z | ||
| + | \end{bmatrix} | ||
| + | = | ||
| + | \begin{bmatrix} | ||
| + | D_x & D_y & D_z | ||
| + | \end{bmatrix} | ||
| + | |||
| + | $$ | ||
| + | |||
| + | Unfortunately for us, depth sensors tell us their absolute z position $D_z$, but they have no way to know $D_x$ or $D_y$. So our measurement is a single scalar value $D_z$. Meaning the first two columns of that state-matrix are irrelevant. | ||
| + | |||
| + | $$ | ||
| + | |||
| + | \begin{bmatrix} | ||
| + | p_x & p_y & p_z & 1 \\ | ||
| + | \end{bmatrix} | ||
| + | \begin{bmatrix} | ||
| + | R_{31} \\ | ||
| + | R_{32} \\ | ||
| + | R_{33} \\ | ||
| + | z | ||
| + | \end{bmatrix} | ||
| + | = | ||
| + | \begin{bmatrix} | ||
| + | D_z | ||
| + | \end{bmatrix} | ||
| + | |||
| + | $$ | ||
| + | |||
| + | Here we have 4 unknowns and 1 measurement. If we know the bottom row of the $R$ Matrix (ie. know our pitch and roll) we can calculate the submarine's depth from there. However, it is clear that we can make the left emission matrix full-rank with proper choice of depth sensor positions. Were we to use 4 or more depth sensors, we could fully determine our roll, pitch, and depth. It would help our accelerometers separate out gravity from its translational acceleration, for instance. | ||
| + | |||
| + | Why do we need 4 depth sensors and not 3? Consider any 3 depth sensors. Their positions would define some sort of plane. Were the submarine to rotate to an orientation where that plane was level with the external $x,y$ plane, the sensors would read equal depth values. If the submarine rolled over 180 degrees, the depth sensors would all be on the same plane again, and the submarine would have no way of differentiating between the two cases. A 4th depth sensor not co-planar to the other three must be added. It will then be above, or below the other three sensors in each case, removing ambiguity. | ||
| + | |||
| + | Thus, to make the emission matrix full-rank, we need at least 4 non-co-planar depth sensors. | ||
| + | |||
| + | $$ | ||
| + | \begin{bmatrix} | ||
| + | p_{1x} & p_{1y} & p_{1z} & 1 \\ | ||
| + | p_{2x} & p_{2y} & p_{2z} & 1 \\ | ||
| + | p_{3x} & p_{3y} & p_{3z} & 1 \\ | ||
| + | p_{4x} & p_{4y} & p_{4z} & 1 | ||
| + | \end{bmatrix} | ||
| + | \begin{bmatrix} | ||
| + | R_{31} \\ | ||
| + | R_{32} \\ | ||
| + | R_{33} \\ | ||
| + | z | ||
| + | \end{bmatrix} | ||
| + | = | ||
| + | \begin{bmatrix} | ||
| + | D_{1z} \\ | ||
| + | D_{2z} \\ | ||
| + | D_{3z} \\ | ||
| + | D_{4z} \\ | ||
| + | \end{bmatrix} | ||
| + | |||
| + | $$ | ||
