I assume you already heard of Kalman Filter. In general, you are controlling a moving vehicle and would like to know where it is at the next moment.
You made your guess of the vehicle's next moment based on your current speed, and at the next moment you also measured the vehicle's next location. Neither of them were accurate. Thus, both of them were in Normal distributions, with some room for errors / noise. The best estimation is to take a product of the two Normal distributions.
Here is how to take the product of two Normal distributions, based on this video (voiced in Chinese):
Taking a product of two Gaussian Distribution X ~ ( µ1, δ1) , Y, ~ ( µ2, δ2), the result Gaussian Distribution is:
k = δ1 / (δ1 + δ2)
µ = µ1 + k * ( µ2 - µ1 )
δ ** 2 = δ1** 2 + k * δ2 ** 2
How do I guess my next location? Use middle school math: knowing the current location p and speed v, we could guess the location of the vehicle in the next moment to be:
pk = pk-1 + vk-1 * t
vk = vk-1
(Since there is no external force, the next speed u is constant.)
Let state X = (p, v) be the current position and speed of the vehicle, rewrite the two formulas into a matrix form. That is:
pk = 1* pk-1 + t * vk-1
vk = 0* pk-1 + 1 * vk-1
Then write (pk, vk) as Xk and ((pk-1, vk-1) as Xk-1 and rewrite the expressions above as matrix multiplication
Xk = [1 , t ] * Xk-1
[0, 1]
Call the matrix as F in front of Xk-1 . The formula becomes Xk = F * Xk-1 . We will use the matrix F later.
If there is an external acceleration a, then next p needs to add a * t**2 / 2. and next u needs to add a * t, middle school math again. See this part of the video . The external acceleration is your control. It is how much force to apply to make the vehicle faster.
Xk = [1 , t ] * Xk-1 + [ t**2 / 2 ] * a
[0, 1] + [ t ]
(Note: some articles use a different variable name for acceleration a as u. )
Let P be the covariance matrix of X along p and u axis. P represents the noise / errors in the prediction. It is a 2x2 matrix. This covariance matrix defines the oval shape and rotation of the Gaussian distribution in the (p, v) space.
Given the covariance matrix P at the k-1 moment, we'd like to calculate the covariance matrix at the next moment k. So given the covariance cov(Xk-1) = Pk-1 , we'd like to find out cov(Xk), which is
cov(Xk) = cov(F * Xk-1)
Co-variance function has the following property: cov(A*X ) = A * cov(X) * AT . Use this property, thus:
cov(Xk) = cov(F * Xk-1) = F * cov(Xk-1) * FT
So, given the covariance matrix at the moment k-1, we can calculate the covariance matrix at moment k. (So, the Gaussian distribution of the next moment can be calculated based on Gaussian distribution of the current moment.)
How do I know my measurement?
In your measurement device, create a few samples and compare them with known distances. Calculate the standard deviation in the data collected from the measurement device.
Take the product
Now we have two Normal distributions of the next position: one by the linear equation, and another by measurement device. Use the equation µ = µ1 + k * ( µ2 - µ1 ) to take a product of two Normal distributions, as shown in the first section. Since X is in (p, u) space. So the equation should be in vector form, instead of in scalar form. The value of k and mean value are calculated the same, exception k is now calculated based on covariance. Covariances are combined in this way:
K' = ∑1 / ( ∑1 + ∑2 )
∑' = ∑1 + K' * ∑1
The result looks crazy after plugging in the guessed values based on matrix F, but really they are just the result of the product of two Gaussian distributions.
After Xk is calculated, it will be used as the position to predict in the next round (at k+1 moment). And the calculated ∑' will be used as the covariance matrix for the next round.
Reference:
- https://youtu.be/2-lu3GNbXM8?si=4Xbeiq-LBgTMxbGh&t=750 (voiced in Chinese)
- https://www.youtube.com/watch?v=KD0cH4fTFFU (voiced in Chinese)
