广义高斯滤波-高斯滤波器,gauss-hermite卡尔曼滤波器,容积卡尔曼滤波器
高斯滤波器
要将不同的高斯逼近统一为非线性变换,可进行如下的高斯积分逼近计算:
要是上述积分可以计算,那么就可以直接进行分布矩匹配,来对
x
,
y
\bm{x,y}
x,y的逼近。
噪声可叠加型高斯滤波器
叠加变换通式:(后面的叠加变换中省略)
叠加变换的高斯矩匹配:
基于矩匹配高斯逼近的
x
,
y
\bm{x,y}
x,y分布如下:
叠加噪声的高斯(卡尔曼)滤波器:
预测步骤,利用系统动态模型:
f
(
⋅
)
\bm{f(\cdot)}
f(⋅)计算预测均值和预测协方差:
更新步骤,利用系统量测模型:
h
(
⋅
)
\bm{h(\cdot)}
h(⋅)计算预测均值,l量测量预测协方差,状态量与量测量之间的互协方差和滤波增益,滤波均值与方差。(各个变量计算公式按照顺序给出)
噪声不可叠加型高斯滤波器
非爹在变换的通式:(后面的非叠加变换中省略)
非叠加变换的高斯矩匹配:
基于矩匹配高斯逼近的
x
,
y
\bm{x,y}
x,y的联合部分为:
非叠加噪声高斯(卡尔曼)滤波器的预测与更新过程:
(所需要计算的量与叠加噪声滤波器一致)
Guass-hermite 卡尔曼滤波器
一维Guass-Hermite积分是指对函数和单位权值函数的积求积分的特例,其逼近形式为:
其中,
W
i
\bm{W_i}
Wi是权值,
x
(
i
)
\bm{x^{(i)}}
x(i)为估计点或横坐标,有时也称为
s
i
g
m
a
\bm{sigma}
sigma点。
选择权值和
s
i
g
m
a
\bm{sigma}
sigma点的准则是构造
H
e
r
m
i
t
e
\bm{Hermite}
Hermite多项式进行逼近。如下所示:
其中,
p
\bm{p}
p是阶数。
在相同的权值和
s
i
g
m
a
\bm{sigma}
sigma点下,非单位高斯权数函数
N
(
x
∣
m
,
p
)
\bm{N(x|m,p)}
N(x∣m,p)的积分,可通过换元法变为单位高斯权函数计算,其表达式为:
Gauss-Hermite积分算法
设一维积分的表达式为:
以上是对一维
p
\bm{p}
p阶Guass-Hermite逼近;
1).计算 [ H e r m i t e ] \bm[Hermite] [Hermite]多项式的根 [ ξ ( i ) ] , i = 1 , ⋅ ⋅ ⋅ , p \bm[\xi^{(i)}],i=1,\cdot\cdot\cdot,p [ξ(i)],i=1,⋅⋅⋅,p,将其作为单位 s i g m a \bm{sigma} sigma点。这里将三对角矩阵的特征值作为根,并未构建多项式并计算多项式的根。
2).计算权值:
3). 积分近似为:
Gauss-Hermite容积算法
设多维积分的表达式为:
1). 先计算出一维权值
W
i
\bm{W_{i}}
Wi和单位
s
i
g
m
a
\bm{sigma}
sigma点,
ξ
(
i
)
,
i
=
1
,
⋅
⋅
⋅
,
p
\bm\xi^{(i)},i=1,\cdot\cdot\cdot,p
ξ(i),i=1,⋅⋅⋅,p。
2). 利用一维权值和一维单位
s
i
g
m
a
\bm{sigma}
sigma点计算多维权值和多维单位
s
i
g
m
a
\bm{sigma}
sigma点。3). 积分近似为:
叠加型 G a u s s − H e r m i t e \bm{Gauss-Hermite} Gauss−Hermite卡尔曼滤波(GHKF)算法:
预测步骤:
1). 选取
s
i
g
m
a
\bm{sigma}
sigma点:
2). 将
s
i
g
m
a
\bm{sigma}
sigma点带入动态模型:
3). 计算预测均值和预测协方差:
更新步骤:
1). 选取
s
i
g
m
a
\bm{sigma}
sigma点:
2). 将
s
i
g
m
a
\bm{sigma}
sigma点带入量测模型:
3). 计算预测均值,量测量的预测协方差和状态量和量测量之间的互协方差:
4). 结合量测量
y
k
\bm{y_k}
yk,计算滤波增益,滤波均值和协方差。
非叠加类型与叠加类型一致,但对于Guass-Hermite积分运算的增广状态量维数增加,在计算上较复杂。
容积卡尔曼滤波器
球面容积积分:
多维积分表达式为:
则多维积分三阶球面逼近算法为:
1). 计算单位
s
i
g
m
a
\bm{sigma}
sigma点:
其中,
e
i
\bm{e_i}
ei为坐标轴
i
\bm{i}
i轴上的单位向量。
2). 逼近积分为:
这种积分逼近为无迹变换在参数
α
=
1
,
β
=
0
,
κ
=
0
\bm{\alpha=1,\beta=0,\kappa=0}
α=1,β=0,κ=0的一个特例。
(噪声)叠加型容积卡尔曼滤波器CKF
预测步骤:
1). 构造
s
i
g
m
a
\bm{sigma}
sigma点:
其中,
s
i
g
m
a
\bm{sigma}
sigma点定义式为:
2). 将
s
i
g
m
a
\bm{sigma}
sigma点带入动态方程:
3). 计算预测均值,预测协方差:
更新步骤:
1). 构造
s
i
g
m
a
\bm{sigma}
sigma点:
2). 将
s
i
g
m
a
\bm{sigma}
sigma点带入量测方程:
3). 计算预测均值,量测量的预测协方差和状态量与量测量之间的互协方差:
4). 结合量测量,计算滤波器增益,滤波均值和协方差:
(噪声)非叠加型容积卡尔曼滤波
预测步骤:
1). 对增广矩阵
(
x
k
−
1
,
q
k
−
1
)
\bm{(x_{k-1},q_{k-1})}
(xk−1,qk−1)构造
s
i
g
m
a
\bm{sigma}
sigma点:
这里,
n
′
=
n
+
n
q
\bm{n^{'}=n+n_q}
n′=n+nq,为状态量与噪声的维数之和。
s
i
g
m
a
\bm{sigma}
sigma点的定义为:
2). 将
s
i
g
m
a
\bm{sigma}
sigma点带入动态方程:
这里,
χ
~
k
−
1
(
i
)
,
x
\bm{\widetilde{\chi}^{(i),x}_{k-1}}
χ
k−1(i),x,
χ
~
k
−
1
(
i
)
,
q
\bm{\widetilde{\chi}^{(i),q}_{k-1}}
χ
k−1(i),q分别是
χ
~
k
−
1
(
i
)
\bm{\widetilde{\chi}^{(i)}_{k-1}}
χ
k−1(i)的前
n
\bm{n}
n项和后
n
q
\bm{n_q}
nq项。
3). 计算预测均值和预测协方差:
更新步骤:
1). 对增广变量
(
x
k
,
r
k
)
\bm{(x_k,r_k)}
(xk,rk)构造
s
i
g
m
a
\bm{sigma}
sigma点:
这里,
n
′
′
=
n
+
n
r
\bm{n^{''}=n+n_r}
n′′=n+nr,为状态量的维数和量测噪声的维数之和。单位
s
i
g
m
a
\bm{sigma}
sigma点
ξ
(
i
)
′
′
\bm{\xi^{(i)^{''}}}
ξ(i)′′的定义与预测过程的一致,只是将
n
′
′
\bm{n^{''}}
n′′代替
n
′
\bm{n^{'}}
n′。
2). 将
s
i
g
m
a
\bm{sigma}
sigma点带入量测方程:
这里,
χ
~
k
−
(
i
)
,
x
\bm{\widetilde{\chi}^{-(i),x}_{k}}
χ
k−(i),x,
χ
~
k
−
(
i
)
,
q
\bm{\widetilde{\chi}^{-(i),q}_{k}}
χ
k−(i),q分别是
χ
~
k
−
(
i
)
\bm{\widetilde{\chi}^{-(i)}_{k}}
χ
k−(i)的前
n
\bm{n}
n项和后
n
r
\bm{n_r}
nr项。
3). 计算预测均值,量测量的预测协方差和量测量与状态量的互协方差:
4). 结合量测量
y
k
\bm{y_k}
yk,计算滤波器增益,滤波状态均值和协方差:
本章总结到此结束,上