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 14:36] Brian Moore [Gravity] |
cs:localization:sensors:start [2017/09/03 13:52] (current) Brian Moore [Translational and Rotational Acceleration] |
||
|---|---|---|---|
| Line 177: | Line 177: | ||
| $$ | $$ | ||
| - | a_t = \frac{a_1 + a_2}{2},\quad a_c = \frac{a_1 - a_2}{2} | + | a_t = \frac{a_1 + a_2}{2},\quad a_r = \frac{a_1 - a_2}{2} |
| $$ | $$ | ||
| 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 397: | Line 397: | ||
| {{ cs:localization:sensors:accel_rank_9.png }} | {{ cs:localization:sensors:accel_rank_9.png }} | ||
| - | By removing up to two columns at a time, we can find any pair of columns that are mutually dependent. The first and third collection of three columns are both clearly independent, as removing any one of them diminishes the rank by 1, and removing any 2 diminishes the rank by 2. They correspond to the rotational and centripetal accelerations. That leaves the rotation matrix elements, and the translational accelerations as co-dependent. Interestingly, there is some information relationship between pairs $(\ddot{x},R_{31})$, $(\ddot{y},R_{32})$, and $(\ddot{z},R_{33})$. They have some degree of specific co-dependence, such that losing either column vector does not diminish rank but losing both will. | + | By removing up to two columns at a time, we can find any pair of columns that are mutually dependent. The first and third collection of three columns are both clearly independent, as removing any one of them diminishes the rank by 1, and removing any 2 diminishes the rank by 2. They correspond to the rotational and centripetal accelerations. That leaves the rotation matrix elements, and the translational accelerations as co-dependent. |
| + | |||
| + | This makes sense, as an object can not tell the difference between [normal forces opposing] gravity and acceleration. Rotational and centripetal accelerations can be cessed out because they are felt differently based on position. Translational acceleration and gravity are felt equally and universally. | ||
| - | In truth, this can be explained by the fact that the three rotation vector elements only represent 2 unique pieces of information, pitch and roll. One of them is technically redundant - though necessary to allow us to calculate rotations as linear functions. | ||
| Anyway, as this shows, we need some other information from another sensor to fix the values of either the translational accelerations, or the pitch and roll of the system, in order for us to estimate the other state variables. It also shows that we need only 3 3-axis accelerometers to collect all unique data. Anything beyond this will just provide redundant information that reduces noise - helpful, but not vital. The Kalman filter allows us to make estimations of all states absent other measurements, since it uses the forward-predicted values of the states as additional pseudo-measurements, which in and of itself is a full-rank 'measurement'. However, those measurements will not be very accurate. Accelerometers should be augmented with more sensors. | Anyway, as this shows, we need some other information from another sensor to fix the values of either the translational accelerations, or the pitch and roll of the system, in order for us to estimate the other state variables. It also shows that we need only 3 3-axis accelerometers to collect all unique data. Anything beyond this will just provide redundant information that reduces noise - helpful, but not vital. The Kalman filter allows us to make estimations of all states absent other measurements, since it uses the forward-predicted values of the states as additional pseudo-measurements, which in and of itself is a full-rank 'measurement'. However, those measurements will not be very accurate. Accelerometers should be augmented with more sensors. | ||
| ===== Gyroscopes ==== | ===== Gyroscopes ==== | ||
| + | Gyroscopes are nice and simple. They measure the rotational velocity about an axis. 3 together provide you with a direct measurement of your roll, pitch, and yaw velocities $\dot{\psi},\dot{\phi},\dot{\theta}$. Rotational velocity is felt equally in all placed on a rigid body, so placement of the gyroscopes does not factor in (though proper alignment with the $x,y,z$ axes of the submarine's intrinsic coordinate system is necessary). | ||
| + | Gyroscopes are, however, subject to drift. As they operate, the velocity will have some offset. As a function of temperature and time, this offset value will slowly change. Over short instances it is not problematic - the estimates of angular acceleration $\ddot{\psi},\ddot{\phi},\ddot{\theta}$ the Kalman Filter will generate based off gyroscope measurements will be very accurate. However, the rotational velocity itself is an important measurement and should be accounted for. | ||
| + | |||
| + | $$ | ||
| + | |||
| + | G = | ||
| + | \begin{bmatrix} | ||
| + | G_{\dot{\psi}} \\ | ||
| + | G_{\dot{\phi}} \\ | ||
| + | G_{\dot{\theta}} \\ | ||
| + | \end{bmatrix} | ||
| + | = | ||
| + | \begin{bmatrix} | ||
| + | \dot{\psi} \\ | ||
| + | \dot{\phi} \\ | ||
| + | \dot{\theta} \\ | ||
| + | \end{bmatrix} | ||
| + | + | ||
| + | \begin{bmatrix} | ||
| + | err_{\dot{\psi}} \\ | ||
| + | err_{\dot{\phi}} \\ | ||
| + | err_{\dot{\theta}} \\ | ||
| + | \end{bmatrix} | ||
| + | $$ | ||
| + | |||
| + | This presents a problem. We have two unknown values - the true velocity measurement, and the offset, for every one piece of information - the Gyroscope measurement. | ||
| + | |||
| + | 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. The next section covers how to set-up something better, if desired. | ||
| + | |||
| + | ==== Error Tracking ==== | ||
| + | |||
| + | A possible solution is to have the system itself track the offset of each axis of each gyroscope as an extra state variable. Mathematically, the set-up would look like this: | ||
| + | |||
| + | $$ | ||
| + | \begin{bmatrix} | ||
| + | 1 & 0 & 0 & 1 & 0 & 0 \\ | ||
| + | 0 & 1 & 0 & 0 & 1 & 0 \\ | ||
| + | 0 & 0 & 1 & 0 & 0 & 1 | ||
| + | \end{bmatrix} | ||
| + | \begin{bmatrix} | ||
| + | \dot{\psi} \\ | ||
| + | \dot{\phi} \\ | ||
| + | \dot{\theta} \\ | ||
| + | err_{\dot{\psi}} \\ | ||
| + | err_{\dot{\phi}} \\ | ||
| + | err_{\dot{\theta}} \\ | ||
| + | \end{bmatrix} | ||
| + | = | ||
| + | \begin{bmatrix} | ||
| + | G_{\dot{\psi}} \\ | ||
| + | G_{\dot{\phi}} \\ | ||
| + | G_{\dot{\theta}} \\ | ||
| + | \end{bmatrix} | ||
| + | |||
| + | $$ | ||
| + | |||
| + | Obviously, this will have trouble working on its own. We have 6 unknown values and only 3 measurements. However, let's see what happens when we use an additional gyroscope. | ||
| + | |||
| + | $$ | ||
| + | \begin{bmatrix} | ||
| + | 1 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\ | ||
| + | 0 & 1 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\ | ||
| + | 0 & 0 & 1 & 0 & 0 & 1 & 0 & 0 & 0\\ | ||
| + | 1 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\ | ||
| + | 0 & 1 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\ | ||
| + | 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 1 | ||
| + | \end{bmatrix} | ||
| + | \begin{bmatrix} | ||
| + | \dot{\psi} \\ | ||
| + | \dot{\phi} \\ | ||
| + | \dot{\theta} \\ | ||
| + | err_{1\dot{\psi}} \\ | ||
| + | err_{1\dot{\phi}} \\ | ||
| + | err_{1\dot{\theta}} \\ | ||
| + | err_{2\dot{\psi}} \\ | ||
| + | err_{2\dot{\phi}} \\ | ||
| + | err_{2\dot{\theta}} \\ | ||
| + | |||
| + | \end{bmatrix} | ||
| + | = | ||
| + | \begin{bmatrix} | ||
| + | G_{1\dot{\psi}} \\ | ||
| + | G_{1\dot{\phi}} \\ | ||
| + | G_{1\dot{\theta}} \\ | ||
| + | G_{2\dot{\psi}} \\ | ||
| + | G_{2\dot{\phi}} \\ | ||
| + | G_{2\dot{\theta}} \\ | ||
| + | \end{bmatrix} | ||
| + | |||
| + | $$ | ||
| + | |||
| + | This is better. Now we have 9 unknowns and 6 measurements. However, no matter how many gyroscopes we add, we will always be 3 measurements short. We need some prime gyroscope with no error to definitively fix the rotational rates, after which the remaining gyroscopes could be used to solve for their own errors. | ||
| + | |||
| + | This doesn't really seem to help us at all. We wouldn't need to track offset errors if we could make a perfect gyroscope. Alone, gyroscopes cannot track their own error. | ||
| + | |||
| + | However, we're not alone. We've got other sensors, and a Kalman Filter. The Kalman Filter provides predicted locations as state variable measurements based on previous states. And the Kalman Filter's states are informed by multiple sensors, including absolute orientation information obtained from magnetometers, accelerometers, and depth sensors, among other things. | ||
| + | |||
| + | Thus, through the Kalman Filter, the sensors describing orientation will also estimate the derivative of orientation - the rotational velocity. If a magnetometer continues to point in the same direction while a gyroscope //insists// the submarine is yawing at $5^o/sec$, the Kalman Filter will resolve the two, estimate a velocity of $2.5^o/sec$ and an orientation to match. The gyroscope's drift will be estimated to be $+2.5^o/sec$. On the next measurement 1 second later, the magnetometer will indicate a heading of $-2.5^o$ and suggest a yaw velocity of $-2.5^o/sec$ while the gyroscope, will assert a rotational velocity of $2.5^o/sec$, after subtracting its offset. The Kalman Filter will again resolve this, and estimate a gyroscopic drift closer to $5^o/sec$, after which the gyroscope and magnetometer should continue to be in agreement until significant drift accumulates in the gyroscope again. | ||
| + | |||
| + | In reality, it is not quite as clean as the above example. In that example, the derivative measurement of the magnetometer was assumed to be absolutely correct, and the gyroscope did not resist its drift changing by several degrees in a few seconds. Gyroscopes do not drift that quickly, and the offset should know and assert that. Meanwhile, magnetometers will also be noisy, and jump around within a certain range, so their noise will inform the system not to trust it - or derivatives derived from its changes between time-steps, to be fully trusted. The Kalman Filter properly informed will still slowly adjust the drift until the gyroscope and magnetometer converge at some point, but it will do so according to the weight given to the two sensors and the offset based off of noise. | ||
| + | |||
| + | As part of the Process noise, the gyroscope's offsets must be given their own value which indicates it's likelihood to drift over time. This value will come into conflict with the error measured between the noisy gyroscope and magnetometer. Too high of a process noise, and all disagreements between the two sensors will just be attributed to a rapidly changing offset error, and the actual gyroscope measurements will not contribute to the state estimation. Too low of a Process Noise, and the gyroscope will stubbornly believe itself over the magnetometer (or other orientation sensors) and force the Filter to estimate significantly deflected headings as rotational. Finding a proper balance between the weights given to the magnetometer, gyroscope, and offset drift noise is the key to resolving this issue. | ||
| + | |||
| + | 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 ===== | ||
| + | |||
| + | ===== 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} | ||
| + | |||
| + | $$ | ||
