1. Kalman filter - the best linear filter

• State shift

Through the physical formula, we can know the position of the car in the next state 、 Speed , But in practice , Noise has an effect on the result ,x^ Is the value under the influence of noise , Noise is usually represented by covariance matrix

• Covariance matrix

A one-dimensional

In one dimension , We use data center values and variances to represent noise , Gaussian distribution

A two-dimensional

In two dimensions , If there is no correlation between the two noises, it can be shown in Figure 1 , When there is a positive or negative correlation between noises , Then use figure 2 、 Figure 3 shows , In Figure 2 、 Third middle school , We use variance 、 Covariance represents the relationship between noises , among 11 22 Variance ,12 12 It means covariance

• Transfer of noise covariance matrix

There are also errors in the transfer process of covariance matrix , therefore Q Represents the noise caused by the prediction model itself ,p It shows the transmission relationship of uncertainty at all times

• Observation matrix
  
  The observation matrix is also noisy , use V Express

• Status update
  
  Xt = Ft * Xt-1 + Bt * Ut
  
  ad locum ,Wk Is the prediction error ,Vk For measurement error
  
  
  X^k-1 Is the estimated value of the last time ,X^{k Is the estimated value of the current moment , because X}k-1 Estimation error , Then based on X^{k-1 Of X}k Greater error .
  Yk Is the current measured value , There are also errors , In order to get a more accurate position , send X^k * Yk Get a new estimate
  
  
  
  

• to update
  

• Example