Using (2) we will have:
now using (5),
Now by using (4):
Now by using (6):
since has no dependency on
, we can omit it:
We can reformulate it as:
For multidimensional data
New mean:
New Covariance:
Fusing and knocking out will give us new gain:
which give us corrected mean:
and new covariance:
For instance for a 2D robot with state and twist command
:
First order Linearization:
Linearization of prediction function:
For example for a 2D mobile robot with laser scanner:
Linearization of update function:
Finding the appropriate values for Q (process noise covariance) and R (measurement noise covariance)
Finding the appropriate values for ( Q ) (process noise covariance) and ( R ) (measurement noise covariance) is crucial for the performance of the Kalman filter. These matrices represent the uncertainties in your model and measurements, respectively.
Here are some methods and guidelines to determine ( Q ) and ( R ):
-
Physical Insights or Expert Knowledge:
- Sometimes, based on the physical understanding of the system, one can approximate the uncertainties in the process and measurements.
-
Statistical Methods:
- If you have access to some ground truth data, you can use it to estimate ( R ). For instance, by comparing your measurements to the ground truth, you can compute the variance or covariance of the measurement error.
- For ( Q ), if you can measure or estimate the "real" state of the system at multiple time points, you can use these data points to estimate the state transition error, and thus ( Q ).
-
Tuning:
- In the absence of clear physical insight or ample ground truth data, a common approach is to tune the values of ( Q ) and ( R ) empirically to get optimal performance.
- Start with an initial guess and adjust based on performance. If your estimate is too noisy, you might need to increase ( R ) or decrease ( Q ). If your estimate is lagging behind the real value, you might need to decrease ( R ) or increase ( Q ).
- This method can be iterative and sometimes time-consuming, but it is often effective.
-
Process Model:
- If you have a physical or mathematical model of the system, you can use it to estimate ( Q ). For example, if you have a motion model and know the variance of acceleration disturbances, you can use that to estimate ( Q ).
-
Maximum Likelihood Estimation:
- If you have a good dataset, you can set up a likelihood function based on the Kalman filter equations and then use optimization techniques to find the values of ( Q ) and ( R ) that maximize this likelihood.
-
Trial and Error:
- Sometimes, simply trying a range of values and evaluating the performance for each can help. This method is brute force and can be computationally intensive but might be effective in some cases.
-
Use Adaptive Filters:
- Adaptive Kalman filters adjust ( Q ) and ( R ) on the fly based on the observed innovations (the difference between estimated measurements and actual measurements). This way, the filter can self-tune to some extent.
Remember that the Kalman filter assumes that the process noise and the measurement noise are Gaussian and white. Ensure that your ( Q ) and ( R ) reflect this assumption, or the filter's performance might be suboptimal.
Lastly, tuning the Kalman filter, especially determining ( Q ) and ( R ), is often as much an art as it is a science. Practical experience, combined with understanding the underlying system, can make the process more intuitive over time.
So at the beginning we have only robot pose (robot start in its own frame) and orientation and zero land mark, as the robot explor we will add landmarks and increase the state space, but here we assume that all landmark are known beforehand, so for a map with landmarks:
dimensional Gaussian
Compact Representation:
Refs: 1
Refs: 1
Refs: 1
Ref: 1