a Kalman filter to fuse gyro and compass readings

[HaWe]

not exactly, Thorvald Nicolai Thiele (* December 24, 1838 in Kopenhagen; † December 26, 1910) and Peter Swerling (*March 4, 1929 – August 25, 2000) developed a similar algorithm before Kalman, and Kalman invented his mathematical filter independently from later application in avionic or space travel.

The filter is named after Rudolf E. Kalman, though Thorvald Nicolai Thiele and Peter Swerling developed a similar algorithm earlier. Stanley F. Schmidt is generally credited with developing the first implementation of a Kalman filter. Richard S. Bucy of the University of Southern California contributed to the theory, leading to it often being called the Kalman-Bucy filter. (...) Kalman's ideas on filtering were initially met with vast skepticism, so much so that he was forced to do the first publication of his results in mechanical engineering, rather than in electrical engineering or systems engineering. Kalman had more success in presenting his ideas, however, while visiting Stanley F. Schmidt at the NASA Ames Research Center in 1960.

But back to the topic:
filtering the Gaussian errors is very important, but as both the compass and the gyro sensor as well as odometry suffer from incredible systematic errors it is very substantial to erase those non-Gaussian errors too - any ideas?

[physics-matt]

filtering the Gaussian errors is very important, but as both the compass and the gyro sensor as well as odometry suffer from incredible systematic errors it is very substantial to erase those non-Gaussian errors too - any ideas?

It all boils down to whether you have a model to handle these systematic errors. This is because what you're trying to determine is the most likely position of the robot given all the available data, and you need a model in order to do this.

If we denote the robot position x (in this case we're only talking about orientation, but it doesn't matter) and the data D, then we are after

x* = max_x Prob(x | D)

The Kalman filter works this out for you by assuming that there is a linear relationship between measurements, D, and x and that all noise is Gaussian. There are other methods for use in other situations, like the extended Kalman filter or particle filters, but these often involve much more complex calculations which may be too slow on a platform like the NXT.

What Aswin has done is to assume that measurements that appear to be spurious provide no reliable information. (This is a technique often used in tracking algorithms, where you try to filter out noise that doesn't correspond to the target you're trying to track) A more precise model would try to allow for the systematic errors, maybe by having a separate distribution for them. If this were Gaussian (or you could assume it was), then you might be able to alter the Kalman filter slightly to account for this model. During the update stage, if you decide that your measurement was spurious, then apply a KF step that uses a different Gaussian.

Matt

[HaWe]

It all boils down to whether you have a model to handle these systematic errors. This is because what you're trying to determine is the most likely position of the robot given all the available data, and you need a model in order to do this.
(...)
The Kalman filter works this out for you by assuming that there is a linear relationship between measurements, D, and x and that all noise is Gaussian. There are other methods for use in other situations, like the extended Kalman filter or particle filters, but these often involve much more complex calculations which may be too slow on a platform like the NXT.
(...)
What Aswin has done is to assume that measurements that appear to be spurious provide no reliable information. (This is a technique often used in tracking algorithms, where you try to filter out noise that doesn't correspond to the target you're trying to track) A more precise model would try to allow for the systematic errors, maybe by having a separate distribution for them. If this were Gaussian (or you could assume it was), then you might be able to alter the Kalman filter slightly to account for this model. During the update stage, if you decide that your measurement was spurious, then apply a KF step that uses a different Gaussian.

Matt

yes, we have to keep the algorithm short and simple if it has to work on the NXT itself. And I assume that Aswin's KF is much better than just taking simple average (mean) or median or both.

But what about my idea of a "voting" of e.g. 3 inputs as I wrote above?
We could make a preselection by discarding the 1 reading with the most unlikely result and calculate the heading (and/or position) by the two remaining ones (as 2 inputs for the KF)?
That would require changing input sources for the KF:
(1) if compass is unlikely: take gyro+odometry,
(2) if odometry is unlikely: take compass+gyro,
(3) if gyro is unlikely: take compass+odometry
(4) In case we have an undoubtedly correct input value (e.g. from an extra sensor, like a detected mark on the floor) we could take this and discard the rest.
In cases (2), (3), and (4) we could use the result to automatically re-calibrate the inputs (in case of a drifting gyro or slippery wheels).
In case we have an undoubtedly correct input value (like a mark on the floor) we could take this and discard the rest.

But what we actually need to have for navigation is both: the heading and the position, and both calculated seperately from all single sensor inputs.

@aswin:
as I have no gyro yet and I don't already see through all your code details:
do you think your code can be modified that way?

[aswin0]

Doc,

About more inputs
Yes you can add more inputs to the filter. Even the NXT is powerful enough to add one or two extra sources of information (especially if the information from the different sources is not correlated). But I doubt that odometry will be useful. In case of my filter I could also calculate heading or turn rate from odometry. Odometric heading is subject to accumulation of errors, so the filter will ignore this info altogether after a few seconds. Odometric turn rate is not subject to accumulation of errors. So this can be used. But how reliable is this information in comparison to the gyro? I have never run any test so I cannot be sure. But Let me visualize the noise of the gyro. Take a one second big pie from a analogue clock and divide this pie in 6 equal parts. This is how big the noise of the gyro is. I find it hard to imagine that one can build a robot that has an odometric error that comes even close.
I do think however that one can use odometry to update the gyro offset. Whenever the robot is motionless you can use raw gyro readings to refine/update the offset.

About a voting mechanism
Why do you want this? Is it to keep the number of calculations low? This could help. Or is it to make the results of the filter better? This will not help, the filter itself has a better way to weight the quality of all the inputs. Or is it to keep non guasian errors out? This is what my filter already does with compass readings it doesn't trust.

Towards localization
For me the next step towards localization will be to use rotation sensors from the motors and an accelleration sensor to calculate speed. Again I will use a Kalman filter to fuse their readings. Once I have both speed and direction and their respective errors I can calculate a location, again with a known error. An error that will still grow bigger over time. That is why At this time I need some reference information to correct location from time to time.

About the undoubtedly correct input value
There is no such thing, you'll have to accept that. Even the red dot on your floor can sometimes be missed by your sensor. Or some stains might be seen as the red dot. You'll always have to take uncertainty into account. But You are right that this red dot (or any information from a sensor that can be related to a position) can be regarded as a reference and help to improve the information about the robots position. Once you have accepted that all things are uncertain there are endless possibilities that can help to correct the position of the robot. There is a well known technique called SLAM were the robot collects reference information while driving around and stores this in a map, at the same time it uses the map, its sensors and its calculated location to improve its estimation of the location.


About me and my project
I do not think that a problem like localization can be solved overnight. That is why I have choosen to make small steps at a time. First step is to improve heading information. My filter does exactly this, nothing more. Next step is to get speed information of a known quality. Then I will use odometry based on heading and speed to get an idea of location. Only then I will start looking for reference information to improve location. By then we have NXT 7.0, this will have enough memory and CPU power to use SLAM on the brick I hope

About my code
Yes you can alter my code any way you like. But first you should buy a gyro and use the filter to find out where it needs improvements. There is no use in trying to correct problems that are not understood or that are irrelevant.

[physics-matt]

And I assume that Aswin's KF is much better than just taking simple average (mean) or median or both.

Yes it is better. You have to ask yourself: why do we take the mean or median? It's because for a large range of problems they give good estimates for the most likely "true" value we are trying to measure. However, this problem is not well modelled in this way, but it does appear to satisfy (loosely) the assumptions behind the KF.

But what about my idea of a "voting" of e.g. 3 inputs as I wrote above?
We could make a preselection by discarding the 1 reading with the most unlikely result and calculate the heading (and/or position) by the two remaining ones (as 2 inputs for the KF)?

This is possible, though why do you assume that only one of the three values will need to be discarded? Or, indeed, do you always need to discard one of the results?

Of course, some kind of scheme like that may produce better results; you would have to try it. Ultimately, as I've said, we're trying to calculate the most likely position (x*) given the data (D):

x* = max_x Prob(x | D)

Aswin has gone down the slightly more rigorous route, but you may be able to get a better approximation to x* by something heuristic like a voting system. Personally, I suspect that in this case Aswin's approach will be better, though the only way to find out which is better is to compare results.

Matt

[HaWe]

About a voting mechanism
Why do you want this? Is it to keep the number of calculations low? This could help. Or is it to make the results of the filter better? This will not help, the filter itself has a better way to weight the quality of all the inputs. Or is it to keep non guasian errors out? This is what my filter already does with compass readings it doesn't trust.

mostly because we do only have 2 inputs for the KF. As you wrote, having 3 inputs would make the vector calculation too complicated and too slow.

The voting mechanism has been already established (IIRC) for error redundand space travel computer architectures:
3 independend computers calculate results independently and compare the results. If 1 has a completely different result (because of e.g. particle radiation to the memory), it's dropped.
In our case it's quite similar: if 1 sensors has a completely different result (because of interferences of what kind ever), it's dropped.

If you take the dimensions for a carpet covering your living room, and you get
2 times 5,00 m x 3,50 m
and 1 time 3,00 m x 3,40 m
will you take the average of all 3? Surely not, because at the last one you surely had made a mistake.

So if two values differ only slightly but 1 significantly: the latter probably is a stray bullet.
Dropping the stray bullet and taking the mean of the others or take it as inputs for the KF) is the most likely and most clever approach.

Odometric heading is subject to accumulation of errors, so the filter will ignore this info altogether after a few seconds. Odometric turn rate is not subject to accumulation of errors. So this can be used. But how reliable is this information in comparison to the gyro? I have never run any test so I cannot be sure.

I have no information about comparing odometric with gyro data, but with compass data I got lots of it:
odometric data are estimated 100 times more reliable.
Using a compass addtionally, and trying to increase the odometric reliabiltity: this attempt fails. It's always worse than odometry alone.
Going ahead is almost 100% reliable (5 cm left/right deviation and 1cm distance difference when 5m going ahead), even if the ground changes from wood to carpet.
But when driving 4x90° in a square of 1m edges 4 times each, odometry gives about 20° deviation and 10-20cm distance to the start-stop-point in the end.
Using a compass for turning calculation, I had 40-50° error in heading at the end and missed the target point at the end of up to 50 cm radius.

So still odometry is the "Golden Standard" for navigation: a filter has to make it far better, not far worse.

[physics-matt]

The voting mechanism has been already established (IIRC) for error redundand space travel computer architectures:
3 independend computers calculate results independently and compare the results. If 1 has a completely different result (because of e.g. particle radiation to the memory), it's dropped.
In our case it's quite similar: if 1 sensors has a completely different result (because of interferences of what kind ever), it's dropped.

In that situation, a voting scheme is appropriate because each computer will be exactly right or wrong. This means, for instance, that if two of the computers obtain different values then we know at least one of them must have done something wrong.

That is different situation to what we have here: we have fundamentally different sensing mechanisms which will each output noisy values. If two sensors produce two different values it does not guarantee that anything went "wrong" in any way. They will also have different noise characteristics and so the data has to be combined in a more careful way (which is what the KF tries to do).

If you take the dimensions for a carpet covering your living room, and you get
2 times 5,00 m x 3,50 m
and 1 time 3,00 m x 3,40 m
will you take the average of all 3? Surely not, because at the last one you surely had made a mistake.

Ok, but as I've already said, that's not the situation we are likely to be faced with. Something more realistic would be:

1 times 5,00m x 3,50 m
1 times 4,90m x 3,60 m
1 times 4,40m x 3,55 m

What do you propose we do here? (hint: any answer that doesn't begin "it depends on the characteristics of the sensors" is likely to be wrong)

I have no information about comparing odometric with gyro data, but with compass data I got lots of it:
odometric data are estimated 100 times more reliable.

Over how much distance? Odometric inaccuracies will accumulate and without some other sensor there is no way of stopping it becoming less accurate over time than an absolute reference such as a compass.

Exactly what does "100 times more reliable" mean?

Matt

[HaWe]

doc-helmut wrote:If you take the dimensions for a carpet covering your living room, and you get
2 times 5,00 m x 3,50 m
and 1 time 3,00 m x 3,40 m
will you take the average of all 3? Surely not, because at the last one you surely had made a mistake.

Ok, but as I've already said, that's not the situation we are likely to be faced with. Something more realistic would be:

1 times 5,00m x 3,50 m
1 times 4,90m x 3,60 m
1 times 4,40m x 3,55 m

From my experience I am convinced that that's absolutely not the situation we are likely to be faced with. Make a couple of simultaneous data collections and you'll see.

Odometric inaccuracies will accumulate and without some other sensor there is no way of stopping it becoming less accurate over time than an absolute reference such as a compass.

never in your (or my) lifetime.

[gloomyandy]

Odometric inaccuracies will accumulate and without some other sensor there is no way of stopping it becoming less accurate over time than an absolute reference such as a compass.

never in your (or my) lifetime.

Clearly this is not true. Try making a robot rotate a few hundred times on the spot and see how far out the orientation information using odometry gets. With a typical Lego robot say the accuracy is as good as 0.5 degrees per rotation (which is very good in my experience), then after 40 rotations you will have an accumulated errors of 20 degrees or so it's rare of a compass sensor to be out by this much...

[HaWe]

make your own investigations and you'll see that I'm right.
I'm not talking about a "ideal theoretic compass model" but about the HT compass used indoors on a NXT (moving e.g. in a room or a maze for a couple of minutes uninterruptedly) .
The HT Compass is always worse than odometrics, as well as when going straight and also when moving in curves or angles.
One of the reasons may be that the compass reading errors are widly non-Gaussian (caused by magnetic and electromagnetic interferences from the vehicle, and from the environment, and from internal reading errors), and when using odometrics you have a very fine regulation method to get the wheels synchronized by encoder readings.
Having an outdoor robot driving through mud and water and ice and holes and over sand dunes things are considered to be different.

So don't speak from the point of a "bloody theoretican" without having data - but proof your theory by the reality (that's indeed natural science).