卡尔曼滤波

最新推荐文章于 2024-08-18 10:13:28 发布

原创最新推荐文章于 2024-08-18 10:13:28 发布 · 309 阅读

0 ·

CC 4.0 BY-SA版权

机器学习算法专栏收录该内容

17 篇文章

订阅专栏

1：卡尔曼滤波是一种线性动态模型，
在这里插入图片描述
针对的也是有隐状态和观测状态的模型,和HMM不同的是：
卡尔曼滤波不要求隐状态是离散的，并且前一个状态和后一个状态满足线性关系，也叫线性高斯模型，噪声服从正态分布

前提条件也符合HMM的前提假设条件：

齐次马尔科夫假设
观测独立假设
公式满足：

卡尔曼滤波属于推断问题的一种，已知 $x_1,x_2,x_3,..x_t$ 时刻的观测状态，求 $z_t隐状态的概率：$
$p(z_t|x_1,x_2,..x_t)$
公式推导：
$p(zt∣x1,x2,..xt)=p(zt,x1,x2,..xt)p(x1,x2,..xt)p(z_t|x_1,x_2,..x_t)=\displaystyle \frac{p(z_t,x_1,x_2,..x_t)}{p(x_1,x_2,..x_t)}$
由于 $p(x_1,x_2,..x_t)$ 可以直接求出，和 $z_t$ 没有关系，这里可以视作常量
$\frac{1}{C}*p(x_1,x_2,..x_t,z_t)$
吸出 $x_t$
$\frac{1}{C}*p(x_t|x_1,x_2,..x_{t-1},z_t)p(x_1,x_2,..x_{t-1},z_t)$
应用观测独立假设和贝叶斯公式
$\frac{1}{C}*p(x_t|z_t)p(z_t| x_1,x_2,..x_{t-1})p(x_1,x_2,..x_{t-1})$
这里 $p(x_1,x_2,..x_{t-1})$ 也可以作为常量处理
$\frac{1}{C}*p(x_t|z_t)p(z_t| x_1,x_2,..x_{t-1})$
应用联合分布和边缘分布关系
$\frac{1}{C}*p(x_t|z_t) \int_{z_{t-1}}p(z_{t-1},z_{t} |x_1,x_2,..x_{t-1})dz_{t-1}$
$\frac{1}{C}*p(x_t|z_t) \int_{z_{t-1}} p(z_t|x_1,x_2,..x_{t-1},z_{t-1}) p(z_{t-1} |x_1,x_2,..x_{t-1})dz_{t-1}$
应用齐次马尔科夫假设：
$\frac{1}{C}*p(x_t|z_t) \int_{z_{t-1}} p(z_t|z_{t-1}) p(z_{t-1} |x_1,x_2,..x_{t-1})dz_{t-1}$
可以看到 $p(z_{t-1} |x_1,x_2,..x_{t-1})是t-1时刻的卡尔曼滤波$

通常记作：
$Bel(zt)=1C∗p(xt∣zt)Bel(zt)‾Bel(z_t)= \frac{1}{C}*p(x_t|z_t)\overline{Bel(z_t)}$ 叫做更新过程，也叫修正过程

$Bel(zt)‾=∫zt−1p(zt∣zt−1)Bel(zt−1)dzt−1\overline{Bel(z_t)}=\int_{z_{t-1}} p(z_t|z_{t-1}) Bel(z_{t-1})dz_{t-1}$ 叫做预测过程

2：正态分布：
一元正态分布：
$\begin{array}{l} p(x)\sim N(\mu ,{\sigma ^2}):\\ p(x) = \frac{1}{{\sqrt {2\pi } \sigma }}{e^{ - \frac{1}{2}\frac{{{{(x - \mu )}^2}}}{{{\sigma ^2}}}}} \end{array}$
多元正态分布：
$\begin{array}{l} p({{x}})\sim {\rm N}({\bf{\mu }},{{\Sigma }}):\\ p({{x}}) = \frac{1}{{{{(2\pi )}^{n/2}}{{\left| {{\Sigma }} \right|}^{1/2}}}}{e^{ - \frac{1}{2}{{({{x}} - {{\mu }})}^t}{{{\Sigma }}^{ - 1}}({{x}} - {{\mu }})}} \end{array}$
其中 $μ∈Rn,Σ∈Rn∗n是半正定方差矩阵，∣Σ∣表示方差矩阵的行列式\mu \in R^n ,\Sigma \in R^{n*n}是半正定方差矩阵，\left| {{\Sigma }} \right| 表示方差矩阵的行列式$

3：正态分布的特点：一代高斯，代代高斯

首先，高斯变量线性变换后，仍为高斯分布，均值和方差如下：
$\left. {\begin{array}{l} {X\sim N(\mu ,\Sigma )}\\ {Y = AX + B\quad \;} \end{array}} \right\}\quad \Rightarrow \quad Y\sim N(A\mu + B,A\Sigma {A^T})$
其次：两个高斯的线性组合仍然是高斯分布
$\left. {\begin{array}{l} {{X_1}\sim N({\mu _1},\sigma _1^2)}\\ {{X_2}\sim N({\mu _2},\sigma _2^2)} \end{array}} \right\} \Rightarrow k_1{X_1} +k_2 {X_2}+c \sim N\left( {{k_1\mu _1} + k_2{\mu _2}+c,k_1^2\sigma _1^2 + k_2^2\sigma _2^2} \right)$
最后，两个相互独立的高斯变量的乘积，仍然为高斯分布，均值和方差如下：
$\left. {\begin{array}{l} {{X_1}\sim N({\mu _1},\sigma _1^2)}\\ {{X_2}\sim N({\mu _2},\sigma _2^2)} \end{array}} \right\} \Rightarrow p({X_1}) \cdot p({X_2}) \sim N\left( {\frac{{\sigma _2^2}}{{\sigma _1^2 + \sigma _2^2}}{\mu _1} + \frac{{\sigma _1^2}}{{\sigma _1^2 + \sigma _2^2}}{\mu _2}, \quad \frac{{\sigma _1^2\sigma _2^2}}{{\sigma _1^2 + \sigma _2^2}}} \right)$

4: Kalman滤波:
状态方程： $zt=Azt−1+But+εtz_t=Az_{t-1}+Bu_t+\varepsilon_t$
观测方程： $xt=Hzt+δtx_t=Hz_{t}+\delta_t$
其中：
$εt∼N(0,Rt)\varepsilon_t \sim N(0,R_t)$
$δt∼N(0,Qt)\delta_t \sim N(0,Q_t)$
$z0∼N(μ0,Pt)z_0 \sim N(\mu_0,P_t)$
应用正态分布线性变换的特点有：
$p(xt∣zt)∼N(Hzt,Qt)p(x_t|z_t) \sim N(Hz_{t},Q_t)$
$p(zt∣zt−1)∼N(Azt−1+But,Rt)p(z_t|z_{t-1}) \sim N(Az_{t-1}+Bu_t,R_t)$
$p_t=AP_{t-1}A^T+R_t$

$z_t:是t时刻的状态，是n维向量$
$A_t：是n*n状态转移矩阵（在没有输入的影响下）$
$B_t：是n*n矩阵（表示u_t如何影响z_t）$
$u_t：是t时刻输入向量$
$x_t：是t时刻观测向量,m维向量$
$H_t：是m*n发射概率矩阵（z_{t} -->x_t）$
$Rt：是n∗n矩阵，表示隐状态噪声εt的方差矩阵R_t：是n*n矩阵，表示隐状态噪声\varepsilon_t 的方差矩阵$
$Qt：是m∗m矩阵，表示观测状态噪声δt的方差矩阵Q_t：是m*m矩阵，表示观测状态噪声\delta_t 的方差矩阵$
$P_t：是n*n矩阵，表示t时刻隐状态z_t的方差矩阵$

5:Kalman滤波算法的推导
1）：系统初始状态
$Bel(z0)=p(z0∣x0)∼N(μ0,P0)Bel(z_0)=p(z_0|x_0) \sim N(\mu_0,P_0)$
2）：预测过程的推导，这里列出预测过程公式：
$Bel(zt)‾=∫zt−1p(zt∣zt−1)Bel(zt−1)\overline{Bel(z_t)}=\int_{z_{t-1}} p(z_t|z_{t-1}) Bel(z_{t-1})$
因为：
$p(zt∣zt−1)∼N(Azt−1+But,Rt)p(z_t|z_{t-1}) \sim N(Az_{t-1}+Bu_t,R_t)$
$Bel(zt−1)∼N(μt−1,Pt−1)Bel(z_{t-1}) \sim N(\mu_{t-1},P_{t-1})$
所以：
$Bel(zt)‾=∫zt−1p(zt∣zt−1)Bel(zt−1)\overline{Bel(z_t)}=\int_{z_{t-1}} p(z_t|z_{t-1}) Bel(z_{t-1})$
$=∫zt−11(2π)n/2∣Rt∣1/2e−12(zt−Azt−1−But)tRt−1(zt−Azt−1−But)∗1(2π)n/2∣Pt∣1/2e−12(zt−1−μt−1)tPt−1(zt−1−μt−1)dzt−1=\int_{z_{t-1}} \frac{1}{{{{(2\pi )}^{n/2}}{{\left| {{\R_t }} \right|}^{1/2}}}}{e^{ - \frac{1}{2}{{({{z_t}} - {{Az_{t-1}-Bu_t }})}^t}{{{R_t}}^{ - 1}}({{z_t}} - {{Az_{t-1}-Bu_t}})}}* \frac{1}{{{{(2\pi )}^{n/2}}{{\left| {{P_t }} \right|}^{1/2}}}}{e^{ - \frac{1}{2}{{({{z_{t-1}}} - \mu_{t-1})}^t}{{{P_t }}^{ - 1}}({{z_{t-1}}} - \mu_{t-1})}}dz_{t-1}$

因为状态方程满足线性关系
所以有：均值更新满足
$μt=Aμt−1+But\mu_t=A\mu_{t-1}+Bu_t$
方差矩阵更新满足：
$P_t=A_tP_{t-1}A^{}$
3）更新过程的推导

$Bel(zt)=1C∗p(xt∣zt)Bel(zt)‾Bel(z_t)= \frac{1}{C}*p(x_t|z_t)\overline{Bel(z_t)}$

$\begin{array}{l} bel({z_t}) = \,\quad \frac{1}{C} \quad \,p({x_t}|{z_t})\quad \quad \quad \quad \quad \quad \overline {bel} ({z_t})\\ \quad \quad \quad \quad \quad \quad \quad \quad \Downarrow \quad \quad \quad \quad \quad \quad \quad \quad \Downarrow \\ \quad \quad \quad \quad \quad \sim N\left( {{H_t}{x_t},{Q_t}} \right)\quad \quad \sim N\left( {{{\overline \mu }_t},{{\overline P }_t}} \right)\\ \quad \quad \quad \quad \quad \quad \quad \quad \Downarrow \\ bel({z_t}) = \frac{1}{C} \;\exp \left\{ { - \frac{1}{2}{{({x_t} - {H_t}{z_t})}^T}Q_t^{ - 1}({x_t} - {H_t}{z_t})} \right\}\exp \left\{ { - \frac{1}{2}{{({z_t} - {{\bar \mu }_t})}^T}\bar P_t^{ - 1}({z_t} - {{\bar \mu }_t})} \right\}\\ \end{array}$
均值和方差的迭代公式满足(这个不知道咋推导的 ^^)
$bel({x_t}) = \left\{ {\begin{array}{l} {{\mu _t} = {{\bar \mu }_t} + {K_t}({z_t} - {H_t}{{\bar \mu }_t})}\\ {{P_t} = (I - {K_t}{H_t}){{\overline P }_t}} \end{array}} \right.\quad \quad {\rm{with }}{K_t} = {\overline P _t}H_t^T{({H_t}{\overline P _t}H_t^T + {Q_t})^{ - 1}}$