Kalman Filter

最新推荐文章于 2024-05-14 10:08:21 发布

Tongust

最新推荐文章于 2024-05-14 10:08:21 发布

阅读量456

点赞数

分类专栏： ML

本文链接：https://blog.csdn.net/baidu_17640849/article/details/72598695

版权

ML 专栏收录该内容

7 篇文章 0 订阅

订阅专栏

Kalman Filter

Kalman Filter

1.1 Linear Optimal Filtering

LOF是kalman filter的精髓，可以描述为：

inputs: x[0], x[1], …, x[n]
outputs: y[0], y[1], …, y[n]
filter: w[0], w[1], …, w[n]
objection function: $e_n = y_n - \hat{y}_n$ , $\hat{y}_n$ spanned by $\{x_n\}$

1.2 Orthogonality Principle

LOF得到的 $\hat{y}_n$ 垂直于 $y_n-\hat{y}_n$

L O F \leftrightarrow y ̂ n ⊥ [y n - y ̂ n] \leftrightarrow E [y ̂ n [y n - y ̂ n] T] = 0

$LOF \ \leftrightarrow \hat{y}_n \bot [y_n-\hat{y}_n] \leftrightarrow E[\hat{y}_n[y_n-\hat{y}_n]^T] = 0$

1.4 State Model

X k + 1 Y K = F k X k + G K U K = H k X k + B k envolution of states observations (1) (2)

$\begin{align} \mathbf{X_{k+1}}&=F_k\mathbf{X_k}+G_K\mathbf{U_K} &\text{envolution of states}&(1)\\ \mathbf{Y_K}&=H_k\mathbf{X_k+B_k} &\text{observations}&(2)\\ \end{align}$

$H_k, F_k, G_k$ are determinated and known, MATRIX
$\mathbf{U_k,B_k}$ : White Noises of state and observation

这里B加粗 $\mathbf{B}$ 表示 $\mathbf{B}$ 是向量。

1.5 Objective And Hypothesis

Objective:

Obtain recursively linear estimators of $\mathbf{X_k}$ based on the observations up to the k instants ( $\mathbf{\{Y_n\}_{n=0,...,k}}$ ) and the signal statistical properties.

Hypothesis:

$E [X 0] = x ¯ 0, E [B k] = 0, E [U k] = 0 E ⎡ ⎣ ⎢ ⎢ ⎡ ⎣ ⎢ ⎢ B k X 0 U k ⎤ ⎦ ⎥ ⎥ [B T l X T 0 U T l] ⎤ ⎦ ⎥ ⎥ R k Q k δ k l = ⎡ ⎣ ⎢ ⎢ R k δ k l 00 0 P 0 0 00 Q k δ k l ⎤ ⎦ ⎥ ⎥ = E [B k B T k] = E [U k U T k] = {1, 0, if k = l; otherwise .$ $E[\mathbf{X_0}] = \bar{x}_0, E[\mathbf{B}_k] = \mathbf{0}, E[\mathbf{U}_k]=\mathbf{0}\\ \begin{align} E\begin{bmatrix} \begin{bmatrix} \mathbf{B}_k\\ \mathbf{X}_0\\ \mathbf{U}_k \end{bmatrix} & \begin{bmatrix} \mathbf{B}^T_l & \mathbf{X}_0^T & \mathbf{U}_l^T \end{bmatrix} \end{bmatrix} &= \begin{bmatrix} \mathbf{R}_k\delta_{kl} & \mathbf{0} & \mathbf{0}\\ \mathbf{0} & \mathbf{P}_0 & \mathbf{0}\\ \mathbf{0} &\mathbf{0} &\mathbf{Q}_k\delta_{kl}\\ \end{bmatrix}\\ \mathbf{R}_{k} &= E[\mathbf{B}_k\mathbf{B}^T_k]\\ \mathbf{Q}_{k} &= E[\mathbf{U}_k\mathbf{U}^T_k]\\ \delta_{kl}&=\begin{cases} 1,&\text{if $k=l$;}\\ 0, &\text{otherwise}. \end{cases} \end{align}$

这里要证明一下，为什么假设 $E[\mathbf{B}_k] = \mathbf{0}, E[\mathbf{U}_k]=\mathbf{0}$ 会是『Without loss of generality 』:
假设 $\mathbf{B}_k=\mathbf{B'}_k+\mathbf{B}_0$ , $\mathbf{U}_k=\mathbf{U'}_k+\mathbf{U}_0$

Y k (Y k - B 0) Y k n e w (X k + 1 + C) C = H k X k + B' k + B 0 = H k X k = (Y k - B 0) = H k X k = F k (X k + C) + G K U' k = (F k - 1) - 1 G k U 0

$\begin{align} \mathbf{Y_k}&=H_k\mathbf{X_k+B'_k+B_0}\\ (\mathbf{Y_k-B_0}) &= H_k\mathbf{X_k}\\ \mathbf{Y_k}^{new} &= (\mathbf{Y_k-B_0}) = H_k\mathbf{X_k}\\ (\mathbf{X}_{k+1} + \mathbf{C}) &= F_k(\mathbf{X_k+C})+G_K\mathbf{U'}_k\\ \mathbf{C} &= (F_k - 1)^{-1}G_k\mathbf{U}_0 \end{align}$

Remark

Stationary: 是指随机过程（不是随机变量）的联合概率密度不随时间改变而改变的状态。

Stationary Process: is a stochastic process whose joint probability distribution does not change when shifted in time. Consequently, parameters such as mean and variance, if they are present, also do not change over time. – From Wiki

须满足

E [X k] E [X k X T l] = x ¯ = Γ k - l

$\begin{align} E[X_k] &= \bar{x}\\ E[X_kX_l^T] &= \Gamma_{k-l} \end{align}$
Proof:

Xk,Yk $X_k,Y_k$ are non-stationary unless

the matrices $H_k , F_k , G_k , R_k, Q_k$ are time invariant (noted by H, F, G, R and Q);
F is a stable “filter”, i.e., all eigenvalues lies within the unit circle;
$\bar{x}_0$ = 0 and the initial covariance $P_0$ is a solution to the Lyapunov equation:
$P = FPF^T+GQG^T$

1.3 Notations

$y_n$ : space spanned by $\{Y_i\}_{i=1,..,n}$ ;
$\hat{X}(n+1|y_n)$ : optimal linear estimate of $X_{n+1}$ knowing $y_n$ ;
$\hat{Y} (n|y_{n-1})$ : optimal linear predictor of $Y_n$ knowing $y_{n-1}$ ;
$\alpha[n]\ = Y_n - \hat{Y} (n|y_{n-1})$ : innovation/error, the information we have gained.

Remark

i = n, $\hat{X}(n|y_{n-1})$ is the Kalman Filter (On line)
i < n, $\hat{X}(i|y_{n-1})$ is the Kalman Smoothing (Off line)

α [n] X ̂ (n + 1 | y n) Y ̂ (n | y n - 1) = Y n - Y ̂ (n | y n - 1) \in {y 1 : n} \in {y 1 : n - 1}

$\begin{align} \alpha[n] &= Y_n - \hat{Y} (n|y_{n-1})\\ \hat{X}(n+1|y_n) &\in \{y_{1:n}\} \\ \hat{Y} (n|y_{n-1}) &\in \{y_{1:n-1}\} \end{align}$

Proposition

$\alpha[n] \bot y_{n-1} \leftrightarrow E[\alpha[n]Y_k^T] = 0,\ 1 \le k \lt n$
$\alpha[n] \bot \alpha[k] \leftrightarrow E[ \alpha[k] \alpha[k]^T] = 0, \ k \ne n$
Linear transformation
$y_n = \{Y_1,...,Y_n\} \leftrightarrow \{\alpha[1], ..., \alpha[1]\}$

证明：
1.

s i n c e a n d \Rightarrow : Y ̂ (n | y n - 1) is optimal linear : α [n] = Y n - Y ̂ (n | y n - 1) : α [n] ⊥ y n - 1

$\begin{align} since&: \hat{Y} (n|y_{n-1}) \text{is optimal linear}\\ and &: \alpha[n] = Y_n - \hat{Y} (n|y_{n-1})\\ \Rightarrow &: \alpha[n] \bot y_{n-1} \end{align}$
这里有一些引理：

s p a c e i f i f : A, B, C, D, A ⊥ B : C \subseteq B \Rightarrow A ⊥ C : B = C \cup D \Rightarrow A ⊥ C & A ⊥ D

$\begin{align} space &: A, B, C, D, A\bot B\\ if &:C \subseteq B\\ &\Rightarrow A \bot C\\ if &: B = C \cup D\\ &\Rightarrow A\bot C \text{&} A\bot D \end{align}$

s i n c e a n d \Rightarrow : Y ̂ (n | y n - 1) is optimal linear : α [n] = Y n - Y ̂ (n | y n - 1) : α [n] ⊥ y n - 1

$\begin{align} since&: \hat{Y} (n|y_{n-1}) \text{is optimal linear}\\ and &: \alpha[n] = Y_n - \hat{Y} (n|y_{n-1})\\ \Rightarrow &: \alpha[n] \bot y_{n-1} \end{align}$

s i n c e s i n c e s i n c e s i n c e : Y n - 1 = H n - 1 X n - 1 + B n - 1 : X n = F n X n - 1 + G n - 1 U n - 1 \Rightarrow {X n} s p a c e \subset {X 0, U 0 : n - 1} s p a c e \Rightarrow {Y n} s p a c e \subset {X 0, U 0 : n - 1, B 0 : n} s p a c e : α [n] ⊥ y n - 1 \Rightarrow α [n] ⊥ {X 0, U 0 : n - 2, B 0 : n - 1} s p a c e : {X 0, U 0 : n - 3, B 0 : n - 2} s p a c e \subset {X 0, U 0 : n - 2, B 0 : n - 1} s p a c e \Rightarrow α [n] ⊥ {X 0, U 0 : n - 3, B 0 : n - 2} \Rightarrow α [n] ⊥ y n - 2 \Rightarrow α [n] ⊥ y i, 0 < i < n

$\begin{align} since &: Y_{n-1} = H_{n-1}X_{n-1} + B_{n-1}\\ since &: \mathbf{X_{n}}=F_n\mathbf{X_{n-1}}+G_{n-1}\mathbf{U_{n-1}}\\ &\Rightarrow \{\mathbf{X_{n}}\}_{space} \subset \{\mathbf{X_{0}},\mathbf{U}_{0:n-1}\}_{space}\\ &\Rightarrow \{\mathbf{Y_{n}}\}_{space} \subset \{\mathbf{X_{0}},\mathbf{U}_{0:n-1},\mathbf{B}_{0:n} \}_{space}\\ since &: \alpha[n] \bot y_{n-1} \\ &\Rightarrow \alpha[n] \bot \{X_0,U_{0:n-2}, B_{0:n-1}\}_{space} \\ since &:\{ X_0,U_{0:n-3}, B_{0:n-2}\}_{space} \subset \{X_0,U_{0:n-2}, B_{0:n-1}\}_{space}\\ &\Rightarrow \alpha[n] \bot \{X_0,U_{0:n-3}, B_{0:n-2}\} \\ &\Rightarrow \alpha[n] \bot y_{n-2}\\ &\Rightarrow \alpha[n] \bot y_{i}, \text{0 < i < n}\\ \end{align}$

◻ $\square$
2.

s i n c e s i n c e \Rightarrow : α [k] = Y k - Y ̂ (n | y k - 1) \in {Y k} s p a c e : α [k] ⊥ y k : α [n] ⊥ α [k], 1 \leq k \leq n - 1

$\begin{align} since&: \alpha[k] = Y_k - \hat{Y} (n|y_{k-1}) \in \{Y_k\}_{space}\\ since&: \alpha[k] \bot y_k \\ \Rightarrow &: \alpha[n] \bot \alpha[k] , 1 \le k \le n-1 \end{align}$
3.

α [n] \Rightarrow α [n] Y n \Leftarrow Y n - 1 = Y n - Y ̂ (n | y n - 1) \in {Y n} = α [n] + Y ̂ (n | y n - 1) \in {α [n], Y n - 1} \in {α [n - 1], Y n - 2} 归 纳 法

$\begin{align} \alpha[n] &= Y_n - \hat{Y} (n|y_{n-1})\\ \Rightarrow \alpha[n] &\in \{Y_n\}\\ Y_n &= \alpha[n] + \hat{Y} (n|y_{n-1}) \in \{\alpha[n], Y_{n-1}\}\\ \Leftarrow Y_{n-1} &\in \{\alpha[n-1], Y_{n-2}\} \text{归纳法} \end{align}$

Conclusion of Space Relationship

{X n} s p a c e {Y n} s p a c e B k B k U k U k B k \subset {X 0, U 0 : n - 1} s p a c e \subset {X 0, U 0 : n - 1, B 0 : n} s p a c e ⊥ U l ⊥ X l, 0 \leq l \leq k ⊥ X l, 0 \leq l \leq k ⊥ Y l, 0 \leq l \leq k ⊥ Y l, 0 \leq l \leq k - 1

$\begin{align} \{\mathbf{X_{n}}\}_{space} &\subset \{\mathbf{X_{0}},\mathbf{U}_{0:n-1}\}_{space}\\ \{\mathbf{Y_{n}}\}_{space} &\subset \{\mathbf{X_{0}},\mathbf{U}_{0:n-1},\mathbf{B}_{0:n} \}_{space}\\ B_k &\bot U_l\\ B_k &\bot X_{l}, 0 \le l \le k\\ U_k &\bot X_{l}, 0 \le l \le k\\ U_k &\bot Y_{l}, 0 \le l \le k\\ B_k &\bot Y_{l}, 0 \le l \le k-1\\ \end{align}$

The Innovation Covariance

ϵ (n, n - 1) K (n, n - 1) Y ̂ (n | y n - 1) α [n] = X n - X ̂ (n | y n - 1) = E [ϵ (n, n - 1) ϵ (n, n - 1) T] = H n X ̂ (n | y n - 1) = Y n - Y ̂ (n | y n - 1) the prediction error covariance the prediction of observation innovation

$\begin{align} \epsilon(n,n-1) &= X_n - \hat{X}(n | y_{n-1}) &\text{the prediction error}\\ K(n,n-1) &= E[\epsilon(n,n-1)\epsilon(n,n-1)^T]&\text{covariance}\\ \hat{Y} (n|y_{n-1}) &= H_n\hat{X}(n | y_{n-1}) &\text{the prediction of observation}\\ \alpha[n] &= Y_n - \hat{Y} (n|y_{n-1})&\text{innovation}\\ \end{align}$

Proof the prediction of observation:

Y ̂ (n | y n - 1) = H n X ̂ (n | y n - 1)

$\hat{Y} (n|y_{n-1}) = H_n\hat{X}(n|y_{n-1})$

Proof: 既然 $\hat{Y} (n|y_{n-1})$ 表示 optimal linear predictor of $Y_n$ knowing $y_{n-1}$ , 那么它与 $Y$ 的差必定与空间spanned by $\{y_{n-1}\}$ 相互垂直.

Y n - Y ̂ (n | y n - 1) \Rightarrow E [(Y n - Y ̂ (n | y n - 1)) (y n - 1) T] s i n c e H n X n + B n \Rightarrow E [(H n (X n - X ̂ (n | y n - 1)) + B n) y T n - 1] \Leftarrow X n - X ̂ (n | y n - 1) ⊥ y n - 1 (O p t i m a l ⊥ y n - 1 = 0 = Y n = 0 F i l t e r p r o p e r t i e s) a n d B n ⊥ y n - 1

$\begin{align} Y_n-\hat{Y}(n|y_{n-1})\ \ &\bot\ \ y_{n-1}\\ \Rightarrow E[(Y_n-\hat{Y}(n|y_{n-1}))(y_{n-1})^T] &= 0\\ since \ \ H_nX_n+B_n &= Y_n\\ \Rightarrow\ \ E[(H_n(X_n - \hat{X}(n|y_{n-1})) + B_n)y_{n-1}^T] &= 0\\ \Leftarrow X_n - \hat{X}(n|y_{n-1}) \bot y_{n-1}(Optimal\ &Filter\ properties) \ and \ B_n \bot y_{n-1} \end{align}\\$

◻ $\square$

Tongust

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
Kalman Filter

Kalman Filter1 Linear Optimal Filtering2 Orthogonality Principle4 State Model5 Objective And HypothesisObjectiveHypothesisRemark3 NotationsRemarkPropositionConclusion of Space RelationshipT
复制链接

扫一扫