Deep understanding of Kalman filter （1）： Background knowledge

Statement ： The pictures and pictures of this article are from https://www.kalmanfilter.net/, If there is infringement, please contact to delete .

This article is from the official account of WeChat 【DeepDriving】 Arrangement , The full text is divided into 3 Part of it . Official account 【DeepDriving】, Background reply keyword 【 Kalman filter 】 The full text is available PDF.

Background knowledge

Before introducing the Kalman filter , Let's first learn some basic knowledge related to Mathematics .

Mean and expectation

mean value （Mean） And expectations （Expected Value） Are two similar but different concepts . If we have 2 gold 5 Penny and 3 gold 10 Cent coins , It's easy to calculate their mean ：

$$V_{mean}=\frac{1}{N}\sum _{n=1}^N{V}_n=\frac{1}{5}\left(5+5+10+10+10\right)=8branch$$

The above results cannot be called expected values , Because the state of the system is not implicit and we use all 5 Coins to calculate the average .

Now suppose a person tests continuously 5 Secondary weight , The results are as follows ：79.8 kg 、80 kg 、 80.1 kg 、79.8 kg 、80.2 kg , The random measurement error of the scale itself leads to different measurement values each time . We don't know what the real weight is , Because this is an implicit variable , But we can deal with 5 Calculate the average value of the measurement results of times to estimate a relatively accurate weight value ：

$$W=\frac{1}{N}\sum _{n=1}^N{W}_n=\frac{1}{5}\left(79.8+80+80.1+79.8+80.2\right)=79.98thousandg$$

The above average value can be called the expected value of implicit variable weight .
The average is usually written in Greek $\mu$ To express , The expected value is written in letters $E$ To express .

Variance and standard deviation

variance （Variance） Used to measure the dispersion of a set of data , That is, the deviation between the sample data and the mean ; Standard deviation （Standard Deviation） Is the square root of the variance , It's usually in Greek $\sigma$ To express , And the variance is expressed as ${\sigma }^2$.

Suppose there are two high school basketball team members whose heights are shown in the following table ：

	team member 1	team member 2	team member 3	team member 4	team member 5	Average
A team	1.89m	2.1m	1.75m	1.98m	1.85m	1.914m
B team	1.94m	1.9m	1.97m	1.89m	1.87m	1.914m

We want to compare the height data of the two basketball teams . First , From the above table, we can know that the average height of the two teams is the same . Further , We can compare their variances and standard deviations . use $x$ It means height ,$\mu$ Indicates the average height , According to the calculation formula of variance and standard deviation

$${\sigma }^2=\frac{1}{N}\sum _{n=1}^N{\left(x_n-\mu \right)}^2$$

$$\sigma =\sqrt[]{\frac{1}{N}\sum _{n=1}^N{\left(x_n-\mu \right)}^2}$$

Get A Variance of team height ${\sigma }_A^2=0.014m^2$, Standard deviation ${\sigma }_A=0.12m$;B Variance of team height ${\sigma }_B^2=0.0013m^2$, Standard deviation ${\sigma }_B=0.036m$. We can know from the variance of the height of the two teams ,A There is a greater difference in the height of team members .

Suppose we want to calculate the mean and variance of the height of all the players of all high school basketball teams , This will be a difficult task , Because we need to collect statistics from every team member in every school . However, we can collect a relatively large data set , Then the mean and variance of the height of all team members are estimated through this data set . such as , We can charge randomly 100 Height data of team members , This data set is enough to accurately estimate the mean and variance of the height of all team members . It should be noted that , At this time, the formula for calculating variance is slightly different from the above , Divisor is $N-1$ instead of $N$：

$${\sigma }^2=\frac{1}{N-1}\sum _{n=1}^N{\left(x_n-\mu \right)}^2$$

coefficient $N-1$ It is called Bessel correction （Bessel's correction）, For detailed mathematical proof, please refer to This article .

Normal distribution

Many natural phenomena follow normal distribution （Normal Distribution） Laws . Take the height of basketball players as an example , If we randomly select the height of the team members to build a large data set , And draw a graph of height value and its occurrence frequency , We will get a bell curve similar to the figure below ：

You can see that this curve is based on the mean 1.9m A symmetrical curve centered , And the number of occurrences of values near the mean value is much higher than that of remote values . The standard deviation of this set of data is 0.2m, As shown in the figure below , Yes 68.26% The value of is within one standard deviation from the mean （1.7m~2.1m）:

Normal distribution is also called Gaussian distribution , The formula is as follows ：

$$f\left(x;\mu ,{\sigma }^2\right)=\frac{1}{\sqrt[]{2\pi {\sigma }^2}}{e}^{\frac{-{\left(x-\mu \right)}^2}{2{\sigma }^2}}$$

The above curve is called the probability density function of normal distribution （Probability Density Function,PDF）.

The measurement error usually conforms to the normal distribution , Therefore, when designing Kalman filter, we will assume that the measurement error is normally distributed .

A random variable

If you use a speed gun to measure the speed of a moving vehicle , Then the measured value of the velocimeter gun is a random variable , The measurement results are normally distributed . Random variables can be continuous , It can also be discrete , All measurements are continuous random variables .

It is estimated that 、 Accuracy and accuracy

It is estimated that （Estimate） It is an estimation of the implicit state of the system . For example, the real position of the aircraft is an implicit state value for the observer , We can use radar and other sensors to measure and improve the accuracy of estimation through multi-sensor fusion and tracking algorithm . The measured or calculated parameters are estimated values .

Accuracy （Accuracy） Used to indicate the closeness between the measured value and the real value .

accuracy （Precision） Used to express the reproducibility of measurement results .

Estimation needs to consider the accuracy and accuracy of the system , The following figure illustrates the relationship between accuracy and precision ：

The variance of the measured value of the high-precision system is small （ Low uncertainty ）, conversely , The variance of the measured value of the low accuracy system is large （ High uncertainty ）, Variance is caused by random measurement error .

Systems with low accuracy are called biased systems , Because there will always be an internal systematic error in its measured value （ deviation ）.

Averaging or smoothing the measured values can significantly reduce the influence of variance . such as , If we use a thermometer with random measurement error to measure temperature , Measurement error will cause the measured value to be higher or lower than the true value . We can make multiple measurements and average them , This estimate will be close to the real value , The more measurements , The closer the estimated value is to the real value . But if the thermometer itself has deviation , Then the estimated value will have a fixed systematic error .

The following figure describes the measured values from a statistical point of view ：

• The measured value is a random variable described by the probability density function ;
• The mean value of the measured value is the expected value of the random variable ;
• The deviation between the mean value of the measured value and the true value is called deviation or systematic measurement error , Used to indicate the accuracy of measurement ;
• The degree of dispersion of the measured value distribution is the accuracy of the measured value , Also known as measurement noise （measurement noise）、 Random measurement error （random measurement error） Or measurement uncertainty （measurement uncertainty）.

Covariance and covariance matrix

Covariance is used to measure two random variables $x$ and $y$ The degree of joint change , It represents the overall error of two variables , This is different from variance , Variance represents only one variable error . Variance can be regarded as a special case of covariance , That is, the two variables are the same . If the trends of the two variables are the same , That is, if one of them is greater than its own expectations , The other is also greater than one's own expectations , Then the covariance between the two variables is positive . If the change trend of two variables is opposite , That is, one of them is greater than its own expectation , And the other is less than its own expectations , Then the covariance between the two variables is negative . If $x$ And $y$ It's statistically independent , So the covariance between them is going to be 0. A random variable $x$ And $y$ The covariance of is calculated as follows ：

$$\sigma (x,y)=\frac{1}{N-1}\sum _{n=1}^N\left(x_n-{\mu }_x\right)\left(y_n-{\mu }_y\right)$$

among ,${\mu }_x$ and ${\mu }_y$ They are random variables $x$ And $y$ Average value .

For a sample containing $k$ Vectors of elements $\mathbf{x}$

$${\left[\begin{array}{ccccc}{x}_1& x_2& x_3& \dots & x_k\end{array}\right]}^T$$

The covariance matrix is

$$\begin{gathered} COV(\mathbf{x})=E\left(\left[\begin{array}{cccc}(x_1-{\mu }_{x_1}{)}^2& (x_1-{\mu }_{x_1})(x_2-{\mu }_{x_2})& \cdots & (x_1-{\mu }_{x_1})(x_k-{\mu }_{x_k})\\ (x_2-{\mu }_{x_2})(x_1-{\mu }_{x_1})& (x_2-{\mu }_{x_2}{)}^2& \cdots & (x_2-{\mu }_{x_2})(x_k-{\mu }_{x_k})\\ ⋮& ⋮& \ddots & ⋮\\ (x_k-{\mu }_{x_k})(x_1-{\mu }_{x_1})& (x_k-{\mu }_{x_k})(x_2-{\mu }_{x_2})& \cdots & (x_k-{\mu }_{x_k}{)}^2\end{array}\right]\right) \\ =E\left(\left[\begin{array}{c}(x_1-{\mu }_{x_1})\\ (x_2-{\mu }_{x_2})\\ ⋮\\ (x_k-{\mu }_{x_k})\end{array}\right]\left[\begin{array}{cccc}(x_1-{\mu }_{x_1})& (x_2-{\mu }_{x_2})& \cdots & (x_k-{\mu }_{x_k})\end{array}\right]\right) \\ =E\left(\left(\mathbf{x}-{\mathbf{\mu }}_{\mathbf{x}}\right){\left(\mathbf{x}-{\mathbf{\mu }}_{\mathbf{x}}\right)}^T\right) \end{gathered}$$

Basic expectation operation rules

A random variable $X$ The expectations of the $E(X)$ Equal to its average ：

$$E(X)={\mu }_X$$

Some basic expectation operation rules are as follows ：

The rules	remarks
$E(X)={\mu }_X=\sum xp(x)$	$p(x)$ yes $x$ Probability
$E(a)=a$	$a$ Constant
$E(aX)=aE(X)$	$a$ Constant
$E(a\pm X)=a\pm E(X)$	$a$ Constant
$E(a\pm bX)=a\pm bE(X)$	$a,b$ Constant
$E(X\pm Y)=E(X)\pm E(Y)$	$Y$ For another random variable
$E(XY)=E(X)E(Y)$	If $X$ and $Y$ Are independent of each other

The random variable $X$ and $Y$ The variances of are recorded as $V(X)$ and $V(Y)$, Their covariance is recorded as $COV(X,Y)$, Here are some basic operation rules ：

The rules	remarks
$V(a)=0$	$a$ Constant
$V(a\pm X)=V(X)$	$a$ Constant
$V(X)=E(X^2)-{\mu }_X^2$
$COV(X,Y)=E(XY)-{\mu }_X{\mu }_Y$
$COV(X,Y)=0$	If $X$ and $Y$ Are independent of each other
$V(aX)=a^{2}V(X)$	$a$ Constant
$V(X\pm Y)=V(X)+V(Y)\pm 2COV(X,Y)$
$V(XY)\ne V(X)V(Y)$

The following is the proof of several formulas ：
(1).
$$\begin{gathered} V(X)=E((X-{\mu }_X{)}^2) \\ =E(X^2-2X{\mu }_X+{\mu }_X^2) \\ =E(X^2)-E(2X{\mu }_X)+E({\mu }_X^2) \\ =E(X^2)-2{\mu }_{X}E(X)+{\mu }_X^2 \\ =E(X^2)-2{\mu }_X{\mu }_X+{\mu }_X^2 \\ =E(X^2)-{\mu }_X^2 \end{gathered}$$

(2).
$$\begin{gathered} COV(X)=E((X-{\mu }_X)(Y-{\mu }_Y)) \\ =E(XY-X{\mu }_Y-Y{\mu }_X+{\mu }_X{\mu }_Y) \\ =E(XY)-E(X{\mu }_Y)-E(Y{\mu }_X)+E({\mu }_X{\mu }_Y) \\ =E(XY)-{\mu }_{Y}E(X)-{\mu }_{X}E(Y)+E({\mu }_X{\mu }_Y) \\ =E(XY)-{\mu }_Y{\mu }_X-{\mu }_X{\mu }_Y+{\mu }_X{\mu }_Y \\ =E(XY)-{\mu }_X{\mu }_Y \end{gathered}$$

(3).
$$\begin{gathered} V(aX)=E((aX{)}^2)-(a{\mu }_X{)}^2 \\ =E(a^2{X}^2)-a^2{\mu }_X^2 \\ =a^{2}E(X^2)-a^2{\mu }_X^2 \\ =a^2(E(X^2)-{\mu }_X^2) \\ =a^{2}V(X) \end{gathered}$$

(4).
$$\begin{gathered} V(X\pm Y)=E((X\pm Y{)}^2)-({\mu }_X\pm {\mu }_Y{)}^2 \\ =E(X^2\pm 2XY+Y^2)-({\mu }_X^2\pm 2{\mu }_X{\mu }_Y+{\mu }_y^2) \\ =E(X^2)-{\mu }_X^2+E(Y^2)-{\mu }_Y^2\pm 2(E(XY)-{\mu }_X{\mu }_Y) \\ =V(X)+V(Y)\pm 2(E(XY)-{\mu }_X{\mu }_Y) \\ =V(X)+V(Y)\pm 2COV(X,Y) \end{gathered}$$

Differential of matrix product trace

Here we will prove two formulas .

(1).
$$\frac{d}{d\mathbf{A}}\left(tr\left(\mathbf{A}\mathbf{B}\right)\right)={\mathbf{B}}^T$$

prove ：

Given two matrices $\mathbf{A}$($m\times n$) and $\mathbf{B}$($n\times m$), The product of them is

$$\mathbf{A}\mathbf{B}=\left[\begin{array}{ccc}{a}_{11}& \cdots & a_{1n}\\ ⋮& \ddots & ⋮\\ a_{m1}& \cdots & a_{mn}\end{array}\right]\left[\begin{array}{ccc}{b}_{11}& \cdots & b_{1m}\\ ⋮& \ddots & ⋮\\ b_{n1}& \cdots & b_{nm}\end{array}\right]=\left[\begin{array}{ccc}{\sum }_{i=1}^n{a}_{1i}{b}_{i1}& \cdots & {\sum }_{i=1}^n{a}_{1i}{b}_{im}\\ ⋮& \ddots & ⋮\\ {\sum }_{i=1}^n{a}_{mi}{b}_{i1}& \cdots & {\sum }_{i=1}^n{a}_{mi}{b}_{im}\end{array}\right]$$

matrix $\mathbf{A}\mathbf{B}$ The trace of $tr(\mathbf{A}\mathbf{B})$ Is the sum of its main diagonal elements ：

$$tr(\mathbf{A}\mathbf{B})=\sum _{i=1}^n{a}_{1i}{b}_{i1}+\cdots +\sum _{i=1}^n{a}_{mi}{b}_{im}=\sum _{i=1}^n\sum _{j=1}^m{a}_{ji}{b}_{ij}$$

Contralateral trace $tr(\mathbf{A}\mathbf{B})$ Find differentiation

$$\begin{gathered} \frac{\partial tr(\mathbf{A}\mathbf{B})}{\partial \mathbf{A}}=\left[\begin{array}{ccc}\frac{\partial ({\sum }_{i=1}^n{\sum }_{j=1}^m{a}_{ji}{b}_{ij})}{\partial a_{11}}& \cdots & \frac{\partial ({\sum }_{i=1}^n{\sum }_{j=1}^m{a}_{ji}{b}_{ij})}{\partial a_{1n}}\\ ⋮& \ddots & ⋮\\ \frac{\partial ({\sum }_{i=1}^n{\sum }_{j=1}^m{a}_{ji}{b}_{ij})}{\partial a_{m1}}& \cdots & \frac{\partial ({\sum }_{i=1}^n{\sum }_{j=1}^m{a}_{ji}{b}_{ij})}{\partial a_{mn}}\end{array}\right] \\ =\left[\begin{array}{ccc}{b}_{11}& \cdots & b_{n1}\\ ⋮& \ddots & ⋮\\ b_{1m}& \cdots & b_{nm}\end{array}\right] \\ ={\mathbf{B}}^T \end{gathered}$$

(2).

$$\frac{d}{d\mathbf{A}}\left(tr\left(\mathbf{A}\mathbf{B}{\mathbf{A}}^{\mathbf{T}}\right)\right)=2\mathbf{A}\mathbf{B}$$

among $\mathbf{B}$ Is symmetric matrix .

prove ：

Given two matrices $\mathbf{A}$($m\times n$) and $\mathbf{B}$($n\times m$),

$$\begin{gathered} {\mathbf{A}\mathbf{B}\mathbf{A}}^T=\left[\begin{array}{ccc}{a}_{11}& \cdots & a_{1n}\\ ⋮& \ddots & ⋮\\ a_{m1}& \cdots & a_{mn}\end{array}\right]\left[\begin{array}{ccc}{b}_{11}& \cdots & b_{1n}\\ ⋮& \ddots & ⋮\\ b_{n1}& \cdots & b_{nn}\end{array}\right]\left[\begin{array}{ccc}{a}_{11}& \cdots & a_{m1}\\ ⋮& \ddots & ⋮\\ a_{1n}& \cdots & a_{mn}\end{array}\right] \\ =\left(\left[\begin{array}{ccc}{\sum }_{i=1}^n{a}_{1i}{b}_{i1}& \cdots & {\sum }_{i=1}^n{a}_{1i}{b}_{in}\\ ⋮& \ddots & ⋮\\ {\sum }_{i=1}^n{a}_{mi}{b}_{i1}& \cdots & {\sum }_{i=1}^n{a}_{mi}{b}_{in}\end{array}\right]\right)\left[\begin{array}{ccc}{a}_{11}& \cdots & a_{m1}\\ ⋮& \ddots & ⋮\\ a_{1n}& \cdots & a_{mn}\end{array}\right] \\ =\left[\begin{array}{ccc}{\sum }_{j=1}^n{\sum }_{i=1}^n{a}_{1i}{b}_{ij}{a}_{1j}& \cdots & {\sum }_{j=1}^n{\sum }_{i=1}^n{a}_{1i}{b}_{ij}{a}_{mj}\\ ⋮& \ddots & ⋮\\ {\sum }_{j=1}^n{\sum }_{i=1}^n{a}_{mi}{b}_{ij}{a}_{1j}& \cdots & {\sum }_{j=1}^n{\sum }_{i=1}^n{a}_{mi}{b}_{ij}{a}_{mj}\end{array}\right] \end{gathered}$$

matrix ${\mathbf{A}\mathbf{B}\mathbf{A}}^T$ The trace of $tr({\mathbf{A}\mathbf{B}\mathbf{A}}^T)$ Is the sum of its main diagonal elements ：

$$tr({\mathbf{A}\mathbf{B}\mathbf{A}}^T)=\sum _{j=1}^n\sum _{i=1}^n{a}_{1i}{b}_{ij}{a}_{1j}+\cdots +\sum _{j=1}^n\sum _{i=1}^n{a}_{mi}{b}_{ij}{a}_{mj}=\sum _{k=1}^m\sum _{j=1}^n\sum _{i=1}^n{a}_{ki}{b}_{ij}{a}_{kj}$$

Contralateral trace $tr({\mathbf{A}\mathbf{B}\mathbf{A}}^T)$ Find differentiation

Because the matrix $\mathbf{B}$ It's a symmetric matrix , therefore $\mathbf{B}={\mathbf{B}}^{\mathbf{T}}$, Available

$$\frac{\partial tr({\mathbf{A}\mathbf{B}\mathbf{A}}^T)}{\partial \mathbf{A}}={\mathbf{A}\mathbf{B}}^T+\mathbf{A}\mathbf{B}=\mathbf{A}\mathbf{B}+\mathbf{A}\mathbf{B}=2\mathbf{A}\mathbf{B}$$