How can I specify the initial variance of the provided heading in setHeading / addHeadingOffset

[doudou]

This is more a cry for help ... I can do it myself but I don't know how it could be done.

It's necessary to implement a north seeking behaviour (or more accurately, any globally-referenced alignment process) properly.

[doudou]

I'm using the IKF component from the simple_ikf_estimator branch

[saarnold]

My guess is that it would not be strait forward to modify a part of the error covariance matrix in an IKF during runtime. Without modifying the quater IKF library the best option might be to reinitialize the filter. This is probably anyway the best choice when changing the heading from outside. @jhidalgocarrio What do you think?

[doudou]

This is probably anyway the best choice when changing the heading from outside.

Interesting. It's not what resetHeading does right now ...

I'm not familiar with the 3x3 covariance matrix for quaternions ... how would I "compose" one based on my measured heading/heading variance ? Would you have a paper / document / webpage that describes this 3x3 covariance matrix representation ?

[saarnold]

I'm not familiar with the 3x3 covariance matrix for quaternions ... how would I "compose" one based on my measured heading/heading variance ? Would you have a paper / document / webpage that describes this 3x3 covariance matrix representation ?

The 3x3 matrix is the covariance of the orientation in axis angle form. The quaternion itself is not part of the state of the filter. The filter only estimates the error in orientation and applies it to the estimated orientation which is a quaternion.

[chhtz]

This is probably anyway the best choice when changing the heading from outside.

Interesting. It's not what resetHeading does right now ...

I guess, a cleaner way to implement resetHeading would be to make it equivalent to a measurement of yaw==0 with essentially 0 covariance -- this would also slightly adapt the current angular velocity (and gyro offsets and their covariances). I did not really have a look into the quater_ikf implementation, though

[jhidalgocarrio]

I did not have a look to the task in a very long time. My approach would be:

• Method resetAttitude (having a look to the code resetHeading actually reset the attitude). Put the heading to zero in case you want and pitch and roll to the current one.
• Reset the filter with the Init method from the quater_ikf. In this method the initial covariance matrix (P_0) is required as an argument. You can use the same one specified in the Task::configureHook. The top-left corner of P_0 is the attitude covariance.
• Invoke the method quater_ikf.setAttitude(reset_attitude)

[saarnold]

I guess, a cleaner way to implement resetHeading would be to make it equivalent to a measurement of yaw==0 with essentially 0 covariance -- this would also slightly adapt the current angular velocity (and gyro offsets and their covariances).

Wouldn't this false measurement affect the estimated biases and uncertainties in a deceptive way? Since the orientation didn't actually change. Changing the orientation itself shouldn't be an issue since it is not directly part of the filter state. I just thought it might would make sense to reset the biases and uncertainties as well when reseting the heading.

I agree with Javier's approach, this is what I also had in mind. We could change the current methods to also support an uncertainty. Keeping the current pitch and roll and their 2x2 covariance matrix, but reset the rest of the internal state to the given uncertainty for yaw and initial values for the rest.

@doudou If you agree I can also implement it.

[chhtz]

Wouldn't this false measurement affect the estimated biases and uncertainties in a deceptive way?

I guess the question is what the intention of that method is. I would have interpreted it as something like "no matter how much I have drifted away, I know that I'm at heading=0 again." And in that case, I would expect the biases to change accordingly.

I agree, it is a different thing if the orientation estimator is used as a pure attitude estimator and one just wants to reset the heading (and its covariance) from time to time ...

[saarnold]

"no matter how much I have drifted away, I know that I'm at heading=0 again." And in that case, I would expect the biases to change accordingly.

Yes, the propose of this resetHeading method is more to do a delayed initialization of the heading when the actual heading is unknown before.

[jhidalgocarrio]

I guess the question is what the intention of that method is

Exactly. If the intention is to estimate the bias in order to give a number of its estimation the init approach does not fit. But since our intention is to estimate the attitude, and for it we need to have an estimation of the bias, at the moment we reset the attitude we should reset the bias.

The Init methods is the safest approach because we do not need to take care of the cross-covariance term in the P matrix (e.g.: cross matrix terms between error quaternion and bias).

[doudou]

Yes, the propose of this resetHeading method is more to do a delayed initialization of the heading when the actual heading is unknown before.

Yes, exactly. Since we seem to agree on the goal, @saarnold, I wouldn't mind taking on your offer. I have to admit that I'm still murky on the 3x3 covariance matrix ...

[saarnold]

I extended #7 accordingly.

[doudou]

Great. I'll try it probably this week end. Thanks !

[doudou]

@saarnold I tried your code. It does correct the heading and does change the covariance on the (3,3) element. However, this covariance very quickly gets lower and lower, to finally converge in a few seconds to something ridiculously low (1e-6 or lower). The overall norm of the covariance matrix goes down as well, so it's not that the variance is "propagated" to the other coordinates. At the end, the filter does report a very accurate heading which is obviously wrong.

We're not using any global heading measurement (i.e. no magnetometers) and the north seeking is disabled.

[saarnold]

As soon as there is any pitch or roll the acceleration also observes the heading, the uncertainty of the acceleration defines how fast it will converge. You could increase the randomwalk of the accelerometers. According to the kvh 1750 data sheet the randomwalk is <= 0,0011768 m/s^2/sqrt(Hz)

[doudou]

As soon as there is any pitch or roll the acceleration also observes the heading, the uncertainty of the acceleration defines how fast it will converge.

How would that would be possible ? It can definitely correct gyro biases, but I fail to see how observing the gravity vector could be an absolute heading measurement.

[saarnold]

Yes your are right, it shouldn't affect the heading uncertainty, at least not to this rate. I'll look into it, tomorrow.

[doudou]

I'll look into it, tomorrow.

Thanks ! I really appreciate that.

[saarnold]

What happens I think is that given a too small uncertainty in the acceleration measurements the filter becomes overconfident. Besides of being to confident about the heading this seems also to mess up the estimated biases. Just by looking at the uncertainty of the raw measurements of the KVH a sigma of ~0.025 seems to be more appropriate. After several runs on a test data set containing a few minutes of rotations at the beginning followed by a long static phase, I would even recommend a value of 0.16 for the acceleration randomwalk.

Another issue seemed to be the initial covariance of the bias values. Especially when the bias instability is smaller then the initial one. I fixed it in #7 by using the bias instability as initial covariance.

[doudou]

Thanks.

But if we do agree that the accelerometers convey no information on the heading, I fail to see how the accelerometer variance should have an effect on the heading variance. In theory, even perfect accelerometers should still not cause a the variance of the heading to converge.

Obviously, making the accelerometer's variance higher will cause the filter to either not converge at all or much slower, i.e. hiding the real problem.

Or am I missing something again ? I'm seeing this from afar, and did not look into the overall filter design in detail.

[saarnold]

The filter does update it's full orientation using the acceleration. I compared the implementation with the paper it is based on and it seems to be correct. In the paper the update is always a combination of acceleration and magnetometer measurement, I guess that is the reason why there is no special handling.

In case of the magnetometer measurement however only the yaw angle is updated. I could add something similar for the acceleration update, excluding the yaw angle. I did some test's and it seems to work, but I can't guarantee that it doesn't have some unwanted side effects. If you want I can push it on a branch.

Personally I would use the filter in the current state with the increased acceleration variance and focus to improve this in the new filter.

[doudou]

If you want I can push it on a branch.

That would be great, yes.

Honestly, at that stage, I was going to add the variance of the initial guess to the output variance post-filter. If the filter does reduce the heading variance with no information that it can use to actually reduce the variance of the estimate, this probably means that this filter is simply not designed for our use-case (with no absolute heading measurement).

Personally I would use the filter in the current state with the increased acceleration variance

I wouldn't. Toying with variances in a kalman filter and hoping that it won't blow in our face is not really something I'm keen on.

and focus to improve this in the new filter.

Generally speaking: sounds like a plan. However, I need this working now ...

In any case, thanks for the help

[saarnold]

If the filter does reduce the heading variance with no information that it can use to actually reduce the variance of the estimate, this probably means that this filter is simply not designed for our use-case (with no absolute heading measurement).

I agree.

Generally speaking: sounds like a plan. However, I need this working now ...

I know.. It is now on the branch excluded_yaw of quater_ikf (in the BIR fork)

[doudou]

Thanks Sascha, I'll try that.

While applying your change, I noticed that our version has one commit that the branch does not have, in which the gyro biases are forcefully set to zero in the filter update method (line 592 of Ikf.hpp). If it can refresh your memory, it is marked as Sascha's sugestion to stop the yaw drift ( same fix was done in germany ). Should we keep that ?

[doudou]

Tried in simulation and I get the same problematic behaviour.

I've looked into the log stream. The covariance on (9,9) is the (3 deg)^2 that I give (around 0.0273 rad^2) for a second - give or take 100ms - and then drops suddenly to 2e-6 and from there goes 1e-7 - 1e-8.

I tried with or without the zeroing of the gyro biases, same thing. Given that it works for you, I really fear that we're having some weird hack somewhere, or that some configuration parameter makes no sense.

[saarnold]

Should we keep that ?

There is no need, the modified update deactivates also the estimation of biases (Which is fine for our sensor and use-case). Inflating the noise on the accelerometers also limits the bias from going to fast to ambitious.

Given that it works for you, I really fear that we're having some weird hack somewhere, or that some configuration parameter makes no sense.

If you can do a log of a view minutes, I would look into it.

[doudou]

If you can do a log of a view minutes, I would look into it.

Given that the covariance drops after a second, do you need a few minutes, or would a few seconds before and after do fine ?

[saarnold]

30-60 Seconds should be sufficient.

[doudou]

I'll do it this evening most probably. Thanks again for you help.