EKF Angular Velocity Instability

[tpreynolds]

@kashton6 can you fill in a description here?

[kashton6]

Description of Problem: EKF seems to perform best only at low angular velocities. The higher it goes, the worse the attitude estimation becomes. Secondly, the bias estimate seems to be always several orders of magnitude too large. Additionally, changing the angle random walk/rate random walk estimate values in the init_MEKF.m file have huge impacts on attitude estimation.

[tpreynolds]

Okay, I’ve hopefully got some ideas. I’m able to recreate the problem. To simplify things, use [0.1;0;0] as the angular velocity. It isolates the issue to 1 axis.

1. check the units that you’re inputting to the filter. Is it designed to work with units of Tesla for mag and a unit vector for the sun, or unit vector for both? In the past I’ve used Tesla (I think)
2. I think you need to have the integration rate (of the unit test) match the value of dt that you used to compute Qchol and Rchol in the init file. I think this is happening, but double check. Make sure the integration is fixed-step, solver is auto, and the step size is set to the value we want.
3. What I'm seeing is some lag in the attitude estimate. The angular velocity estimate is fine, but the quaternion falls more and more out of phase. See the figure attached, which I got using my filter and a unit test built up to match yours (how I verified I could recreate the error). The input units are T for the magnetometer here, with an inertial vector of [14491; 3674; 39514] *1e-9.

Eliminating the noise from the sensor measurements does not seem to solve the problem.

[tpreynolds]

I don't have a copy of the book at home, so I will go into the lab tomorrow to grab it and take another look.

[kashton6]

1. I will try to find somewhere that specifies the input units as far a mag/sun sensors go. A while ago, I found one place where they used directional vectors in degrees to both the sun and mag vector. In my test, I just mimicked having two inertial directions we are measuring, same units, just different directions. I felt like that would be okay in the test since the star camera example used consistent measurements, too (they didn't mix units). Maybe not trying a unit vector for the sun measurements as well is better?

2. Yes, you are correct, I made sure that in the bottom right hand side of the simulink model that the timesteps are all uniform across the sim and the input matrices Qchol, set to fixed-step and auto. I also tried discrete, but made no difference.

3. That lag you see, I have seen in two cases. The first case, the process noise values, sig_u/r are not at 'appropriate values'. Meaning if you change them to both be sqrt(10)*1e-6, for example, the estimates are great with the new mag inertial vector you prescribed. Ideally though, these values should be closer to the ARW and RRW values... The second case where that lag develops, I have seen in the pure propagation stage. But that does not seem to be the case for the test currently with the validity vectors always ==1. I was not able to recreate your angular velocity errors though. I would be curious to see what your bias looked like there as well.

[kashton6]

Actually, the file was too large, I uploaded to our reference material drive: https://drive.google.com/drive/u/0/folders/1wNfjSqpztZ120XqLAocaAm2iOV3vOI9A

…

On Thu, Apr 16, 2020 at 1:55 PM _ ***@***.*** ***@***.***> wrote: I am attaching a copy of the book, as well On Wed, Apr 15, 2020 at 10:58 PM Taylor Reynolds ***@***.***> wrote: > I don't have a copy of the book at home, so I will go into the lab > tomorrow to grab it and take another look. > > — > You are receiving this because you were assigned. > Reply to this email directly, view it on GitHub > <#43 (comment)>, > or unsubscribe > <https://github.com/notifications/unsubscribe-auth/AJLVMS23PCZQJYQWDWEL3VDRM2M7HANCNFSM4MHN5C6Q> > . > -- Kylle Ashton M.Sc. Mechanical Engineering | University of Washington

-- Kylle Ashton M.Sc. Mechanical Engineering | University of Washington

[kashton6]

I am attaching a copy of the book, as well

…

On Wed, Apr 15, 2020 at 10:58 PM Taylor Reynolds ***@***.***> wrote: I don't have a copy of the book at home, so I will go into the lab tomorrow to grab it and take another look. — You are receiving this because you were assigned. Reply to this email directly, view it on GitHub <#43 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AJLVMS23PCZQJYQWDWEL3VDRM2M7HANCNFSM4MHN5C6Q> .

-- Kylle Ashton M.Sc. Mechanical Engineering | University of Washington

[tpreynolds]

1. Hm, by directional vectors do you mean unit vectors (those are unit-less)? I think if we're normalizing these vectors before they go into the filter, then we're effectively coupling the noise terms across the dimensions. For example, that'd be saying that the x-component of the measurement is affected by the y- and z-component's noise terms. So the measurement noise covariance might have to take that into account via the off-diagonal terms. If you don't normalize, then uncoupled noise (like we have now) is OK. Units wise, I think the units used for the variance need to match the units of the measurement. The values in my code are not normalized, so if you borrowed the mag variances...that would explain why the updated mag vector improved things.
   a. Related to this, I think we want to make sure that the noise we're simulating with is orthogonal to the estimated measurement in the body frame. (See Eq.(6.5) in Crassidis). The reason for this is so that the lengths of any two "unit" vectors are the same to first order. This is not something that I did previously, but I've got two references that say to do it this way.

2. It seems like we're not able to correctly estimate the bias. What I see is that the estimated bias moves (nearly linearly) away from its "true" value that is roughly constant, and this causes the angular velocity to slowly move away too. In turn, this causes the quaternion estimate to drift away from it's true value. To me this points to the Kalman gain calculation, since this what gets used to compute \Delta \beta_{k,plus} in Eq. (7.32) -- and I can't pinpoint anywhere else where an error could be introduced into the bias estimate. Can you point me to the reference for the QR decomposition method you've used in the code?

3. In the current gyro model, comment through the resolution blocks (command+Y on mac).

[kashton6]

1. By directional vectors, I meant the measurement values going into the ekf. Prior to the noise being added on, I just had the vectors arbitrarily chosen as [1;0;0] or [0;1;0] as the inertial vector. I was not normalizing them in anyway, they just happened to be chosen as vectors with unit 1 norm. So in that case, it sounds like the noise correlations are still decoupled, right? Implied in the measurement variance matrix are that the units for the variances are the square of the units in the measurements. So the first three rows of the meas. variance matrix corresponding to the sun vector have units of radians^2, where the mag variance diagonals (last three rows) are tesla squared, since the variances are the square of the standard deviation, which has the same units of the measurements.

To your note, I just mentioned in the last GNC meeting that my next direction was to check the 'sensor model' that I have. I thought that maybe that adding WGN to the measurement could be the wrong thing to be doing, like maybe it was physically unrealistic. I did not realize that the noise needed to be orthogonal. I was playing around with adding in the sun sensor library that Cole/you made. Was that, and the mag sensor lib, constructed to add orthogonal noise? And when the sun 2x1 (alpha, beta) dimension sensor measurements get converted back to 3x1, is this noise still orthogonal?

2. Page 30 has the equations for the kalman gain and covariance update/propagation. http://www2.unb.ca/gge/Pubs/TR140.pdf No where on the internet have I found someone doing the actual math for the measurement update steps, however this video: https://www.youtube.com/watch?v=XefqNr9dj7I shows, starting at the 14 minute mark how the time update R (as in qr) matrix is formed using the pre-array matrix, also derived. You can cross check witt my code as well. I assume similar math went into the meas. update stuff as well. Its worth noting that I have found multiple sources with the same equations, but literally no where that is derived in full. But its not like I threw it in and hoped it was right, I compared already with the classic formulation, and the results are always identical. I just checked the bias as well with the original formulation and it was the same thing as with the sqrt stuff- linear-like growth. I can revert the lib back to the classic form for right now if you're not convinced, it would take like 2 minutes. But it seems like the measurement/sensor model might be the thing to focus on?