KF
根据贝叶斯估计的原理,卡尔曼滤波是利用已知系统模型的确定性特性和统计特性等先验知识与观测量获得最有估计,在有初始值的情况下,从先验值和最新观测数据中得到的新值的加权平均来更新状态估计。
1、卡尔曼滤波中的关键要素
状态向量及其协方差、系统模型、观测向量及其协方差、观测模型和滤波算法。
2、卡尔曼滤波中的误差与噪声
功率谱密度:单位带宽内的功率,也是单位带宽内的方差。
滤波过程中误差主要有3种:系统噪声,高斯-马尔可夫过程,白噪声。
白噪声序列(white noise sequence) 是零均值不相关随机变量构成的离散时间序列。白噪声各个频率功率谱密度为常数,高斯白噪声的功率谱密度服从均匀分布,幅度分布服从高斯分布。白噪声序列的方差为:
σ
w
2
=
τ
w
S
w
\sigma_w^2=\tau_wS_w
σw2=τwSw式中
S
w
S_w
Sw为白噪声过程的功率谱密度(power spectral density , PSD) ,即单位带宽内的方差。
高斯一马尔可夫过程(Gauss - Markov processes) 是随时间变化的量,是其先前值和白噪声序列的线性函数
一阶高斯马尔可夫
卡尔曼滤波基本假设:卡尔曼滤波的基本假设是:系统中模型化的误差为系统误差、白噪声或高斯-马尔可夫过程,也可以是它们的线性组合。卡尔曼滤波中假设所有未被建模的误差源均为白噪声,这对卡尔曼滤波非常重要,原因如图保证转换传播过程中依旧是高斯分布
需要用到的一些误差概念:
状态向量残差(state vector residual) 是真实状态向量与其卡尔曼滤波估
计之间的差
δ
x
=
x
−
x
^
\delta x=x-\hat x
δx=x−x^
误差协方差矩阵(error covariance matrix) P 定义为状态估计与真值间偏差平方的期望值。
P
=
E
(
(
x
^
−
x
)
(
x
^
−
x
)
T
)
=
E
(
δ
x
δ
x
T
)
P=E((\hat x-x)(\hat x-x)^T)=E(\delta x\delta x^T)
P=E((x^−x)(x^−x)T)=E(δxδxT)
P矩阵的对角线元素是各个状态估计的方差,而它们的平方根是不确定度。
状态初始化值构成了先验估计值,而初始协方差矩阵值代表状态设置的置信度。
观测新息(measurement innovation)
δ
z
−
\delta z^-
δz−是实际观测向量与观测更新之前、由状态估计计算得来的观测向量之间的差值:
δ
z
−
=
z
−
H
(
x
^
−
)
\delta z^-=z-H(\hat x^-)
δz−=z−H(x^−)
观测残差(measurement residual)
δ
z
+
\delta z^+
δz+,表示实际观测量与观测更新之后、由状态向量计算得到的观测量估计值之间的差值:
δ
z
+
=
z
−
H
(
x
^
+
)
\delta z^+=z-H(\hat x^+)
δz+=z−H(x^+)
3、系统模型介绍
依据状态的动力学方程,来求得到系统矩阵和系统噪声分布矩阵,卡尔曼滤波要求每个状态的时间导数是其他状态和白噪声的线性函数。这样,在t 时刻卡尔曼滤波的实际状态向量x(t) 都可由下面动态模型来描述:
x
˙
(
t
)
=
F
(
t
)
x
(
t
)
+
G
(
t
)
W
s
(
t
)
\dot{x}(t)=F(t)x(t)+G(t)W_s(t)
x˙(t)=F(t)x(t)+G(t)Ws(t)
Φ
k
−
1
=
e
x
p
(
F
k
−
1
t
)
\Phi_{k- 1}= exp(F_{k-1}t)
Φk−1=exp(Fk−1t)
式中:
w
s
(
t
)
w_s(t)
ws(t) 为系统噪声向量 ;F(t) 为系统矩阵 ;G(t) 是连续系统噪声分布矩阵
近似解:
离散状态下
x
k
=
Φ
k
−
1
x
k
−
1
+
Γ
k
−
1
w
s
,
k
−
1
x_k = \Phi_{k-1} x_{k-1} + \Gamma_{k-1} w_{s,k-1}
xk=Φk−1xk−1+Γk−1ws,k−1
式中:
w
s
,
k
−
1
w_{s,k-1}
ws,k−1为离散系统的噪声向量;
Γ
k
−
1
\Gamma_{k-1}
Γk−1 为离散系统噪声分布矩阵
Q
k
−
1
=
E
[
Γ
k
−
1
w
s
,
k
−
1
w
s
,
k
−
1
T
Γ
k
−
1
T
]
Q_{k-1} = E [ \Gamma_{k-1} w_{s,k-1} w_{s,k-1}^T\Gamma_{k-1}^T]
Qk−1=E[Γk−1ws,k−1ws,k−1TΓk−1T]
4、观测模型
观测向量
z
(
t
)
z(t)
z(t) 一般被模型化为真实状态向量
x
(
t
)
x(t)
x(t) 和白噪声源
w
m
(
t
)
w_m(t)
wm(t) 的线性函数,即
Z
k
=
H
k
x
k
+
w
m
k
Z_k=H_k x_k+w_{mk}
Zk=Hkxk+wmk
观测噪声协方差矩阵( measurement noise covariance matrix) R , 即观测噪声平方的期望值:
R
=
E
(
w
m
w
m
T
)
R = E( w_mw_m^T)
R=E(wmwmT)的对角线是每个观测量的方差,非对角线项代表不同观测噪声之间的相关性
5、算法流程
(1)计算状态转移矩阵
Φ
k
−
1
\Phi_{k-1}
Φk−1
(2)计算系统噪声矩阵
Q
k
−
1
Q_{k-1}
Qk−1
(3)状态向量传播
x
^
k
−
=
Φ
k
−
1
x
^
k
−
1
+
\hat{x}^-_k=\Phi_{k-1}\hat{x}^+_{k-1}
x^k−=Φk−1x^k−1+
(4)误差协方差矩阵传播
P
^
k
−
=
Φ
k
−
1
x
^
k
−
1
+
Φ
k
−
1
T
+
Q
k
−
1
\hat{P}^-_k=\Phi_{k-1}\hat{x}^+_{k-1}\Phi^T_{k-1}+Q_{k-1}
P^k−=Φk−1x^k−1+Φk−1T+Qk−1
(5)计算观测矩阵
H
k
H_k
Hk
(6)计算噪声协方差矩阵
R
k
R_k
Rk
(7)计算卡尔曼增益矩阵
K
k
=
P
k
−
1
H
k
T
(
H
k
P
k
H
k
T
+
R
k
)
−
1
K_k=P_{k-1} H_k^T (H_kP_kH_k^T + R_k) ^{-1}
Kk=Pk−1HkT(HkPkHkT+Rk)−1
(8)构建观测向量
δ
Z
k
=
Z
k
−
H
k
x
^
k
−
\delta Z_{k}=Z_k-H_k\hat{x}^-_k
δZk=Zk−Hkx^k−
(9)状态向量估计值的更新
x
^
k
+
=
x
^
k
−
+
K
δ
Z
k
\hat x^+_k=\hat x^{-}_k+K\delta Z_{k}
x^k+=x^k−+KδZk
(10)误差协方差矩阵的更新
P
k
+
=
(
I
−
K
k
H
k
)
P
k
−
1
P^+_k= (I - K_kH_k)P_{k-1}
Pk+=(I−KkHk)Pk−1
对应公式:
6、卡尔曼滤波稳定性
卡尔曼滤波初始时更加依赖于观测量,增益矩阵较大,逐渐收敛的过程中更加依赖于系统预测,卡尔曼滤波设计得很好时,由误差协方差矩阵定义的状态不确定度的降低,会伴随着对应的状态残差的降低。这时,卡尔曼滤波是收敛的。反之,设计得不好时,会导致状态不确定度比状态残差小很多,甚至不确定度下降时残差还变大,这种现象就是发散。
7、闭环卡尔曼滤波
卡尔曼滤波得到的误差估计值,在每次迭代中或按固定时间间隔反馈回去,用于校正系统本身,使过程中卡尔曼滤波状态趋于零值。卡尔曼滤波算法中,反馈状态估计的最佳时机,是在观测更新后立即进行。反馈过程中,会在真实和估计的状态上,加上或减去相同的量,因而误差协方差矩阵P 不受此反馈过程的影响,所以误差协方差传播
8、序贯更新
卡尔曼滤波的序贯观测更新,也称为标量观测更新或序贯处理,是指在迭代
过程中,同一时刻只用观测向量的一个元素。
前提是各元素互相独立即R矩阵为对角阵(不独立也能计算,但是需要进行对角化分解)
更新的增益矩阵
更新计算
9、参数调整和算法稳定性
卡尔曼滤波的参数调整是指,由设计者或使用者为三个矩阵选值的过程。这些矩阵包括系统噪声协方差矩阵
Q
k
Q_k
Qk 、观测噪声协方差矩阵
R
k
R_k
Rk以及误差协方差矩阵初始值
P
0
+
P_0^+
P0+卡尔曼滤波中的关键参数是误差协方差矩Pk和观测噪声协方差矩阵Rk的比值,因为它们决定了卡尔曼增益矩阵,
P
/
R
P/R
P/R越大 ,卡尔曼增益越大,越依赖于观测值,越有可能出现滤波不稳甚至发散,
P
/
R
P/R
P/R越小,卡尔曼增益越小,收敛速度越慢。
10、算法设计
观测更新时间间隔有时候会非常大,以至于不能计算转移矩阵,这是因为只有当系统传播间隔t足够小时,才可将系统矩阵F作为常数。卡尔曼滤波算法可以有四种不同的迭代速率:状态传播、误差协方差传播、观测累积和观测更新
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)