（六）卡尔曼滤波（KALMAN）

大家好，欢迎来到IT知识分享网。

一、滤波模型

1、卡尔曼滤波的状态方程：

$x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1}$

A：状态转移矩阵

B：输入关系矩阵

$w_{k-1}$ ：过程噪声向量

u：系统的输入向量

x：系统的状态向量

描述了系统状态如何随时间变化，时系统的动态模型 $x_{k}=Ax_{k-1}+w_{k-1}$ (一般将uk归到w中)

关系桥接：

由多个状态变量组成的状态向量 $x_{k}$ ，全面描述了系统在当前时刻的运行状态。值一般是未知的，但却是想要求得的。由于各个状态变量 $x_{k}$ 必须具有可观性，即它们的值直接间接的反映在对系统的观测量 $y_{k}$ 中。所以用卡尔曼滤波来从系统观测量 $y_{k}$ 来估算系统状态 $x_{k}$ ！

2、卡尔曼滤波的测量方程：

$y_{k}=Cx_{k}+v_{k}$

C：输入关系矩阵

$v_{k}$ ：测量噪声向量

$y_{k}$ ：系统观测量

$x_{k}$ ：系统状态向量

描述了系统测量值与系统状态之间的关系，C为简化起见将其视为常系数阵

过程噪声向量 $w_{k}$ →过程噪声向量的协方差矩阵

对于状态方程中的过程噪声向量 $w_{k}$ ，代表系统内部所产生的随机噪声误差，在卡尔曼滤波中假定每个过程的噪声变量是一个均值为零的正态白噪声。

$E\left ( w_{k} \right )=0$

$Cov\left ( w_{k} \right )=E\left ( w_{k}w_{k}^{T} \right )=Q$

即 $w_{k}$ 的概率分布呈N（0，Q），为过程噪声向量 $w_{k}$ 的协方差矩阵（对称阵）

测量噪声向量 $v_{k}$ →测量噪声向量的协方差矩阵

对于测量方程中的各种噪声向量 $v_{k}$ ，代表系统观测量 $y_{k}$ 所包含的随机测量误差与噪声，在卡尔曼滤波中假定每个测量噪声变量是一个均值为零的正态白噪声。

$E\left ( v_{k} \right )=0$

$Cov\left ( v_{k} \right )=E\left ( v_{k}v_{k}^{T} \right )=R$

即 $w_{k}$ 的概率分布呈N（0，R），为测量噪声向量 $v_{k}$ 的协方差矩阵（对称阵）

同时假定过程噪声向量 $w_{k}$ 的各分量与测量噪声向量 $v_{k}$ 的各分量互不相关！（i、j为任一有效历元）

$E\left ( w_{i}v_{j}^{T} \right )=0$

二、滤波算法推理

1、先验估计误差 ${e}_{k}^{-}$ → 先验状态均方误差阵 ${P}_{k}^{-}$

对系统状态进行最优估计，满足最小均方误差（MMSE）

$\hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1}$

$\hat{x}_{k}^{-}$ ：先验估计值（ $\hat{x}_{k}^{-}$ 还未得到 $y_{k}$ 的验证）-：先验 ^:估计值

先验估计误差 ${e}_{k}^{-}$ ：为系统状态的真实值 ${x}_{k}$ 与其先验估计值 ${x}_{k}^{-}$ 的差异

${e}_{k}^{-} ={x}_{k}-{\hat{x}}_{k}^{-}$

先验状态均方误差阵 ${P}_{k}^{-}$ ：先验估计误差 ${e}_{k}^{-}$ 的协方差阵 ${P}_{k}^{-}$ 称为状态均方误差阵，可表示为：

${P}_{k}^{-}=E\left ( {e}_{k}^{-}{e}_{k}^{-} {_{}}^{T}\right )$

观测量残余 $r_{k}$ ：有了先验估计值 $\hat{x}_{k}^{-}$ ，将其与实际观测值 $y_{k}$ 建立关系， $r_{k}$ 就相当于测量方程里面的 $v_{k}$ ，在这里将 $r_{k}$ 作为观测量残余。

$r_{k}=y_{k}-C\hat{x}_{k}^{-}$

2、后验估计误差 ${e}_{k}$ → 后验状态均方误差阵 ${P}_{k}$

随后卡尔曼滤波将先验估计值 $\hat{x}_{k}^{-}$ 和观测量残余 $r_{k}$ 的线性组合作为对状态 $x_{k}$ 的最优估计值 $\hat{x}_{k}$ 即

$\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}r_{k}=\hat{x}_{k}^{-}+K_{k}\left (y_{k}-C\hat{x}_{k}^{-}\right )$

$K_{k}$ ：卡尔曼滤波增益

$\hat{x}_{k}$ ：后验估计值（ $y_{k}$ 已经核对、校正了估计值 $\hat{x}_{k}$ ）-：先验 ^:估计值

后验估计误差 ${e}_{k}$ ：为系统状态的真实值 ${x}_{k}$ 与其后验估计值 $\hat{x}_{k}$ 的差异

${e}_{k}={x}_{k}-{\hat{x}}_{k}$

后验状态均方误差阵 ${P}_{k}$ ：后验估计误差 ${e}_{k}$ 的协方差阵 ${P}_{k}$ 称为状态均方误差阵，可表示为：

${P}_{k}=E\left ( {e}_{k}{e}_{k}{_{}}^{T}\right )$

注：后验估计状态均方误差阵 ${P}_{k}$ 的各个对角线元素分别对应于各个状态变量估计值的均方误差。

SQR（ ${v}_{*}$ ）

3、推导卡尔曼滤波增益 $K_{k}$ 最优值 →状态均方误差阵 ${P}_{k}$ 对角线元数之和最小

a、建立后验估计误差 ${e}_{k}$ 与先验估计误差 ${e}_{k}^{-}$ 、卡尔曼滤波增益 $K_{k}$ 、测量噪声向量 $v_{k}$ 、输入关系矩阵C的关系

————即各分量的估计值的均方误差最小，使得状态方程处于最优解

① $y_{k}=Cx_{k}+v_{k}$

② ${e}_{k}^{-} ={x}_{k}-{\hat{x}}_{k}^{-}$

③ $\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}r_{k}=\hat{x}_{k}^{-}+K_{k}\left (y_{k}-C\hat{x}_{k}^{-}\right )$

④ ${e}_{k}={x}_{k}-{\hat{x}}_{k}$ (将①-③带入④)

得到：

${e}_{k}=\left ( I-{K}_{k}C \right ){e}_{k}^{-}-{K}_{k}{v }_{k}$

b、联合上式建立后验状态均方误差阵 ${P}_{k}$ 与先验状态均方误差阵 ${P}_{k}^{-}$ 、卡尔曼滤波增益 $K_{k}$ 、测量噪声向量的协方差矩阵、输入关系矩阵C的关系

① $Cov\left ( v_{k} \right )=E\left ( v_{k}v_{k}^{T} \right )=R$

② ${P}_{k}^{-}=E\left ( {e}_{k}^{-}{e}_{k}^{-} {_{}}^{T}\right )$

③ ${e}_{k}=\left ( I-{K}_{k}C \right ){e}_{k}^{-}-{K}_{k}{v }_{k}$

④ ${P}_{k}=E\left ( {e}_{k}{e}_{k}{_{}}^{T}\right )$ (将①-③带入④)

得到后验状态均方误差阵 ${P}_{k}$ 与先验状态均方误差阵 ${P}_{k}^{-}$ 的关系：

${P}_{k} = {P}_{k}^{-}-{K}_{k}C{P}_{k}^{-}-\left ( {K}_{k}C{P}_{k}^{-} \right )^{T}+ {K}_{k}\left ( C{P}_{k}^{-}C^{T}+R\right ){K}_{k}^{T}$

在此方程中假设先验估计误差 ${e}_{k}$ 与当前历元的测量噪声向量 $v_{k}$ 之间互不相关！

c、利用后验状态均方误差阵 ${P}_{k}$ 和卡尔曼滤波增益 $K_{k}$ 之间的函数关系，求当 ${P}_{k}$ 最小时 $K_{k}$ 的值

对上式 ${P}_{k}$ 的对角线元素之和进行求导，结合以下求导公式：

① $\frac{d\left ( tr\left ( FG \right ) \right )}{dF}=G^{T}$

② $\frac{d\left ( tr\left ( FHF^{T} \right ) \right )}{dF}=2FH$

FGH可为任意矩阵，FG必须为方阵，H必须为对称阵。tr：求迹

${P}_{k}$ 或 tr( ${P}_{k}$ )是一个关于 $K_{k}$ 的二次型函数,且 $\left ( C{P}_{k}^{-}C^{T}+R \right )$ 正定，所以关于 $K_{k}$ 的tr( ${P}_{k}$ )存在最小值，tr( ${P}_{k}$ )对 $K_{k}$ 求偏导，使其导数为零得到使 tr( ${P}_{k}$ )值最小的 $K_{k}$ 的解

$\frac{d\left ( tr\left ( {P}_{k} \right ) \right )}{d{K}_{k}}=-2\left ( C{P}_{k}^{-} \right )^{T}+2{K}_{k}\left ( C{P}_{k}^{-} C^{T}+R\right )\equiv0$

解的：

$K_{k}={P}_{k}^{-}C^{T}\left ( C{P}_{k}^{-} C^{T}+R\right )^{-1}$

d、将求得的当 ${P}_{k}$ 最小时 $K_{k}$ 的值，带回到 ${P}_{k}$ 原方程里可得 ${P}_{k}$ 和 ${P}_{k}^{-}$ 间的关系：

① $K_{k}={P}_{k}^{-}C^{T}\left ( C{P}_{k}^{-} C^{T}+R\right )^{-1}$

② ${P}_{k} = {P}_{k}^{-}-{K}_{k}C{P}_{k}^{-}-\left ( {K}_{k}C{P}_{k}^{-} \right )^{T}+ {K}_{k}\left ( C{P}_{k}^{-}C^{T}+R\right ){K}_{k}^{T}$ (将①带入②)

得到 $K_{k}$ 取最优解时后验状态均方误差阵 ${P}_{k}$ 与先验状态均方误差阵 ${P}_{k}^{-}$ 之间的关系：

$P_{k}=\left (I-{K}_{k}C\right )P_{k}^{-1}$

三、滤波算法整体的应用

1、预测

$\hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1}$

${P}_{k}^{-}=A{P}_{k-1}A^{T}+Q$

——————————————————————————————————————————

${P}_{k}^{-}=A{P}_{k-1}A^{T}+Q$ 中，由于过程噪声值 $w_{k}$ 未知，因而状态先验估计值的均方误差阵计算公式中添加了过程噪声的协方差Q，以降低状态先验估计值的可靠性和报此状态均方误差阵的正确性。通常再进行了这一过程后，状态均方误差阵会变大————后续进行校正更新

——————————————————————————————————————————

2、更新

$K_{k}={P}_{k}^{-}C^{T}\left ( C{P}_{k}^{-} C^{T}+R\right )^{-1}$

$\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}r_{k}=\hat{x}_{k}^{-}+K_{k}\left (y_{k}-C\hat{x}_{k}^{-}\right )$

$P_{k}=\left (I-{K}_{k}C\right )P_{k}^{-}$

———————————————————————————————————————————

在求出令先验状态均方误差阵 ${P}_{k}^{-}$ 的迹最小的卡尔曼滤波增益 $K_{k}$ 后，使得各分量的均方误差最小，即最优解。随即将 $K_{k}$ 带带入到先验估计值和后验估计值的关系方程中，求出后验估计值 $\hat{x}_{k}$ ，对估计值进行更新。随后在计算出后验状态均方误差阵 ${P}_{k}$ ， ${P}_{k}$ → ${P}_{k-1}$ 用于下一状态的先验状态均方误差阵 ${P}_{k}^{-}$ 的赋值 ${P}_{k}^{-}=A{P}_{k-1}A^{T}+Q$

———————————————————————————————————————————

在RTKLIB代码中（A→F，C→H）：

x=F*x, P=F*P*F+Q <预测>

K=P*H*(H’*P*H+R)^-1, xp=x+K*v, Pp=(I-K*H’)*P <校正>

———————————————————————————————————————————

卡尔曼滤波器可提供系统状态的无偏估计zhu

参考GPS原理与接收机设计

免责声明：本站所有文章内容,图片，视频等均是来源于用户投稿和互联网及文摘转载整编而成，不代表本站观点，不承担相关法律责任。其著作权各归其原作者或其出版社所有。如发现本站有涉嫌抄袭侵权/违法违规的内容,侵犯到您的权益，请在线联系站长,一经查实,本站将立刻删除。本文来自网络,若有侵权，请联系删除，如若转载，请注明出处：https://haidsoft.com/144708.html

（六）卡尔曼滤波（KALMAN）

一、滤波模型

1、卡尔曼滤波的状态方程：

2、卡尔曼滤波的测量方程：

二、滤波算法推理

三、滤波算法整体的应用

1、预测

2、更新

相关推荐

发表回复