 # How do I understand standard deviation in the poseestimator class?

Hello! This past month I became very interested in the PoseEstimator class (specifically DifferentialDrivePoseEstimator) to find an accurate field position using both vision and the gyro. However, I don’t know how to find a fitting standard deviation for each of the standard deviation parameters in the class and I also don’t really understand the Matrix class’s use as a parameter in the class’s constructor as well. It would be really helpful if someone could help me out with this!

1 Like

Do you know what “standard deviation” means, as a starting point?

I did, but I guess I’m still having trouble understanding how a standard deviation works. All I really know is that it’s the square root of the variance, and the variance has something to do with averages. Are there any useful resources that I could use to understand standard deviation better?

I just know some very scattered information about it, but I guess I don’t really understand it. I think maybe once I understand the concept better maybe this will be a bit easier?

The variance is the measure of the “spread” of a distribution. The standard deviation is also a measure of the “spread” of a distribution; the two are the different ways of looking at the same thing.

A distribution with a wider spread (e.g. a measurement with more noise) will have a bigger variance and standard deviation.

1 Like

That makes sense, thank you! But I’m still not really sure on how to find standard deviations for my state, local, and global measurements. I guess I don’t really understand what noise would look like for vision, the gyro, and the encoders. How would I know what “noise” would be and how could I use that to find the standard deviataion?

The WPILib system identification toolsuite can give you some rough estimates (you might need some care in interpreting them if the test scenario does not match the loading scenario in practice).

Learning and understanding the system equations that SysId is based on is very important - without a working understanding of that, none of this will be easy to use.

Ok, I guess I’ll do some research on SysID. Thank you for your help!

1 Like

Basically the standard deviation you supply for a vision update or for the odometry are an indicator of how much you trust the accuracy of that calculated position from either the odometry or the measurement system.

Lets say you drive the robot from a datum point of x=0,y=0,Theta=0 to a position of x=3.0m, y=3.0m and theta=90 degrees a hundred times. Every time it does this trip it will end up in a slightly different position. As you are doing this drive the odometry class is continually calculating your position (x,y,theta) as it drives along. Because of a bunch of factors like backlash, sampling delay/errors, carpet scrub etc etc it will never be in exactly the same position. If at the end of each of these drives you accurately measured the x distance, y distance and angle of the robot relative to your known starting location, you would observe a spread of errors in each one of these dimensions. If you plotted these errors you would notice that they form a gaussian (Bell Curve). There is a formula for calculating the standard deviation value from a set of samples. Google is your friend.
In layman’s terms 68% of the samples will fall in middle area of the bell curve out to one standard deviation from the average value, 95% will fall within 2 standard deviations and 99.7% will fall within 3 standard deviations. So if the SD are small this curve is very narrow and pointy, if the SD’s are large this curve is wider and flatter.
You have to supply the vision update with a set of SD values for each dimension. This is like a measure of how well you trust the vision measurement. You could also do a similar test with the vision system. ie Move the robot to a known position, let your vision system calculate its position. Do this 100 times and again 68% of the measured errors will fall between ±1 SD, 95% within ±2SD, etc etc. The reason all this stuff if here is because it is used be the pose estimator to help localise the robot and reduce its uncertainty.
By using this probabilistic approach you increase the robustness of your localisation system. A good book on this is Probabilistic Robotics. Pretty heavy reading but well worth it if you are interested in this field. This book explains many different localisation algorithms and also deals with measurement uncertainty that doesn’t fit into a bell curve.
Word of advice, If you are going to heavily utilise this odometry and pose estimation stuff in your robot code I’d have an option/command/button on your robot to force a reset of the odometry to a known position. I saw a top team on the weekend that somehow managed to get itself 90 degrees out to the field and their brilliant turret shooter that had auto targeted the goal continuously all competition all of sudden would not lock onto the target and they could not shoot all match.
Hope this helps.
Regards
Warren

8 Likes

So in order to arrive at good standard deviations for the input of the PoseEstimators you need to do multiple sample runs and determine error of each pose input (vision, wheel encoders…) Is that correct?

I am not sure how SysID factors into that.

SysId reports residuals from the model fit.

I am feeling a bit slow today I guess. If the standard deviation we are calculating is how different are the reported positions from reality how do residuals from sysid factor into that since sysid doesn’t know anything about actual position just odometry?

1 Like

It doesn’t; but encoder measurements are still quite accurate and can give you insight into the amount of system noise. You can calculate a standard deviation for the encoder measurements themselves from the encoder resolution (assuming no slip).

1 Like

Another less mathematically rigorous but faster method is to just pick some ballpark numbers and tune based on how the filter performs.

1 Like