[Probabilistic robotics] Karman卡尔曼滤波

目录

• 内容说明
• 基础概念
• 例子
• 黄金称重
• 跟踪恒速飞机
• 跟踪加速飞机
• 一维卡尔曼滤波
• 卡尔曼增益
• 不确定性
• 总结
• 估算建筑物高度
• 过程噪音
• 例子
• 估计罐中液体的温度
• 估计加热液体的温度
• 多维卡尔曼滤波器
• 概念
• 数学
• 状态外推方程
• 协方差外推
• 测量方程
• 状态更新方程
• 协方差更新方程
• 卡尔曼增益

内容说明

本文内容为https://www.kalmanfilter.net/default.aspx的学习记录
感谢原作者

基础概念

精度与准度

测量值与真实值

例子

黄金称重

经过数学推导可得


factor α = 1/N ，也写作K
随着N的增加而减少。这意味着，一开始，我们没有足够的重量值信息，因此我们将估算值基于测量值。继续进行下去，由于1/N减少。在某些时候，新测量的贡献可以忽略不计。

跟踪恒速飞机

跟踪加速飞机

一维卡尔曼滤波

卡尔曼增益

卡尔曼增益告诉您通过测量可以更改多少估计值。

如您所见，卡尔曼增益 （K_n） 是我们赋予测量的权重。并且 （1-K_n） 是我们赋予估计的权重。

当测量不确定度很大而估计不确定度很小时，卡尔曼增益接近于零。因此，我们对估计值给予较大的权重，而对测量值给予较小的权重。

另一方面，当测量不确定度很小且估计不确定度很大时，卡尔曼增益接近于1。因此，我们对估计值给予较小的权重，而对测量值给予较大的权重。

不确定性

当测量不确定度较大时，则卡尔曼增益将较低，因此，估计不确定度的收敛速度将很慢。但是，当测量不确定度较小时，则卡尔曼增益将很高，估计不确定度将迅速收敛至零。

总结

估算建筑物高度

1、初始化
x^_0,0=60
人眼误差σ=15 ，p_0,0=σ² = 225

2、预测
x^_1,0=x_0,0
p_1,0=p_0,0

3、第一次迭代
①测量
z₁=48.54
测量误差σ=5 ，r₁=σ² = 25
②更新
K₁=p_1,0 / (p_1,0+r₁) =0.9
x^_1,1= x^{~1,0~+K~1~(z~1~−x}_1,0) = 49.69
p_1,1= (1−K₁)p_1,0 = 22.5

4、预测
x^_2,1=x_1,1
p_2,1=p_1,1

5、第二次迭代
…………

过程噪音

在现实世界中，系统动力学模型存在不确定性。

例如，当我们要估计电阻器的电阻值时，我们假定为恒定动态模型，即在两次测量之间电阻不变。但是，由于环境温度的波动，电阻可能会略有变化。当用雷达跟踪弹道导弹时，动力学模型的不确定性包括目标加速度的随机变化。

另一方面，当我们使用GPS接收器估算静态物体的位置时，动态模型的不确定性为零，因为静态物体不会移动。动态模型的不确定性称为过程噪声。在文献中，它也称为工厂噪声，行驶噪声，动力学噪声，模型噪声和系统噪声。过程噪声产生估计误差。

例子

估计罐中液体的温度

T是恒温，w_n是具有方差q的随机过程噪声
我们认为我们有一个准确的模型，因此我们将过程噪声方差（q）设置为0.0001

计算同上一个，只是预测p的公式改变了

估计加热液体的温度

尽管实际系统动力学不是恒定的（因为液体正在加热），但我们将系统视为具有恒定动力学（温度不变）的系统
计算同上

卡尔曼滤波器估计中存在滞后误差
导致滞后误差的原因有两个：
1、动态模型不适合这种情况。
2、流程模型的可靠性。
我们选择了非常低的过程噪声（q= 0.0001） 而实际的温度波动要大得多。

由于我们的过程是没有很好地界定，我们将增加不确定性的过程（q） 从0.0001到0.15

我们可以通过设置较高的过程不确定性来消除滞后误差。但是，由于我们的模型定义不明确，因此我们得到的噪声估计几乎等于测量值，因此无法实现卡尔曼滤波器的目标。

最佳的卡尔曼滤波器实施方案应包括非常接近实际的模型，并为过程噪声留出很小的空间。但是，精确模型并不总是可用，例如，飞机驾驶员可以决定执行突然的操作，这将改变预测的飞机轨迹。在这种情况下，应增加过程噪声。

多维卡尔曼滤波器

概念

协方差

时不变系统

数学

状态外推方程

主要过程

一维推广多维：

矩阵指数可以通过泰勒级数展开来计算：

更普遍的形式：

协方差外推

推导过程

测量方程

推导过程

状态更新方程

协方差更新方程

推导过程

卡尔曼增益

推导过程