有关卡尔曼滤波的文章非常多,我在这里就只写写学习卡尔曼滤波器的时候遇到的坑。
什么是滤波器
所谓滤波就是从混合在一起的诸多信号中提取出所需要的信号。
所谓估计就是根据测量得出的与状态
X
(
t
)
X(t)
X(t)有关的数据
Z
(
t
)
=
H
X
(
t
)
+
V
(
t
)
Z(t)=HX(t)+V(t)
Z(t)=HX(t)+V(t)解算出
X
(
t
)
X(t)
X(t)的值。估计的方法有很多,但每个方法都有一个准则。例如最小二乘估计的指标是:使各次量测
Z
i
Z_i
Zi与由估计
X
X
X确定的量测的估计
Z
=
H
X
Z=HX
Z=HX之差的平方和最小。
而卡尔曼滤波估计是最小方差估计,
量测方程
看了三天的姿态融合,才明白量测方程,这个坑还挺大的。
看网上的通俗的理解,有一个估计房间温度的例子,需要一个自己的估计值和一个温度计的测量值,两个值都不太准,所以需要卡尔曼滤波综合考虑每个值,最后给出一个最优估计值。
这个例子其实很好,但是总给人一种感觉,就是我想要估计的状态量必须也能测量
Z
=
X
Z=X
Z=X,也就是我必须能够测量温度,来当做量测值。但这其实是不对的,实际情况下,我要估计的值并不能量测出来,不信你看量测方程
Z
(
t
)
=
H
X
(
t
)
+
V
(
t
)
Z(t)=HX(t)+V(t)
Z(t)=HX(t)+V(t)这里还有个
H
H
H,如果
H
H
H不是单位矩阵,那么
Z
Z
Z和
X
X
X就不相等。
拿惯导融合做例子:
INS和GNSS融合时,状态量是
Δ
X
\Delta X
ΔX,而量测值是
X
I
−
X
G
X_I-X_G
XI−XG,这时你可能就不理解了,其实,真正的量测值是
Δ
X
I
−
Δ
X
G
\Delta X_I-\Delta X_G
ΔXI−ΔXG,这个很容易推导的,那为什么是
Δ
X
I
−
Δ
X
G
\Delta X_I-\Delta X_G
ΔXI−ΔXG呢,因为
Δ
X
I
−
Δ
X
G
\Delta X_I-\Delta X_G
ΔXI−ΔXG,可以用
Δ
X
\Delta X
ΔX线性表示,不信可以看
H
H
H中的具体值是什么。到这里如果还想继续深入理解,就只能看量测方程的推导了。
状态量
又遇到了两个坑,都是状态量的理解,主要还是大
F
F
F矩阵没有理解,导致的理解偏差。
状态量有15个,包含:速度偏差、位置偏差、姿态角偏差、陀螺仪偏差、加速度计偏差。
- 速度和位置偏差。读代码的时候发现,每次调用卡尔曼滤波,速度和位置偏差的初值都设为0,表示完全不理解,不应该是用上一次的数据吗?
原来,每次计算完误差,都会去校正,所以就认为:校正完之后,位置和速度是准确的,既然是准确的,那么位置和速度的偏差当然是0啦。 - 姿态角、陀螺仪、加速度计的偏差都是单次的,所以计算误差要累加。
参考
- 卡尔曼滤波器介绍-Greg Welch, Gary Bishop