11 replies to this topic

[odtu_ee]

I stumbled upon your project while looking for some papers. What you have done is interesting and worth the praise. However, I believe it needs to be significantly improved.
I quickly reviewed your gpsins code. Here is a couple suggestion that may be helpful:

1. In the CovariancePrediction the effect of Q must be multiplied by T not by T^2. (Q is the psd level of WHITE NOISE. the power of its integral is proportional with T not T^2)

2. It would be much better for you to clearly separate INS and Kalman filter implementations. It is absolutely not necessary to propagate (predict) Kalman covariance at the same rate as the navigation states. Updating Kalman covariance so frequently tremendously increases the load of your processor for no good reason at all.

3. You had better use a better INS algorithm. Your current implementation is too simplistic to rely on. At least you must have included the transport rate update equations in your INS algorithms (e.g to your StateEq() function). Even if you have perfect inertial sensors, just ignoring the earth's rotation rate causes 20m errors in 1 minute.

4. Use wgs84 gravity model. Constant 9.81 assumption is not acceptable.

5. Basically only 3 states are sufficient to model the errors on quaternions. (Currently you use 4 states)

6. If I were you I would not use the accelerometers of ST. It seems to me that ST’s inertial team does not know any single thing about inertial navigation. I strongly believe that it will cause lots of undetected problems in your application. Also, (again if I were you) I would prefer intersense’s gyroscope rather than invensens.

7. You must compensate the effect of temperature on the inertial sensors. I could not see any code segment for this purpose in your program (perhaps it is done somewhere else. I did not check the entire project). Even if you do not perform a dynamic temperature compensation, you have to put some additional codes to remove extra bias on sensors outputs during stationary startup.

8. In general, you do not need to use magnetometers during the entire operation. Once you initialize the heading angle with magnetometers, gyroscope based attitude solution is all you need. Magnetometers have non-markovian error components. This means that you cannot effectively process magnetometer outputs with a Kalman filter. In your simple implementation I do not think that you will notice these problems. However,  these problems may become more visible as you improve your navigation codes in the future.


hope these help.

[peabody124]

odtu_ee, on 16 July 2011 - 07:46 PM, said:

1. In the CovariancePrediction the effect of Q must be multiplied by T not by T^2. (Q is the psd level of WHITE NOISE. the power of its integral is proportional with T not T^2)

If you look in the CovariancePrediction function(s) you'll see it uses T^2.  There are a few places in the non-general form where T pops up, but that came from a symbolic expansion of the correct equation.

Quote

2. It would be much better for you to clearly separate INS and Kalman filter implementations. It is absolutely not necessary to propagate (predict) Kalman covariance at the same rate as the navigation states. Updating Kalman covariance so frequently tremendously increases the load of your processor for no good reason at all.

Two thoughts on this though.  A single EKF will be more accurate than two loosely coupled ones, although I admit we haven't systematically tested how much those off the block diagonal terms help.  Also, in this implementation we are still running fast enough so if it aint broke...

Quote

3. You had better use a better INS algorithm. Your current implementation is too simplistic to rely on. At least you must have included the transport rate update equations in your INS algorithms (e.g to your StateEq() function). Even if you have perfect inertial sensors, just ignoring the earth's rotation rate causes 20m errors in 1 minute.

More details here would help.  What do you mean transport rate?  Also I would assume the opposite - far from perfect inertial sensors.  Without the GPS to ground it the position prediction would get quite poor (or SLAM when used indoors).

Quote

4. Use wgs84 gravity model. Constant 9.81 assumption is not acceptable.

Yeah we have a UAVObject definition to allow this to change.  However, we also have the 16 state variant that estimates the accel bias which for mostly level flight is a poor mans way of doing this too.

Quote

5. Basically only 3 states are sufficient to model the errors on quaternions. (Currently you use 4 states)

True, but I'd have to go through the math and implementation to really see if that's a win.  The memory to store the states isn't the issue, it's the computation on it.  If that created a more complex computation or something worse to linearize it might not be an improvement.  To me switching between a 3 and 4 state representation of attitude sort of defeats the whole point of quaternions (i.e. take an extra state to simplify the representation).

Quote

6. If I were you I would not use the accelerometers of ST. It seems to me that ST’s inertial team does not know any single thing about inertial navigation. I strongly believe that it will cause lots of undetected problems in your application. Also, (again if I were you) I would prefer intersense’s gyroscope rather than invensens.

I don't know.  We haven't a problem with them, and have actually been quite happy.  Also Im' not sure what they need to know about inertial navigation as long as it gives me an accurate acceleration.  I haven't heard of intersense but googling briefly looks like a 1500$ part?

Quote

7. You must compensate the effect of temperature on the inertial sensors. I could not see any code segment for this purpose in your program (perhaps it is done somewhere else. I did not check the entire project). Even if you do not perform a dynamic temperature compensation, you have to put some additional codes to remove extra bias on sensors outputs during stationary startup.

That's in there (outside EKF in ahrs.c) but I generally don't use it.  The gyro drift tracking seems to work quite well most of the time.  Sambas had to start throwing it from indoors to the snow to really see a problem (which was fixable by changing the drift rate).

Quote

8. In general, you do not need to use magnetometers during the entire operation. Once you initialize the heading angle with magnetometers, gyroscope based attitude solution is all you need. Magnetometers have non-markovian error components. This means that you cannot effectively process magnetometer outputs with a Kalman filter. In your simple implementation I do not think that you will notice these problems. However,  these problems may become more visible as you improve your navigation codes in the future.

Something we'll keep an eye out for.  My general approach is just to jack up the observation noise on them much higher than their RMS noise while stationary.

Quote

hope these help.

Thanks for the suggestions.  Seems like you know your stuff so hopefully you'll stick around and get involved when pro is available.

[Kenn Sebesta]

peabody124, on 16 July 2011 - 08:21 PM, said:

odtu_ee, on 16 July 2011 - 07:46 PM, said:

1. In the CovariancePrediction the effect of Q must be multiplied by T not by T^2. (Q is the psd level of WHITE NOISE. the power of its integral is proportional with T not T^2)

If you look in the CovariancePrediction function(s) you'll see it uses T^2.  There are a few places in the non-general form where T pops up, but that came from a symbolic expansion of the correct equation.

The conversion from Q_c in continuous time to Q_k in discrete time is done by simply multiplying by delta T. Where is T^2 showing up in the symbolic equations?

Quote

odtu_ee, on 16 July 2011 - 07:46 PM, said:

2. It would be much better for you to clearly separate INS and Kalman filter implementations. It is absolutely not necessary to propagate (predict) Kalman covariance at the same rate as the navigation states. Updating Kalman covariance so frequently tremendously increases the load of your processor for no good reason at all.

Two thoughts on this though.  A single EKF will be more accurate than two loosely coupled ones, although I admit we haven't systematically tested how much those off the block diagonal terms help.  Also, in this implementation we are still running fast enough so if it aint broke...

If I understand correctly, the current implementation is limited in data bandwidth because the processor doesn't have enough spare cycles to devote to preparing all the UAV messages. Would this not be a way to save some processor?

That being said, if it ain't broke, and spending $0.50 more on a processor fixes this second problem, I agree to leave it alone.

Quote

odtu_ee, on 16 July 2011 - 07:46 PM, said:

3. You had better use a better INS algorithm. Your current implementation is too simplistic to rely on. At least you must have included the transport rate update equations in your INS algorithms (e.g to your StateEq() function). Even if you have perfect inertial sensors, just ignoring the earth's rotation rate causes 20m errors in 1 minute.

More details here would help.  What do you mean transport rate?  Also I would assume the opposite - far from perfect inertial sensors.  Without the GPS to ground it the position prediction would get quite poor (or SLAM when used indoors).

I think he means Coriolis effects. I had no idea that they were so pronounced.

Edit: in fact, this is the effect due to inertial rotation of a body as it follows a curved surface, remaining perpendicular to the surface. So as the Earth rotates, if a body stays absolutely rigid in space it will appear to rotate vs. our reference frame, even if it stays put in x-y-z space. Imagine Foucault's pendulum.

Quote

odtu_ee, on 16 July 2011 - 07:46 PM, said:

4. Use wgs84 gravity model. Constant 9.81 assumption is not acceptable.
Yeah we have a UAVObject definition to allow this to change.  However, we also have the 16 state variant that estimates the accel bias which for mostly level flight is a poor mans way of doing this too.

Still, he's right, for most of the Earth's surface where people live, gravity is closer to 9.80 than 9.81. This could be effected immediately for the CC. Surprisingly, gravity being slightly off can really mess with a filter.

Quote

odtu_ee, on 16 July 2011 - 07:46 PM, said:

5. Basically only 3 states are sufficient to model the errors on quaternions. (Currently you use 4 states)

True, but I'd have to go through the math and implementation to really see if that's a win.  The memory to store the states isn't the issue, it's the computation on it.  If that created a more complex computation or something worse to linearize it might not be an improvement.  To me switching between a 3 and 4 state representation of attitude sort of defeats the whole point of quaternions (i.e. take an extra state to simplify the representation).

Yup, yup.

Quote

odtu_ee, on 16 July 2011 - 07:46 PM, said:

8. In general, you do not need to use magnetometers during the entire operation. Once you initialize the heading angle with magnetometers, gyroscope based attitude solution is all you need. Magnetometers have non-markovian error components. This means that you cannot effectively process magnetometer outputs with a Kalman filter. In your simple implementation I do not think that you will notice these problems. However,  these problems may become more visible as you improve your navigation codes in the future.

Something we'll keep an eye out for.  My general approach is just to jack up the observation noise on them much higher than their RMS noise while stationary.

Huh, hadn't thought about that before. However, jacking the observation noise up on them is a good way of effecting exactly the needs of simple flight. My biggest problem with magnetometers is not the noise, but their propensity to being attracted to other magnetic fields. Nothing like flying a quad along to suddenly have it yaw in the direction of an electric cable or piece of steel. That's the non-markovian error component showing up, for sure. What to do about it, though? Monitor the magnetometer for absolute magnitude and then ignore it when in the presence of a secondary field?

odtu_ee, on 16 July 2011 - 07:46 PM, said:

hope these help.

Neat observations, thanks. I surmise you are, or were, in Ankara. What do you know about Türk Hava Kurumu Üniversitesi, www.thk.edu.tr ? They're looking to fill some controls posts and I'm wondering if it's worth pursuing.

Edited by Kenn Sebesta, 19 July 2011 - 02:10 AM.

[Kenn Sebesta]

Quote

Quote

odtu_ee, on 16 July 2011 - 07:46 PM, said:

3. You had better use a better INS algorithm. Your current implementation is too simplistic to rely on. At least you must have included the transport rate update equations in your INS algorithms (e.g to your StateEq() function). Even if you have perfect inertial sensors, just ignoring the earth's rotation rate causes 20m errors in 1 minute.

More details here would help.  What do you mean transport rate?  Also I would assume the opposite - far from perfect inertial sensors.  Without the GPS to ground it the position prediction would get quite poor (or SLAM when used indoors).

I think he means Coriolis effects. I had no idea that they were so pronounced.

Ah, no, I guessed wrong. He actually means something I found in Strapdown Inertial Navigation Technology by David H. Titterton and John L. Weston

Strapdown Inertial Navigation Technology said:

For a vehicle moving at a velocity, vx, in a single plane around a perfectly spherical Earth of radius Ro this rate is given by vx/(Ro + z) where z is the height of the vehicle above the surface of the Earth. This is often referred to as the transport rate.

Edited by Kenn Sebesta, 19 July 2011 - 02:41 PM.

[odtu_ee]

Kenn Sebesta, on 18 July 2011 - 11:07 PM, said:

The conversion from Q_c in continuous time to Q_k in discrete time is done by simply multiplying by delta T. Where is T^2 showing up in the symbolic equations?

For instance, it is written in insgps13state.c (line 361) that:

 //  Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]

(the successive implementation also uses T^2.). It should be changed to
Pnew = (I+F*T)*P*(I+F*T)' + T*G*Q*G'

Kenn Sebesta, on 18 July 2011 - 11:07 PM, said:

I think he means Coriolis effects. I had no idea that they were so pronounced.

V=(Cbn*a+g) - ((wie_n+win_n)xV)
C=(Cbn*S(wib_) - (S(win_n)*Cbn)
In the current code only the first parenthesis are implemented. I meant the calculations in the second paranthesis.

Kenn Sebesta, on 18 July 2011 - 11:07 PM, said:

Huh, hadn't thought about that before. However, jacking the observation noise up on them is a good way of effecting exactly the needs of simple flight. My biggest problem with magnetometers is not the noise, but their propensity to being attracted to other magnetic fields. Nothing like flying a quad along to suddenly have it yaw in the direction of an electric cable or piece of steel. That's the non-markovian error component showing up, for sure. What to do about it, though? Monitor the magnetometer for absolute magnitude and then ignore it when in the presence of a secondary field?

Thank you. This is exactly what I wanted to say. Those external magnetic field deviations instantly spoil everything. I know significant amount paper describing how to self-calibrate mags in a Kalman filter. I really don't understand those people. The problem with mags is not the calibration or noise but those unexpected magnetic deviations. I don't know how to deal with it either. That is why I always try to avoid use of mags as much as possible.

Kenn Sebesta, on 18 July 2011 - 11:07 PM, said:

What do you know about Türk Hava Kurumu Üniversitesi

It has not been opened yet. As it will be a newly born university, I don't think that they will be able to compete with the existing ones.

Edited by odtu_ee, 19 July 2011 - 02:48 AM.

[Kenn Sebesta]

odtu_ee, on 19 July 2011 - 02:46 AM, said:

For instance, it is written in insgps13state.c (line 361) that:

 //  Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]

(the successive implementation also uses T^2.). It should be changed to
Pnew = (I+F*T)*P*(I+F*T)' + T*G*Q*G'

Yup, you're right odtu_ee. Peabody, if you've not had the time yet to look into why this is the case, you can convince yourself that the delT^2 implementation is incorrect based off of dimensional analysis when compared with the original continuous-time Kalman Filter. Or just look at the continuous-time covariance prediction and integrate by delT. Both ways lead to a single delT on Q.

Edit: Whoops, I hadn't correctly interpreted Q and G. Classically, Q is the continuous variance, Q_c, whereas here it's presented as the discrete variance, Q_k, which is variant with delT. So if Q as given in the above is implemented as a function of delT, then it is Q_k and the implementation is correct. If, however, Q is set once and for all, then it is in fact Q_c and the implementation is incorrect. Personally, I prefer not to use Q_k as it makes life sloppy if you move to an asynchronous filter.

Quote

V=(Cbn*a+g) - ((wie_n+win_n)xV)
C=(Cbn*S(wib_B )) - (S(win_n)*Cbn)
In the current code only the first parenthesis are implemented. I meant the calculations in the second paranthesis.

I'm not sure I follow, as I don't know what values are being referred to in the equations. Was the reference I found for "transport rate" not correct?

Quote

Thank you. This is exactly what I wanted to say. Those external magnetic field deviations instantly spoil everything. I know significant amount paper describing how to self-calibrate mags in a Kalman filter. I really don't understand those people. The problem with mags is not the calibration or noise but those unexpected magnetic deviations. I don't know how to deal with it either. That is why I always try to avoid use of mags as much as possible.

Likewise, but they have their purposes. Perhaps with GPS data one should be able to estimate the relative size of the magnetic field (along with declination, but that's another question). Knowing this, it would be possible to filter out the magnetic compass update if ever the field strength were different from anticipated. As you say, and as we have a lot of experience here with, the gyroscopes do a great job of keeping error down so it might be feasible to shut off the mag when in these situations. As long as the perturbation transient is fast enough, one should not lose control.

Quote

It has not been opened yet. As it will be a newly born university, I don't think that they will be able to compete with the existing ones.

Do you have any confidence in new Turkish universities?

Edited by Kenn Sebesta, 19 July 2011 - 04:56 PM.

[odtu_ee]

The issue with Q matrix is in fact one of the minor problems in the current implementation. Therefore, it may be a little bit unnecessary to continue with this discussion. However, let me say this:
We all agree that if Q is a psd level of the white noise (in other words Q=Qc) then the correct formula must be T*G*Q*G'.
On the other hand if Q if is the power (variance) of a white sequence (in other words Q=Qd) as it is claimed in the comments just above the corresponding code segment, then the (approximately - not theoretically but in practice) correct form should be n*G*Q*G' where n=T_covariance_update_time/T_model_discretization_time.



Edit for a little bit more information:
In fact the correct (conventional) technique to compute the covariance is based on van loan's matrix exponential formula. However, at this level, first order taylor series expansion (the method intended to be used in the code) can also be assumed acceptable.

Edited by odtu_ee, 19 July 2011 - 07:17 PM.

[peabody124]

Sorry - I'm not ignoring this discussion.  I'm just not quite as versed in these things as you guys so need to go back to the equations.  I'm getting the impression though that the current implementation is ok.  The Q are estimated by sampling the noise at the same dT as the EKF is run.  The EKF runs at a fixed rate so no variable n for time steps.

As for the issue with the rotational frame of reference, would how severe this is depend on the speed you were going?  For example something with a velocity estimate of zero just sitting there - are the horizontal accelerometers going to read non zero because of it?

[dschin]

I guess I should jump in here since it is (mostly) my code.  Sorry for not seeing this before now.  Finding time for OpenPilot is challenging sometimes.

Kenn Sebesta, on 19 July 2011 - 03:17 PM, said:

odtu_ee, on 19 July 2011 - 02:46 AM, said:

For instance, it is written in insgps13state.c (line 361) that:

 //  Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]

(the successive implementation also uses T^2.). It should be changed to
Pnew = (I+F*T)*P*(I+F*T)' + T*G*Q*G'

Yup, you're right odtu_ee. Peabody, if you've not had the time yet to look into why this is the case, you can convince yourself that the delT^2 implementation is incorrect based off of dimensional analysis when compared with the original continuous-time Kalman Filter. Or just look at the continuous-time covariance prediction and integrate by delT. Both ways lead to a single delT on Q.

Edit: Whoops, I hadn't correctly interpreted Q and G. Classically, Q is the continuous variance, Q_c, whereas here it's presented as the discrete variance, Q_k, which is variant with delT. So if Q as given in the above is implemented as a function of delT, then it is Q_k and the implementation is correct. If, however, Q is set once and for all, then it is in fact Q_c and the implementation is incorrect. Personally, I prefer not to use Q_k as it makes life sloppy if you move to an asynchronous filter.

The T^2 comes from converting the input matrix of the continuous time system to discrete time not from trying to use a psd spec to get a discrete covariance. The Q is a discrete time Q_k, and as Kenn recognizes it is then correct to use the T^2. However, I will argue with Kenn that Q_k does not necessarily change with changing T. If all I do is change the sample rate, without changing any of the front end analog filters on the gyros and accels or change the number of discrete points used in averaging after sampling, then Q_k should not change.  

I know that much of the theory simply states that if we change the sample rate of a noisy signal the discrete time variance changes.  But that is an overly simplified (and often misused) principle.  If I sample white noise with a period T to create a discrete sequence I can find the variance.  Then, I can take every second point from that sequence and find another variance.  Will the new variance be half of the first?  I think it is obvious that the two variances will be the same, since the second data set is just a subsample of the first data set.  However, if instead I cut the sample time in half by averaging every two samples to get a single sample then the variance will be cut in half. Or if when cut the sample time in half I also lower the cuttoff frequency of a front end analog filter (which is like averaging) then I will lower the variance.  The problem is that most the "text books" fail to mention any assumption of increased averaging between samples when lowering the sample rate.

Getting all the discrete conversions when dealing with noise is one of the most complicated things to get right in my experience. So maybe since I have two people with some knowledge of this stuff to check me, I should lay it out better.

Here it is:

I have a dynamic system (the INS), that has a continuous time model and a model converted to discrete time, respectively, as follows:

xdot = F*x + G*u
x(k+1) = A*x(k) + B*u(k)

where from first order approximation

A ~= I+F*T
B ~= G*T

Now consider that u is the gyro and accel measurements, which contain the true accel and angular rate plus noise on each.  So when I "run" the dynamic system I am actually running the discrete time system directly by simulating the continuous system in a computer program. This simulation is the prediction step of the EKF, which I like to call the INS in nav systems. The process noise that is added to this dynamic system comes directly from the noise on my measurements of accel and angular rate.  So I really don't need to consider conversions to discrete time any more if I can directly estimate the discrete covariance, Q_k, of the noise that is feeding into my simulation of the continous time system.

All I need to do is use the discrete time Lyaponov equation to propigate the covariance of x(k), as follows:

P(k+1) = A*P(k)*A' + B*Q_k*B'
      ~= (I+F*T)*P(k)*(I+F*T)' + T^2*G*Q_k*G'

Q_k is estimated directly from the sample variance of the gyros and accel measurements, after filters and averaging, just as they enter the INS.

I am pretty sure we have it implemented correctly, but please do check my logic/math again.


I will have to address some of the other issues in later messages.  They are more subjective .

Edited by dschin, 26 July 2011 - 02:02 PM.
a little clarification

[Kenn Sebesta]

dschin, on 25 July 2011 - 10:21 PM, said:

However, I will argue with Kenn that Q_k does not necessarily change with changing T. If all I do is change the sample rate, without changing any of the front end analog filters on the gyros and accels or change the number of discrete points used in averaging after sampling, then Q_k should not change.  

For a rebuttal, check out slide 6 of http://signal.hut.fi...lmanfilter.pdf. The system has continuous process noise covariance Q_c, and so should follow this relationship. The discrete process covariance can be approximated by Q_k=Q_c* delT, as is shown in slide 7.

[peabody124]

Kenn Sebesta, on 02 August 2011 - 04:32 AM, said:

For a rebuttal, check out slide 6 of http://signal.hut.fi...lmanfilter.pdf. The system has continuous process noise covariance Q_c, and so should follow this relationship. The discrete process covariance can be approximated by Q_k=Q_c* delT, as is shown in slide 7.

I think Dale's point is that it depends a lot on where your noise is truly coming from, since it's never true white noise (which that slide specifies is when it applies).  What comes in on the AHRS gyros (as an example) has an LPF at 100 Hz.

[dschin]

Kenn Sebesta, on 02 August 2011 - 04:32 AM, said:

For a rebuttal, check out slide 6 of http://signal.hut.fi...lmanfilter.pdf. The system has continuous process noise covariance Q_c, and so should follow this relationship. The discrete process covariance can be approximated by Q_k=Q_c* delT, as is shown in slide 7.

Yes, those equations are correct if we have white noise being put into a continuous time system and we want to approximate the behavior of that system with a discrete time model (i.e. we want P_k ~= P(t), when t=k*T). In this case if we use two different sample times to discretize the system then the covariance, P_k, will have approximately the same time history, modeling the continuous time independent of the sample time. The noise level of the discrete time system is determined by the desire to model the continuous time system.

But that is not what we have in most aided INS systems.  We actually have a continuous time system that is converted to discrete time, and then the noise is added in as a discrete noise sequence, after discretization, as we run the prediction step. I want my Q_k to capture this noise level. The level of the noise is determined by our sensors and our sampling scheme, not by the need to develop a discrete model that approximates a continuous time system.  How we do the averaging/filtering helps determine the noise level, along with the actual sensors.  For, example if all we do is cut the sample frequency in half by taking every other data point (no more filtering or averaging) then the variance of gyros and accels will be the same.  This is the noise level that is truly going into my system in the prediction step.  It is the variance of the discrete sequence  (i.e. Q_k), and it is not changing with sample frequency.  Now, however, if I average those two data points then the variance drops.