State Estimation Study Notes in Robotics (1) Chapter 2 Basics of Probability Theory

The so-called state estimation problem is essentially a problem of re-estimating the internal state of the system based on the prior model and observation data of the system.

2.1 Probability Density Function

2.1.1 Definition

Define x as a random variable on the interval [a, b] , subject to a certain probability density function $p (x)$ , then the function $p (x)$ must satisfy:
$\int_{a}^{b} p(x)dx=1$ $The integral of the p (x)$ function is 1 in order to satisfythe axiom of total probability.
For conditional probability, suppose p(x|y) represents the independent variable x $\in$ [a,b] in condition y $\in$ probability function under [r,s], then it satisfies:
$(\forall y)\int_{a}^{b} p(x|y)dx =1$ The joint probability density function of N-dimensional continuous random variables can also represent $p (x)$ , where x=(x₁,…,x_N). For each x_i, satisfy
x_i $\in$ [a_i,b_i], then, in fact, $p(x_{1} ,x_{2} ,...,x_{N } )$ instead of $p (x)$ .
You can also use $p (x, y)$ to represent the joint density of x, y.

2.1.2 Bayesian formula and inference

A joint probability density can be decomposed into the product of a conditional probability density and an unconditional probability density:
$p (x, y) = p (x ∣ y) p (y) = p (y ∣ x) p (x)$ Bayesian formula can be obtained from the above formula:
$p(x|y)=\frac {p(y|x)p(x)}{p(y)}$ 由于 $p(y)=p(y)\underset{1}{\underbrace{\int p(x|y)dx} } =\int p(x|y)p(y)dx=\int p(x,y)dx=\int p(y|x)p(x)dx$ 所以 $p(x|y)=\frac{p(y|x)p(x)}{p(y)}= \frac{p(y|x)p(x)}{\int p(y|x)p(x)dx}$ Therefore, if the prior probability density function $p (x)$ and sensor model $p (y ∣ x)$ , the posterior probability density function $p (x ∣ y)$ 。

2.1.3 Moments

The 0th moment of the probability density is the probability of the entire event, which is always equal to 1.
The probability first-order moment is called expectation, with $μ$ expression:
$\mu =E[x]=\int xp(x)dx$ For the general matrix function F(x), it is:
$E[F(x)]=\int F(x )p(x)dx$ expansion form is:
$E[F(x)]=E[f_{ij} (x)]=\int f_{ij}p(x)dx$ probability is called the covariance matrix $\Sigma$ :
$\Sigma =E[(x-\mu )(x-\mu)^{T} ]$ The third and fourth moments are called skewness and kurtosis, respectively.

2.1.5 Statistical independence and uncorrelation

Two random variables x and y are statistically independent if their joint probability density function can be factorized as follows:
$p (x, y) = p (x) p (y)$ Similarly, if the expected operation of these two variables satisfies the following formula, they are uncorrelated:
$E[xy^{T } ]=E[x]E[y]^{T}$ But "uncorrelated" is weaker than "independent". If two random variables are statistically independent, then they must be uncorrelated, but if two random variables are uncorrelated, then they are not necessarily statistically independent.

2.1.6 Normalized product

If p ₁ (x) and p ₂ (x) are two different probability density functions of a random variable x, then their normalized product is defined as:
$p(x)=\eta p_{1} (x)p_{2} (x)$ 其中 $\eta=\left ( \int p_{1} (x)p_{2} (x)dx \right ) ^{-1}$ is a constant normalization factor used to ensure that p(x) satisfies the axiom of total probability.

2.2 Gaussian probability density function

2.2.1 Definition

In the one-dimensional case, the Gaussian probability density function is expressed as:
$p(x|\mu ,\ sigma^{2} )=\frac{1}{\sqrt{2\pi \sigma^{2}} }exp\left ( -\frac{1}{2}\frac{(x-\mu )^ {2} }{\sigma^{2}} \right )$ whose one-dimensional Gaussian PDF image is as follows:
insert image description here
the Gaussian distribution of multidimensional variables is expressed as:
$p(x|\mu ,\Sigma )=\frac{1}{\sqrt{ (2\pi)^N det\Sigma} }exp\left ( -\frac{1}{2} (x-\mu)^{T}\Sigma ^{-1} (x-\mu ) \right )$ except $\mu=E[ x]=\int_{-\infty }^{\infty } x\frac{1}{\sqrt{(2\pi)^N the\sigma} }exp\left ( -\frac{1}{2} (x-\in)^{T}\Sigma ^{-1} (x-\in ) \right )dx$ $\Sigma =E[(x-\mu )(x-\mu )^{T} ]=\int_{-\infty }^{\infty } (x -\mu )(x-\mu )^{T}\frac{1}{\sqrt{ (2\pi)^N det\Sigma} }exp\left ( -\frac{1}{2}(x -\mu)^{T}\Sigma ^{-1} (x-\mu ) \right )dx$ Conventionally, the normal distribution (Gaussian distribution) is recorded as:
$x\sim N(\mu ,\Sigma )$ If the random variable x satisfies: $x\sim N(0 ,1 )$ Please add a picture description
where 1 is an N×N identity matrix, it can be considered that the random variable x obeys the standard normal distribution.

2.2.3 Joint Gaussian probability density function, decomposition and inference

Suppose a pair of variables (x,y) obey multivariate normal distribution, their joint probability density function is:
$p(x,y)=N\left ( \begin{bmatrix} \mu _{x} \\\mu _{y} \end{bmatrix} ,\begin{bmatrix} \Sigma _{xx} & \Sigma _{xy}\\ \Sigma _{yx} &\Sigma _{yy} \end{bmatrix}\right )$ . : $[$
Σ xx Σ
$\begin{bmatrix} \Sigma _{xx} &\Sigma _; {xy}\\\Sigma_{yx} &\Sigma_{yy}\end{bmatrix}=\begin{bmatrix}1 & \Sigma_{xy}\Sigma_{yy}^{-1}\\ 0 &1 \end{bmatrix} \begin{bmatrix} \Sigma _{xx}-\Sigma _{xy}\Sigma _{yy}^{-1}\Sigma _{yx} & 0 \\ 0 &\Sigma _{yy} \end{bmatrix} \begin{bmatrix} 1&0\\\Sigma _{yy}^{-1}\Sigma _{yx}&1\end{bmatrix}$ among which 1 is the unit matrix。再安全通过求逆可得：
$\begin{bmatrix} \Sigma _{xx} &\Sigma _{xy} \\ \Sigma _{ yx} &\Sigma _{yy} \end{bmatrix}^{-1} =\begin{bmatrix} 1 & 0 \\ -\Sigma _{yy}^{-1}\Sigma _{yx} &1\ end{bmatrix} \begin{bmatrix} \left ( \Sigma _{xx}-\Sigma _{xy}\Sigma _{yy}^{-1}\Sigma _{yx} \right ) ^{-1} & 0 \\ 0 &\Sigma _{yy}^{-1} \end{bmatrix} \begin{bmatrix} 1 & -\Sigma _{xy}\Sigma _{yy}^{-1} \\ 0 &1 \end{bmatrix}$ Therefore, we can get:
Please add a picture description
carefully observing the above formula, we can know that the result after simplification is the sum of two quadratic terms, which can be recorded as:
$x\sim N(\mu _{x} +\Sigma _{xy}\Sigma _{yy}^{-1} (y-\mu _{ y}),\Sigma _{xx}-\Sigma _{xy}\Sigma _{yy}^{-1} \Sigma _{yx})$ $y\sim N(\mu _{y},\Sigma _{yy} )$ Therefore, it can be obtained that
Please add a picture description
the factors p(x|y) and p(y) are both Gaussian probability density functions, so if the observed value y is known, it can be obtained by calculating p(x|y) through formula (2.53b) The likelihood of x given the value of y.
actually this isGaussian inferenceThe most important part: according to the prior probability distribution of the known state, combined with the observation model to narrow the range, and further obtain the posterior probability distribution. In formula (2.53b), it can be seen that some adjustments have taken place in the mean, and the covariance matrix has also become smaller, that is to say, its uncertainty has decreased.

2.2.4 Statistical independence, irrelevance

In the case of Gaussian probability density functions, statistical independence and uncorrelation are equivalent, that is, two Gaussian variables that are statistically independent are uncorrelated (as is usually true for general probability density functions), and two uncorrelated Gaussian variables The variables are also statistically independent (not all probability density functions hold).

2.2.5 Linear Transformation of Gaussian Random Variables

Suppose there is a Gaussian random variable $x\in \mathbb{R} ^{N} \sim N(\mu _{x },\Sigma _{xx} )$ and another random variable y linearly dependent on x: $y = Let G x$ be a continuous variable:
Please add a picture description
it is also a function $\sim N(\mu _{y},\Sigma _{yy } )=N(G\mu_{x},G\Sigma_{yy}G^{T} )$ .
Another method is the variable substitution method. Assume that this mapping is injective, that is, two x values cannot correspond to the same y value; in fact, this injective condition can be simplified by a stricter condition, that is, G is reversible (so M=N).
According to the axiom of total probability: $\int_{a}^{b} p(x)dx=1$ The x in a small area is mapped to y, and becomes: $d y = ∣ d e t G ∣ d x$ can then be substituted into the above formula, and we can get: $\mu _{y} =G\mu _{x}$
Please add a picture description
can also be obtained from the above formula $m_{y} = G μ_{x}$ Form $\ Sigma _{yy} = G\ Sigma _{xx}G^{T}$ _
However, if M<N, the linear map is not injective, and it is impossible to find the distribution of y by definite integral variable substitution. However, if M<N, rank(G)=M, the linear mapping from y to x can also be considered. But this will be a bit troublesome, because the mapping will expand the variables into a larger space, so the actual covariance matrix of x will become larger. In order to avoid this problem, the form of information matrix is adopted. The so-called information matrix is the inverse of the covariance matrix, which is used to represent the reliability of this measurement, that is, the smaller the uncertainty, the greater the reliability. Let $yu=\Sigma _{yy} ^{-1} y$ can be obtained $u\sim N(\Sigma _{yy} ^{-1} \mu _{y},\Sigma _{yy} ^ {-1} )$ Similarly, let $xv=\Sigma _{xx} ^{-1} x$ can be obtained $u\sim N(\Sigma _{xx} ^{-1} \mu _{y},\Sigma _{xx} ^{ -1} )$ Since the mapping from y to x is not unique, it is necessary to choose a special mapping, set as: $yv=G^{T} u\Leftrightarrow \Sigma _{xx}^{-1} x=G^{T} \Sigma _{yy}^{-1} y$ Then the expectation can be calculated:
Please add a picture description
It is worth noting that if $\Sigma _{xx}^{-1}$ Without full rank, it is impossible to recover $\Sigma _{xx}$ 和 $\mu _{x}$ , so the distribution can only be expressed in the form of information. But the distribution of this form of information expression can also be fused.

2.2.6 Normalized product of Gaussian probability density function

There is a useful property in the Gaussian probability density function, that is, the normalized product of K Gaussian probability density functions is still a Gaussian probability density function:
Please add a picture description
$\eta$ is a normalizing constant that ensures that the density function satisfies the axiom of total probability. When fusing multiple estimates together, a Gaussian normalized product is used, as shown in the figure below:

We have similar results for linear transformations of Gaussian distributed random variables:
Please add a picture description

2.2.7 Sherman-Morrison-Woodbury equation

The Sherman-Morrison-Woodbury (SMW) identity is also known as the matrix inversion lemma.
For an invertible matrix, it can be decomposed into a lower-diagonal-upper-triangular (LDU) form or an upper-diagonal-lower-triangular (UDL) form as follows: Then invert both sides of Eq
Please add a picture description
. For LDU, it can be obtained:

For UDL, it can be obtained:
Comparing the results of Equation (2.73) and Equation (2.74), the following equation can be obtained:

These will be frequently used when dealing with the covariance matrix of the Gaussian probability density function later identity.

2.2.8 Nonlinear Transformation of Gaussian Random Variables

Next, study the Gaussian distribution after a random nonlinear transformation, that is, calculate: $dxp(y)=\int_{-\infty }^ {\infty } p(y|x)p(x)dx$ minus $dxp(y)=\int_{-\infty }^{\infty } p (y|x)p(x)dx$ $p(x)=N(\mu _{x} ,\Sigma _{xx} )$ Here g(·) means $yg:x\mapsto y$ , is a nonlinear map. It is affected by Gaussian noise with covariance R. This type of stochastic nonlinear mapping is needed later to model the sensor. It is necessary to transfer the Gaussian distribution into the nonlinear transformation. For example, in Bayesian inference, the denominator of the Bayesian formula often has such a nonlinear transformation.

Nonlinear Mapping in the Scalar Case

First look at the simplified situation: x is a scalar, and the nonlinear function g( ) is determined (ie R=0). Let $x\in \mathbb{R} ^{1}$ is a Gaussian random variable: $x\sim N(0,\sigma ^{2} )$ From the x-PDF: $P(x)=\frac{1}{\sqrt{2\pi\sigma^{2} } } exp(-\frac{1}{2}\frac{x^{2}}{\sigma^{2}} )$ Now consider the non-linear mapping: $y = e x p (x)$ it is obviously reversible: $x = l n (y)$ on the infinite small interval, the relationship between x and y is: $d y = e x p (x) d x$ 或者 $dx=\frac{1}{y} dy$ According to the axiom of total probability, there is:
Please add a picture description
the above formula is the exact expression of p(y), when $\sigma ^2=1$ , its image is shown in the figure below:

the part below the curve is from y=0 to $\infty$ is 1. After a large number of samples of x and the nonlinear transformation g(·), a gray histogram can be obtained, which can be regarded as an approximation of the black curve. It can be seen that the approximate value is consistent with the real value, which verifies the correctness of the above transformation.
Note that p(y) no longer obeys the Gaussian distribution because of the nonlinear transformation.

General case of linearization

However, closed-form solutions cannot be obtained for every g(·), and in the case of multi-dimensional variables, the calculation will become extremely complicated. Also, when the nonlinear transformation is random (R > 0), the mapping is necessarily irreversible due to redundant noise inputs, so a different approach is required to address this situation. There are many ways to deal with it. This time we will introduce the most commonly used methods.linearization.
After linearizing the nonlinear transformation, we get: $g(x)\approx \mu _{y} +G(x-\mu _{x} )$ $G=\frac{\partial g(x)}{\partial x} \mid_{ x=\mu _{x} }$ $\mu _{y}=g(\mu _{x})$ where G is the Jacobian of g(·) with respect to x. After linearization, the "closed-form solution" of the above problem can be obtained. This solution is actually an approximate solution of the above problem. The approximate solution is only established when the nonlinear property of the mapping is not strong.
Please add a picture description As shown in the above figure, it describes the result of a one-dimensional Gaussian PDF after nonlinear transformation g( ), where g( ) is linearized.
From this formula: $dxp(y)=\int_{-\infty }^{\infty } p(y|x)p(x) dx$ can be obtained:

where $\eta$ is a normalization constant, and the matrix F is defined such that: $GF^{T} (G^{T} R^{-1} G+\Sigma _ {xx} ^{-1} )=R^{-1} G$ then needs to complete the square term in the integral, that is,
Please add a picture description
the second factor has nothing to do with x, so it does not need to be integrated. The first factor is the Gaussian distribution of x, so the integration of x can get a constant, and then with the constant $\eta$ merged. Similarly, for p(y), there are:

where $\rho$ is a new normalization constant. This formula is the Gaussian distribution of y: $y\sim N(\mu _{y},\Sigma _{ yy} )=N(g(\mu _{x} ),R+G\Sigma _{xx} G^{T})$

2.3 Gaussian process

Will satisfy the Gaussian distribution of variables $x\in \mathbb{R} ^{N}$ 记的： $x\sim N(\mu ,\Sigma )$ and use this type of random variable to express discrete-time state quantities.
The continuous state quantity at time t will be discussed next. To do this, a Gaussian process needs to be introduced first. The following figure describes the trajectory represented by the Gaussian process, where the mean value at each moment is represented by a mean function $\sigma(t)$ describes the variance at two different moments with a covariance function $\Sigma (t,t')$ description. Please add a picture description
The black solid line is the mean function, and the shaded area is the covariance function.
The entire trajectory can be considered as a random variable in a collection of functions. The closer a function is to the mean function, the more similar the trajectories are. The covariance function describes the smoothness of the trajectory by describing the correlation of random variables at two moments t, t'. We denote this random variable function as: Please add a picture description It shows that the continuous time trajectory is a Gaussian process. In fact, Gaussian processes are not limited to the case where the expression is one-dimensional with respect to time.
If only for a specific time $\tau$ If you are interested in the case of $τ$ $x(\tau)\sim N(\mu (\tau ),\Sigma (\tau ,\tau ))$ hereΣ $\Sigma (\tau ,\tau )$ is the ordinary covariance matrix. We can marginalize all other moments, leaving only this particular time $\tau$ x under $τ$ $x(\tau)$ can be regarded as a general Gaussian random variable.
Usually Gaussian processes have different manifestations. A commonly used Gaussian process is the zero-mean, white-noise Gaussian process. For zero-mean white noise $\omega (\tau )$ ,Determine: $\omega (\tau )\sim \mathcal{g} \mathcal{p} (0,Q\delta (tt'))$ where Q is the energy spectral density matrix, $\delta (tt')$ is Dirac $\delta$ function. Since its value only depends on the time difference t-t', the zero-mean white noise process is actually a stationary noise process.

Reference:
"State Estimation in Robotics"