Kaformer personal notes

Many advantages ： Implement a simple , Is a pure time domain filter, Frequency domain transformation is not required , So it has many applications in engineering .

Take a classic example :

        There is a car in motion , We use position and velocity to express its current state , In matrix form, it is a two-dimensional column vector , The two elements in this column vector are its states , Location And speed , In addition, the driver can step on the accelerator or brake , There is a forward or backward acceleration , Represents a control quantity for the vehicle , If the driver doesn't step on the accelerator or brake , Is equal to 0, The car will move in a straight line at a constant speed .

        If we know the state of the last moment , So the current state What can expression be , As shown in the figure

         We observe these two formulas and find that Output variables are only input variables A linear combination , This is why we say that the Kalman filter is The best linear filter , Because it It can only describe the linear relationship between States , Since it is a linear relationship , We can write it as Matrix form , We further extract the two state transformation matrices , Then the formula can be simplified to the formula shown in the lower right corner .

         This formula is the first formula of Kalman filter ( State prediction formula ), among It's called State shift matrix , It shows how we Infer the status of the current time from the status of the previous time , among be called Control matrix , It represents the control quantity How to act on the current state （ Look at the formula in the upper left corner to understand it well ）

 

here . Said to Of An estimate , instead of True value of , Because we can't know the real state of the car , We can only estimate as much as possible by observation Value . There's another. Superscript of minus sign , Indicates that the value is It is inferred from the state of the last moment Of , We'll have to To correct by observation Value , Only after correction can it be regarded as the best estimate . That is, there is no minus superscript Of , With the state prediction formula , We can infer the state of the current moment , But we know all Of It is speculated that the noise is included , The greater the noise, the greater the uncertainty , How can we express how much uncertainty speculation brings , In this case, we will use the covariance matrix to express , So what is the covariance matrix ？ Let's explain it from one-dimensional simple case ···

        Suppose we have a one-dimensional data with noise , Each measurement is different , But they are all around a central value , So the easiest way to show their distribution is to write it down Central value and variance , This is actually assuming that the data is a Gaussian distribution

        Let's look at the two-dimensional situation , The two-dimensional data are projected on two coordinate axes respectively , Gaussian distribution on both axes ( l ), When we express its distribution , Just write down the center value and variance of the two Gaussian distributions respectively . If two noises are independent , It can be expressed in this way , But when the two dimensions are related , For example, when the noise in one dimension increases , Another dimension of noise also increases ( In the figure ), For example, when the noise in one dimension increases , Another dimension of noise reduction ( r ), At this time, there is no difference between the projection of the two coordinate axes and the left of the figure , It's still a Gaussian distribution , So for Indicates the correlation between these two dimensions , In addition to recording the variance of the two dimensions , There is also a covariance to indicate the degree of correlation between the two dimensions .

        Put two dimensions （ Location , Speed ） The covariance of is written in the form of a matrix , The two values of the diagonal are the variances of the two dimensions , The two values on the opposite diagonal are equal , It's this pile Covariance of two-dimensional data . In all three cases , covariance =0（ Independent ）,>0（ positive correlation ）,<0（ negative correlation ）.

a key ： In Kalman filter, covariance matrix is used to describe all uncertainties

         In the case of our car , The state uncertainty at each moment is determined by the covariance matrix P To express Of , So the next question is , How do we make this Uncertainty is passed from moment to moment Well ？ The answer is to multiply by the state transition matrix F, This time it's going to be multiplied on the left and right sides

         The specific point is like this , from The covariance of the current time is inferred from the previous time , It is equal to multiplying both sides of the covariance at the previous time by the state transition matrix , As for why we should multiply by two sides , This is a property of covariance matrix ,

         x The covariance of is P, Want to calculate FX The covariance , You can put the F Mention both sides from the inside , At this time, we have one more thing to consider , Our prediction model is not 100% accurate , It also contains noise ,

         So we're going to add a Covariance matrix Q To represent the noise caused by the prediction model itself ,

         This formula represents the second formula of Kalman filter , it It represents the transmission relationship of uncertainty at various times .

         Back to our model car , Suppose we put... At one end of the road Laser range finder , The position of the car can be observed at every moment , The observed values are recorded as Zt, So from the state of the car itself Xt To the observation state Zt There is a transformation relationship between , We remember it as H, Of course, this relationship is also It can only be linear , Because the Kalman filter is a linear filter , So we take it for granted that H In matrix form ,X And Z The dimensions of are not necessarily the same , In our case ,X Is a two-dimensional column vector ,Z Just a scalar value ,H It should be a matrix with one row and two columns , The elements inside are 1 and 0, such H and X When you multiply, you get a scalar value Z, and Z That is, the location of the car , It and X The first element is equal , Then why add a lowercase v Well , Because we The observations are not 100% reliable , So we need to add a v To express Observation noise , And this Covariance matrix of noise , We use it R To express , Because in our case , The observed value is a one-dimensional value , So this R Is not a matrix, but a single value , It just means Z The variance of , Suppose we have a laser rangefinder , There are other measurement methods that can observe a certain feature of a car , that Z It becomes a multidimensional column vector , It will contain the measured values of each measurement method , And every measured value is only an incomplete representation of the real state , We can infer the true state from several incomplete statements , and The function of Kalman filter data fusion , It is reflected in this measurement matrix ,

         We already have Observation and measurement Z And it's Noise covariance matrix R, So how do we integrate them into our state X My estimate , We've already got the minus sign before , Now we just need to add a term after it to correct its value （ The following formula 1）, We can get our best estimate , So what is the added item ？ Look at the brackets first What's inside , this Represents the residual between the actual observation and our expected observation , This residual is multiplied by a coefficient Kt, Can be used to correct The value of the , This Kt A critical , be called Kalman coefficient , In fact, it is also a matrix , Its formula is like this （ Formula 2 in the figure below ）, The derivation of this formula is complicated , So we just make a qualitative analysis , This Kalman coefficient K The function mainly has two aspects , One is to weigh the predicted state covariance P And the covariance matrix of observations R Size , To decide whether we believe in more predictive models or more observational models , If you believe in more prediction models , The weight of this residual will be smaller , vice versa . Second, the expression form of residual error from Transition from observation field to state field , What does that mean ？

         We just talked about the observed values Z Is a one-dimensional vector , state X Is a two-dimensional column vector , The units they use and even the characteristics they describe may be different , Then how can we use Observation value ( Only location information ) Residual of Go to Update status values ( Position and speed ) Well ？ In fact, this Kalman coefficient K It is doing such a transformation for us , In the case of this car , We only observe the position of the car , but K It already contains the covariance matrix P Information about , So it takes advantage of the correlation between the two dimensions of position and velocity , from From the position residuals, the velocity residuals are calculated , thus Let's talk about the State X Two dimensions of meanwhile Amendment , Okay , Now it's just one last step away , The last step is to update the noise distribution of the best estimate , This value is reserved for the next iteration , In this step , The state uncertainty is reduced , In the next iteration , Due to the introduction of transmission noise , Uncertainty will increase again , Kalman filter is to seek a balance in this uncertainty .

         Up to now , There is already a Kalman filter Five formulas , Make a complete list of them , In these five formulas , The first two It's through The state of the previous time is used to predict the state of the current time , What we get from these two formulas is a superscript with a minus sign X and P, This means that this is not the best estimate , Superscripts of the minus sign indicate that they still owe something , The missing thing is the information from the observations , Because we The observations at the current time have not been taken into account , The last three formulas are Update with current observations X and P, The updated value is Best observations 了 .

use Matlab Examples of implementations

 

 

1. You need to incorporate variables into nn.Parameter
2. have a look Karman-attention 2020 nuerips spotlight

Reference material

【 Don't look at the old tutorial of Kalman filter 】2022 A great sacrifice , Kalman filtering - Target tracking from theory to practice, the latest version of a full set of tutorials ！ Recommended collection _ Bili, Bili _bilibili // Most of the content is extracted from the video