Anatomy of the Kalman family from scratch - (05) Kalman filter → formula derivation, easy to understand from a one-dimensional level

Explaining the summary link of a series of articles about slam:The most comprehensive slam in history starts from scratch, for the Kalman family explained in this column Zero anatomy link:The Kalman family’s anatomy from scratch - (00) catalog latest explanation without blind spots: https://blog.csdn.net/weixin_43013761/article/details/133846882
 
$The\; center\; directly\; below\; the\; end\; of\; the\; articleprovides\; mycontact\; information,click\; on\; my\; photo\; to\; displaya>XW\to Official\; certification$

$Zheng\; heavy\; statement:\; 该\; SERIES\; EXPO\; CUSTOMER\; HIMSELF\left(WenhaiZhu\right)Single\; private\; ownership,Prohibited\; abuse,Thank\; you\; for\; your\; first\; report!$
 

I. Introduction

        In a blog, through the previous series of blogs, I have a certain understanding of Karl filtering, and the entire process was sorted out in the previous blog. First of all, Bayesian filtering is just an ideological guide, and there is no way to apply it directly. Among them, Kalman diffuse filtering is an instantiation. Of course, it is not a complete instantiation. If you want to implement the Kalman filtering algorithm, then A further step is needed to conduct modeling based on actual application conditions.

        Kalman filtering is based on the LG (Linear Gaussian) system for inference, so it is only applicable to the LG mathematical model. If you want to apply to the NL or NG system, you need to use other variant algorithms, such as EKF (Extended Kalman filter), these contents will be analyzed in detail in the following blogs. In the previous article, the formula is summarized as follows:
$$f_{X_k}^{+}(x)={the}_k\cdot f_{X_k|Y_k}(x)\cdot f_{X_k}^{-}\left(x\right)={the}_k\cdot f_{R_k}\left[y_k-h(x)\right]\cdot {\int }_{-\infty }^{+\infty }{f}_{Q_k}[x-f(v)]f_{X_{k-1}}^{+}(v)\mathrm{d}v\text{\hspace{1em}(01)}$$$${the}_k=[{\int }_{-\infty }^{+\infty }(f_{R_k}\left[y_k-h\left(x\right)\right]\cdot {\int }_{-\infty }^{+\infty }{f}_{Q_k}[x-f(v)]f_{X_{k-1}}^{+}(v)\mathrm{d}v\mathrm{d}x{]}^{-1})\mathrm{d}x\text{\hspace{1em}(02)}$$It has been analyzed that the main culprits that hinder the implementation of the algorithm are the above two Infinite integral, then the next core is how to avoid infinite integral, or directly solve infinite integral. Now let’s look at how the Ermei filter avoids infinite products from the source. The main steps are as follows:

$\left(01\right)$ state transfer function $f\left(x\right)$ 观测function$h\left(x\right)$ The city is linear, Below ( $\check{.}$ Display destination,$\widehat{.}$ represents posterior): $$\begin{gathered} Equation\; of\; state:{\check{x}}_k=f{\hat{x}}_{k-1}+q_{k}fis\; a\; one-dimensional\; constant \\ observation\; equation:\hat{x}=h{\check{x}}_k+r_{k}his\; a\; one-dimensional\; constant\text{\hspace{1em}(03)} \end{gathered}$$$(02)$ 其上 $q_k\in Q\sim N(u_{q_k},p_k^2)$， $r_k\in R\sim N(u_{r_k},p_{r_k}^2)$, a large distribution.

It should be noted that the subsequent derivation process starts with a one-dimensional example and then expands to high dimensions, because one-dimensional is not designed to include matrices, multivariate Gaussians, covariance matrices, etc. It is called seeing the essence through phenomena. Although one-dimensional ones are rarely used in practical applications, it is still more appropriate to understand the most appropriate and lowest-level principles from one dimension. Because it is one-dimensional, it is represented by lowercase letters. In addition, the above assumption is an instantiation of a random process, that is, ${\check{x}}_k$ 、${\check{x}}_k$、$q_k$、$r_k$Represents the specific values ​​of the random process, rather than random variables. In addition, the derivation of formula (02) uses assumptions:

$(03):$ $X_0$ 与 $Q_1$、$Q_2$、$Q_3$、$......$、$Q_k$ Mutually independent.
$(04):$ $X_1$ give $R_1$、$R_2$、$R_3$、$......$、$R_k$Independent.

So it needs to be recorded here, that is, in the actual application process, this assumption cannot be ignored, otherwise it will not be suitable for Bayesian filtering. In addition to the above assumptions, additional knowledge points are required:

$(05):$ The normal distribution function is linearly transformed and still conforms to the normal distribution.
$(06):$ The product of two normal distributions is still a normal distribution (remember the conclusion first and then deduce it later).

The result of the product of two normal distributions is as follows (note, remember the result first, don’t get entangled in the process and fall into a dead end):
$$x_1\in X_1\sim N(v_1,p_1^2)x_2\in X_2\sim N(v_2,p_2^2)\text{\hspace{1em}(04)}$$$$f(x_1)\ast f\left(x_2\right)=N\left(\frac{p_1^2}{p_1^2+p_2^2}{m}_2+\frac{p_2^2}{p_1^2+p_2^2}{m}_1，\frac{p_1^2{p}_2^2}{p_1^2+p_2^2}\right)\text{\hspace{1em}(05)}$$

2. Ideological guidance

First of all, let’s analyze it as a whole, assuming the prior state ${\hat{x}}_{k-1}$Probability density function of $f_{k-1}^{+}$ consistent with normal distribution$N(v_{x_{k-1}},p_{x_{k-1}}^2)$, then according to the state transition function$f({\check{x}}_k)=af({\hat{x}}_{k-1})+q_k$，结合 【$(05):$ The normal distribution function is linearly transformed and still conforms to the normal distribution. It can be known that $f({\check{x}}_k)$ 】，可知 $f\left({\check{x}}_k\right)=f_{X_k}^{+}\left(x\right)$ $k-1$ The posterior at time conforms to the normal distribution, then $The\; prior\; at\; time\; k$ also conforms to the normal distribution. Let’s look at formula (01):$$f_{X_k}^{+}(x)={the}_k\cdot f_{X_k|Y_k}(x)\cdot f_{X_k}^{-}\left(x\right)\text{\hspace{1em}(06)}$$ Kaichimichi, Waka $f_{X_k|Y_k}($$f_{X_k}^{+}\left(x\right)$ η k \eta_k ${the}_k$ is a constant), so the recursion formula comes out. According to the formula (1):$$f_{X_k|Y_k}\left(x\right)=f_{R_k}\left[y_k-h(x)\right]\text{\hspace{1em}(07)}$$ $h\left(x\right)$ is the linearity function, < /span>${and}_k-h\left(x\right)$ is equivalent to The original too-normal distribution graph has been translated or scaled, so the result $f_{X_k|Y_k}\left(x\right)$ Yorukyushota distribution.

$core:$ According to the above derivation, it can be seen that if $f_{k-1}^{+}$ conforms to the normal distribution, then $f_{X_k}^{-}(x)$ 与 $f_{X_k|Y_k}\left(x\right)$ Tofu Shota, Mata ${the}_k$ is a parameter, according to [$(06):$ The product of two normal distributions is still a normal distribution], it can be seen that$f_{X_k}^{+}\left(x\right)$ 为正太distribution. Last possible, young $x_0$ is consistent with the normal distribution and can be derived continuously ${\hat{x}}_1$、${\hat{x}}_2$、$\cdots$、${\hat{x}}_{k-1}$、${\hat{x}}_k$All are consistent with the normal distribution.

Additional addition here $\eta$ is a constant, and its expression is inDissection of the Kalman family from scratch - (01) Preliminary knowledge points$$the=[f_{{AND}_k}(x){]}^{-1}=[{\int }_{-\infty }^{+\infty }{f}_{{AND}_k\mid X_k}(y_k\mid x)f_X(x)\mathrm{d}x{]}^{-1}\text{\hspace{1em}(08)}$$ Its essence comes from the total probability formula of continuous random variables. In fact, it is relatively Easy to understand, currently observed ${and}_k$，那么 $P(y_k)$ can already be determined, because it has been assumed that $f_{{AND}_k}\left(X\right)$ Signed regular distribution.观测到 ${and}_k$The total probability of is not all $In\; the\; x$ state, it can be observed ${AND}_k$Is the sum of probabilities of probabilities?

3. Formula derivation

Before deriving further, you must first understand the known conditions and the purpose of derivation, so as to avoid not knowing what you are doing during the calculation process, as follows:
$$A\; known:{\hat{x}}_{k-1}Solve:{\hat{x}}_{k-1}\text{\hspace{1em}(09)}$$Of course, there is no way to proceed with just one known condition. , so additional assumptions are needed to infer based on LG (linear Gaussian system), so$f_{X_k}^{-}(x)$、 $f_{X_k}^{+}(x)$、$f_{Q_k}(x)$ 、$f_{R_k}\left(x\right)$ Integral range:$$f(x)=\frac{1}{p\sqrt{2\pi }}{It\; is}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}X\sim N\left(\mu ,p^2\right)\text{\hspace{1em}(10)}$$并且$$f_{Q_k}(x)\sim N(0,p_{q_{k-1}})f_{R_k}\left(x\right)\sim N(0,p_{R_{k-1}})\text{\hspace{1em}(11)}$$

above $\mu$ Display uniformity, $\sigma$ represents the standard deviation. In addition, it is also necessary to instantiate the state transition equation and observation equation in (03). Here we assume: $$\begin{gathered} Equation\; of\; state:{\check{x}}_k=f{\hat{x}}_{k-1}+q_{k}fis\; a\; one-dimensional\; constant \\ observation\; equation:\hat{x}=h{\check{x}}_k+r_{k}his\; a\; one-dimensional\; constant\text{\hspace{1em}(12)} \end{gathered}$$The above assumptions are relatively simple,$f$、$h$ are two parameters and will not change, $f$、$h$ The scenarios that will change will not be considered here. The process is relatively complicated, and of course the application will be more extensive.

1. Prediction step

First refer to equation (01), which can guide $f_{X_{k-1}}^{-}$ given $f_{X_{k-1}}^{+}$The relationship of is as follows:
$$f_{X_k}^{-}(x)={\int }_{-\infty }^{+\infty }{f}_{Q_k}[x-f(v)]f_{X_{k-1}}^{+}(v)\mathrm{d}v\text{\hspace{1em}(11)}$$设 $f_{Q_k}(x)$ 的均值为 $m_{Q_k}=0$，方差为 $p_{Q_k}$；$f_{X_{k-1}}^{+}\left(x\right)$ 的 uniformity ${\hat{x}}_{k-1}$, the variance is $p_{X_{k-1}}^{+}$6 Reduce the equation:
$$f_{X_{k-1}}^{+}(v)=\frac{1}{p_{Xk-1}\sqrt{2\pi }}{It\; is}^{-\frac{(v-{\hat{x}}_{k-1}{)}^2}{2{\sigma }_{Xk-1}^{-2}}}\text{\hspace{1em}(12)}$$$$f_{Q_{k-1}}(x-f(v))=\frac{1}{p_{Qk-1}\sqrt{2\pi }}{It\; is}^{-\frac{(x-av{)}^2}{2{\sigma }_{Qk-1}^2}}in:f(v)=fv\text{\hspace{1em}(13)}$$
Top demand caution $m_{Q_k}=0$, lower surface regrasp $f_{X_{k-1}}^{+}(v)$ 与 $f_{Q_{k-1}}(x-f(v))$ Differential (11) Given:$$f_{X_k}^{-}(x)={\int }_{-\infty }^{+\infty }\frac{1}{p_{Qk-1}\sqrt{2\pi }}{It\; is}^{-\frac{(x-fv{)}^2}{2{\sigma }_{Qk-1}^2}}\cdot \frac{1}{p_{Xk-1}^{+}\sqrt{2\pi }}{It\; is}^{-\frac{(v-{\hat{x}}_{k-1}{)}^2}{2{\sigma }_{Xk-1}^{+2}}}\cdot \mathrm{d}v\text{\hspace{1em}(14)}$$ At this time, if we look at the above formula, it is actually relatively clear. As mentioned earlier, the product of two Gaussian density functions can be calculated using Mathematic software to get the answer (will be manually derived later), where the variables $v$ will be integrated. After integration, there will be only one variable left $x$ ，得答案如下：
$$f_{X_k}^{-}\left(x\right)\sim N({\check{x}}_k,p_{X_k}^{-})=N(f{\hat{x}}_{k-1},f^2{\sigma }_{X_{k-1}}^{+}+p_{Q_{k-1}})\text{\hspace{1em}(15)}$$

2.Update step

Complete the prior probability density function $f_{X_k}^{-}The\; derivation\; of\; (x)$ is now to find the likelihood probability density function, which is Eq.中$f_{X_k|Y_k}\left(x\right)=f_{R_k}\left[y_k-h\left(x\right)\right]$ The result of , not to mention other things, seems to be too simple intuitively. Directly refer to the previous formulas (10) (11) and bring it in:
$$f_{X_k|Y_k}\left(x\right)=\frac{1}{p_{R_k}\sqrt{2\pi }}{It\; is}^{-\frac{(y-hx{)}^2}{2{\sigma }_{R_k}^2}}\text{\hspace{1em}(15)}$$Needless to say, the above is obvious$N(hx,p_{Rk-1}^2)$ is a normal distribution. Now we need to use it to calculate the previous $N({\mu }_{X_k}^{-},p_{X_k}^{-2})$ Define, infinitesimal $N({\mu }_{X_k}^{+},p_{X_k}^{+2})$, of course the correction process cannot forget to multiply a normalized variable:
$${the}_k=[{\int }_{-\infty }^{+\infty }{f}_{X_k|Y_k}(x)\cdot f_{X_k}^{-}(x)dx{]}^{-1}\text{\hspace{1em}(16)}$$The answer is still given directly here. I will deduce it in detail when I have time later. , what is given here is$\hat{x}=E(f_{X_k}^{+}(x))$ ：
$$X_k^{+}=({\hat{x}}_k,p_{X_k}^{+})\sim N\left(\frac{h{\sigma }_{X_k}^{-}{and}_k+p_{R_k}{\check{x}}_k}{h^2{\sigma }_{X_k}^{-}+p_{R_k}},\frac{p_{R_k}{p}_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}\right)\text{\hspace{1em}(17)}$$

3. Simplify the formula

Although the answer has basically been figured out through the above derivation, it is not very friendly to the implementation of programming, so we need to continue to simplify it. Before that, we first sort out the above formula, (15 ) (17) The equivalent formula is as follows:
$${\check{x}}_k=f{\hat{x}}_{k-1}{p}_{X_k}^{-}=f^2{\sigma }_{X_{k-1}}^{+}+p_{Q_{k-1}}\text{\hspace{1em}(18)}$$$${\hat{x}}_k=\frac{h{\sigma }_{X_k}^{-}{and}_k+p_{R_k}{\check{x}}_k}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}{p}_{X_k}^{+}=\frac{p_{R_k}{p}_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}\text{\hspace{1em}(19)}$$According to the above formula, we can know intuitively that it is a recursion Derivation, if it is known${\hat{x}}_0$ 与 $p_{X_0}^{+}$, and observations at each time ${and}_k$, indicates:
$$【{\hat{x}}_0，{\sigma }_{X_0}^{+}，y_1】\to 【{\hat{x}}_1，{\sigma }_{X_1}^{+},{and}_2】\to \cdots \to 【{\hat{x}}_k，{\sigma }_{X_k}^{+}】\text{\hspace{1em}(20)}$$individual(19) is an infinitesimal:$$\begin{array}{rl}{\hat{x}}_k& =\frac{h{\sigma }_{X_k}^{-}{and}_k+p_{R_k}{\check{x}}_k}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}\\ & =\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}{and}_k+\frac{p_{R_k}{\check{x}}_k}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}\\ & =\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}{and}_k+\frac{\check{x}({\sigma }_{R_k}+h^2{\sigma }_{X_k}^{-})-{\check{x}}_k{h}^2{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}\\ & =\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}{and}_k+\check{x}-\frac{\check{x}{h}^2{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}\\ & =\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}(y_k-h\check{x})+\check{x}\end{array}\text{\hspace{1em}(21)}$$ $$\begin{array}{rl}{p}_{X_k}^{+}& =\frac{p_{R_k}{p}_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}\\ & =\frac{(h^2{\sigma }_{X_k}^{-}+p_{R_k}){\sigma }_{X_k}^{-}-h^2{\sigma }_{X_k}^{-}{p}_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}\\ & =p_{X_k}^{-}-\frac{h^2{\sigma }_{X_k}^{-}{p}_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}\\ & =(1-\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}h){\sigma }_{X_k}^{-}\end{array}\text{\hspace{1em}(22)}$$You can see that they all have one thing in common, that is Karl Mann gain term: $$k_k=\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}\text{\hspace{1em}(23)}$$Bring it to (22) (23): $${\hat{x}}_k=k_k(y_k-h\check{x})+\check{x}{\sigma }_{X_k}^{+}=(1-h{k}_k){\sigma }_{X_k}^{-}\text{\hspace{1em}(24)}$$

4. Summary

Through the above series of derivation, the following five formulas can be obtained, which are Kalman’s five core formulas, namely the two formulas in (15) (need to be expanded), and (23) (24) , organized as follows:
$$①：{\check{x}}_k=f{\hat{x}}_{k-1}②：{\sigma }_{X_k}^{-}=f^2{\sigma }_{X_{k-1}}^{+}+p_{Q_{k-1}}\text{\hspace{1em}(25)}$$ $$③：k_k=\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+p_{R_k}}\text{\hspace{1em}(26)}$$$$④：{\hat{x}}_k=k_k(y_k-h\check{x})+\check{x}⑤：{\sigma }_{X_k}^{+}=(1-h{k}_k){\sigma }_{X_k}^{-}\text{\hspace{1em}(27)}$$
The above five formulas are obviously recursive. If we assume that Know${\hat{x}}_0$、$p_{X_0}^{+}$, and observations at each time ${and}_k$, then it can be derived that ${\hat{x}}_k$、$p_{X_k}^{+}$，如下：
$$【{\hat{x}}_0,p_{X_0}^{+},{and}_1】\to 【{\hat{x}}_1,p_{X_1}^{+},{and}_2】\to \cdots \to 【{\hat{x}}_k,p_{X_k}^{+}】\text{\hspace{1em}(28)}$$
However, there is a big problem with the formula derived above, that is, its Based on one-dimensional derivation, one-dimensional Kalman filter is rarely used in actual applications. Although derivation in this way can provide a good understanding of the entire derivation process, it has too many limitations.

In addition, there are still many places that have not been explained clearly, such as the product and integral of two Gaussian distributions, the linear change of Gaussian distribution, etc., which have not been explained in detail. Don’t worry, subsequent blogs will expand on detailed analysis and deduction.