卡尔曼滤波器：3.卡尔曼增益

卡尔曼滤波器：3.step by step derivation of kalman Gain

状态空间方程
$$\begin{array}{rl}{X}_k& =A{X}_{k-1}+B{u}_{k-1}+{\omega }_{k-1}\\ Z_k& =H{X}_k+{\nu }_k\end{array}$$
其中

KaTeX parse error: {align} can be used only in display mode.

状态过程方程

${\omega }_{k-1}$过程噪声，是不可测量的，但是自然界的噪声都被假设成服从正太分布，

则其概率分布$p(\omega )\sim (0,Q)$，其中0代表了其期望，Q代表其协方差矩阵

如果对$Q$进行求解，则
KaTeX parse error: Undefined control sequence: \matrix at position 13: Q = E\left[\̲m̲a̲t̲r̲i̲x̲{\omega\cdot\om…
(A)式是怎么来的呢？举一个小小的栗子：

假设KaTeX parse error: Undefined control sequence: \matrix at position 12: x_k =\left[\̲m̲a̲t̲r̲i̲x̲{x_1\\x_2}\righ…则过程噪声也是二维的KaTeX parse error: Undefined control sequence: \matrix at position 15: \omega =\left[\̲m̲a̲t̲r̲i̲x̲{\omega_1\\\ome…
KaTeX parse error: Undefined control sequence: \matrix at position 39: …mega^T &=\left[\̲m̲a̲t̲r̲i̲x̲{\omega_1\\\ome…
概率论基础公式：
$$VAR(x)=E(x^2)-E^2(x)$$
其中$VAR$方差 $E$ 期望

则对于$p(\omega )\sim (0,Q)$,期望是0，则$VAR(x)=E(x^2)$

则式子（1）就可以是以下形式
KaTeX parse error: Undefined control sequence: \matrix at position 40: …mega^T &=\left[\̲m̲a̲t̲r̲i̲x̲{E(\omega_1^2)&…
用这样一个式子就能表现出来${\omega }_1{\omega }_2$之间关系

测量方程

同样测量噪声也是被认为服从正态分布$p(\nu )\sim (0,R)$和上面的一样
$$R=E(\nu {\nu }^T)$$

综合分析

①对于过程噪声和测量噪声是无法进行建模的，因为缺少了过程噪声，只靠前半部分求解出来的只是一个估计值

${\hat{x}}_k=A{x}_{k-1}+B{u}_{k-1}$

②因为因为我们没有对上面的式子做任何的处理，只是从上面的式子copy出来的，要再加一个标志，表示先验估计

${\hat{x}}_k^{-}=A{x}_{k-1}+B{u}_{k-1}$

③因为上一时刻的$x_{k-1}$也是看不到的，也是要根据上一时刻的估计值带入到运算(算出来的先验结果)

${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}$

④根据$z_k=H{x}_k$可以得出（测量出来的测量结果）

根据测试的结果，加上hat表示估计，${\hat{x}}_{kmea}=H^{-1}{z}_k$

但是我们发现不管是算出来的还是测出来的结果都不具备考虑噪声的影响，因为如果能对噪声进行建模就不存在利用卡尔曼滤波器的作用了，因为你可以直接得出来结果了，但是因为有了这些不确定的噪声不管是测量结果还是先验结果都是不准确的，这时候卡尔曼滤波器的作用就体现出来了，我们如何利用两个不太准确的结果得出来一个准确的结果，可以回忆一下上面讲到的数据融合的问题，这里面就用同样的方法进行处理

卡尔曼滤波的推导开始了

①我们一个新的${\hat{x}}_k$，就是状态的新的估计值，注意这里面没有-号了，就代表了是后验结果，或者可以说是最终的一个估计值利用数据融合的形式：

${\hat{x}}_k={\hat{x}}_k^{-}+G(H^{-}{z}_k-{\hat{x}}_k^{-})$

分析

KaTeX parse error: No such environment: flalign* at position 8: \begin{̲f̲l̲a̲l̲i̲g̲n̲*̲}̲ &G=0:\space \h…
但是呢，我们在教科书上看到的不是这样的一个结果，它是有一个很简单的变换：

$G=K_{k}H$

②结合G的变换得到结果如下
KaTeX parse error: No such environment: flalign* at position 8: \begin{̲f̲l̲a̲l̲i̲g̲n̲*̲}̲ &\hat{x}_k=\ha…
大家可以看一下：
KaTeX parse error: No such environment: flalign* at position 8: \begin{̲f̲l̲a̲l̲i̲g̲n̲*̲}̲ &K_k = 0:\hat{…
下面的推导就很明显了，目标：寻找$k_k$使得误差最小，使得${\hat{x}}_k=x_k$即使得估计值趋近于真实值

这个必然和测量噪声和过程噪声产生的误差息息相关

如何量化误差呢？
KaTeX parse error: No such environment: flalign* at position 8: \begin{̲f̲l̲a̲l̲i̲g̲n̲*̲}̲ &e_k=x_k-\hat{…
误差也是符合正太分布的一种形式
KaTeX parse error: No such environment: flalign* at position 8: \begin{̲f̲l̲a̲l̲i̲g̲n̲*̲}̲ &p(e_k) \sim(0…
误差越是趋近于0，方差就会越小，我们希望求出K使得$tr(P)={\sigma }_{e_1}^2+{\sigma }_{e_2}^2$最小，就是矩阵的迹最小，也就是方差最小

将①式求解方差
$$
\begin{flalign*}
&P_k=E[ee^T]\
&=E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T]&

\end{flalign*}
$$因为$$
\begin{flalign*}
&\hat{x}_k=\hat{x}^-_k + K_k(z_k - H\hat{x}^-_k) \
&Z_k=HX_k+\nu_k&
\end{flalign*}
$$带进去得到$$
\begin{flalign*}
&x_k-\hat{x}_k=x_k-\hat{x}^-_k - K_k(z_k - H\hat{x}^-_k)\
&=x_k-\hat{x}^-_k - K_kZ_k+K_kH\hat{x}^-_k\
&=x_k-\hat{x}^-_k -K_kHX_k-K_k\nu_k+K_kH\hat{x}^-_k\
&=x_k-\hat{x}^{-_k-K_kH(x_k-\hat{x}}-_k)-K_k\nu_k\
&=(I-K_kH)(x_k-\hat{x}^-_k)-K_k\nu_k&

\end{flalign*}
$$
根据误差的定义，把$x_k-{\hat{x}}_k^{-}$定义为先验误差$e_k^{-}$

则带入到方差P的求解中得到：
$$
\begin{flalign*}
&P_k=E[ee^T]\
&=E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T]\
&=E[[(I-K_kH)e^{-_k-K_k\nu_k][(I-K_kH)e}-_k-K_k\nu_k]^T]&

\end{flalign*}
$$插入概率论的基本公式：$$
\begin{flalign*}
(AB)^T=BTA^T\
(A+B)^T=AT+B^T
\end{flalign*}
$$继续推导$$
\begin{flalign*}
&P_k=E[ee^T]\
&=E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T]\
&=E[[(I-K_kH)e^{-_k-K_k\nu_k][(I-K_kH)e}-_k-K_k\nu_k]^T]\
&=E[[(I-K_kH)e^{-_k-K_k\nu_k][{e}-_k}^T(I-K_kH)T-\nu_k^TK_kT]]\
&=E[(I-K_kH)e^-_k{e-_k}^T(I-K_kH)T-(I-K_kH)e^-_k\nu_kTK_k^T-K_k\nu_k{e-_k}^T(I-K_kH)T+K_k\nu_k\nu_k^TK_kT]\
&=E((I-K_kH)e^-_k{e-_k}^T(I-K_kH)T)-E((I-K_kH)e^-_k\nu_kTK_k^{T)-E(K_k\nu_k{e}-_k}^T(I-K_kH)T)+E(K_k\nu_k\nu_k^TK_kT)\
&

\end{flalign*}
$$其中：$$
\begin{flalign*}
&E((I-K_kH)e^-_k\nu_kTK_k^{T)=(I-K_kH)E(e}-_k)E(\nu_k^T)K_kT=0 \
&E(K_k\nu_k{e^-_k}T(I-K_kH)^{T)=K_kE(\nu_k}T)E(e^-_k)(I-K_kH)
&

\end{flalign*}
$$继续推导：$$
\begin{flalign*}
&P_k=E[ee^T]\
&=E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T]\
&=E[[(I-K_kH)e^{-_k-K_k\nu_k][(I-K_kH)e}-_k-K_k\nu_k]^T]\
&=E[[(I-K_kH)e^{-_k-K_k\nu_k][{e}-_k}^T(I-K_kH)T-\nu_k^TK_kT]]\
&=E[(I-K_kH)e^-_k{e-_k}^T(I-K_kH)T-(I-K_kH)e^-_k\nu_kTK_k^T-K_k\nu_k{e-_k}^T(I-K_kH)T+K_k\nu_k\nu_k^TK_kT]\
&=E((I-K_kH)e^-_k{e-_k}^T(I-K_kH)T)-E((I-K_kH)e^-_k\nu_kTK_k^{T)-E(K_k\nu_k{e}-_k}^T(I-K_kH)T)+E(K_k\nu_k\nu_k^TK_kT)\
&=E((I-K_kH)e^-_k{e-_k}^T(I-K_kH)T) + E(K_k\nu_k\nu_k^TK_kT)\
&=(I-K_kH)E(e^-_k{e-_k}^T)(I-K_kH)T + K_kE(\nu_k\nu_k^T)K_kT\
&因为P=E[ee^T] \space R=E(\nu\nu^T)\
&=(I-K_kH)P^-_k(I-K_kH)T + K_kE(\nu_k\nu_k^T)K_kT\
&=(P^-_k-K_kHP-_k)(I-H^TK_kT)+K_kRK_k^T\
&=P^-_k - K_kHP^-_k - P^-_kHTK_k^T+K_kHP-_kH^TK_kT+K_kRK_k^T
&
\end{flalign*}
$$

推导结果是：求解K使得$P_k$最小，可以理解为$tr(p_k)$最小
$$P_k=P_k^{-}-K_{k}H{P}_k^{-}-P_k^{-}{H}^T{K}_k^T+K_{k}H{P}_k^{-}{H}^T{K}_k^T+K_{k}R{K}_k^T$$

KaTeX parse error: No such environment: flalign* at position 8: \begin{̲f̲l̲a̲l̲i̲g̲n̲*̲}̲ &(P^-_kH^TK_k^…

概率论数学基础：
$$\frac{d_{tr(AB)}}{d_A}=B^T$$

KaTeX parse error: No such environment: flalign* at position 8: \begin{̲f̲l̲a̲l̲i̲g̲n̲*̲}̲ &\frac{d_{tr(P…
因此就的出来了一个最重要的卡尔曼增益的求解：
$$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$
分析一下
$$
\begin{flalign*}
&R\uparrow:K_K\rightarrow 0 \space \hat{x}_k=\hat{x}^-_k\
&R\downarrow : K_K\rightarrow H^-\space \hat{x}_k=H^-z_k&\
&

\end{flalign*}
$$