【滤波】一维卡尔曼滤波器
本文主要翻译自rlabbe/Kalman-and-Bayesian-Filters-in-Python的第4章节04-One Dimensional Kalman Filters（一维卡尔曼滤波器）。
%matplotlib inline
#format the book
import book_format
book_format.set_style()
现在，我们了解了离散贝叶斯滤波器和高斯，我们准备实现一个卡尔曼滤波器。我们将和离散贝叶斯滤波器章节一样完成这项工作，根据对问题的推理一步一步地编写代码。
一维意味着滤波器只跟踪一个状态量，例如xxx轴上的位置。因此，在本文中，我们将学习如何使用高斯函数来实现贝叶斯滤波器。这就是卡尔曼滤波的全部：一种使用高斯的贝叶斯滤波。
问题描述
和离散贝叶斯滤波器章节一样，我们跟踪正在走廊中移动的狗。假设有人发明了一个RFID跟踪器，它提供了一个相当准确的狗的位置。传感器以mmm为单位，返回狗距离走廊最左端的距离。所以，23.4意味着狗距离走廊左端23.4m23.4m23.4m。
当然，传感器不完美。读数为23.4，可能狗的真实位置是23.7或23.0。然而，它不太可能对应于47.6的位置。传感器是合理准确的，虽然有误差，但误差很小。并且，有误差的观测值均匀分布在真实位置的两侧。也许我们可以用高斯函数来模拟。
我们对狗的位置进行预测，当然这个预测也不完美。有时我们的预测会超出狗的实际位置，有时会不及狗的实际位置。我们预测虽然也有偏差，但也不会很大。也许我们也可以用高斯函数来模拟。
高斯概率
我们可以用高斯曲线，来表示我们对狗位置的概率。假设我们觉得狗在10m10m10m处，且方差是1m21m^{2}1m2，那么可以简写成N(10,1)N(10, 1)N(10,1)。概率密度函数pdf为：
import filterpy.stats as statsstats.plot_gaussian_pdf(mean=10., variance=1., xlim=(4, 16), ylim=(0, .5))
这个图描述了我们对狗位置的不确定性。它表示狗的位置可能不太准确，虽然我们认为狗最有可能在10m10m10m处，但从9m9m9m到11m11m11m也很有可能。假设狗静止不动，我们再次用传感器进行观测。这一次传感器的数值是10.2m10.2m10.2m。我们能用这个额外的信息来改进我们的估计吗？
直觉告诉我们可以。考虑一下：如果我们读取传感器500次，每次它返回的值都在8到12之间，且都以10为中心，那么我们应该非常确信狗的位置接近10。当然，可能有不同的解释。也许我们的狗是随机游荡来回的方式，完全随机模拟高斯分布。但这似乎是极不可能的——我从来没有见过狗这样做。让我们看一下N(10,1)N(10, 1)N(10,1)的500次的结果：
import numpy as np
from numpy.random import randn
import matplotlib.pyplot as pltxs = range(500)
ys = randn(500)*1. + 10.
plt.plot(xs, ys)
print(f'Mean of readings is {np.mean(ys):.3f}')
Mean of readings is 10.021
然而，有噪声的传感器数据看起来确实是这样的，读数均值的计算结果几乎正好是10。假设狗站着不动，我们可以说狗在位置10，方差为1。
高斯概率跟踪
离散贝叶斯滤波器使用概率直方图来跟踪狗。直方图中的每个直方列代表一个位置，该列对应的值是狗处于该位置的概率。
跟踪是通过不断地预测和更新的循环来执行的。我们用了下列方程式计算新的概率分布：
{xˉ=x∗fx(∙)Predictx=L⋅xˉCorrect\left\{\begin{matrix} \bar{\mathbf{x}} = \mathbf{x}*f_{\mathbf{x} }(\bullet ) & Predict \\ \mathbf{x} = \mathcal{L}\cdot \bar{\mathbf{x}} & Correct \end{matrix}\right.{xˉ=x∗fx(∙)x=L⋅xˉPredictCorrect
回想一下，xˉ\bar{\mathbf{x}}xˉ是先验值，L\mathcal{L}L是观测值和先验值xˉ\bar{\mathbf{x}}xˉ的似然，fx(∙)f_{\mathbf{x} }(\bullet )fx(∙)是过程模型，∗*∗表示卷积。x\mathbf{x}x是粗体的，表示它是一个向量，是直方图的所有可能性的值，而非是单独的值。
这种方法是可行的，但最终结果可能会表现出狗可以同一时间在多个地方（多峰）。而且，对于大型问题，计算速度非常慢。
那我们能用高斯N(μ,σ2)N(\mu, \sigma^{2})N(μ,σ2)代替直方图x\mathbf{x}x吗？当然可以！我们已经学会了如何用高斯函数来表达概率。高斯分布是一个单独的数对N(μ,σ2)N(\mu, \sigma^{2})N(μ,σ2)，可以代替整个概率直方图：
import kf_book.kf_internal as kf_internalkf_internal.gaussian_vs_histogram()
我们可以用一对数字替换成百上千个数字：x=N(μ,σ2)x = N(\mu, \sigma^{2})x=N(μ,σ2)。
高斯分布的尾部在两侧延伸到无穷远，因此它在直方图中包含无数列。如果这代表了我们对狗在走廊中位置的概率，那么这个高斯分布覆盖了整个走廊（甚至这个走廊延申出的整个宇宙）。我们可能认为狗的位置在10，也可能在8、14，或者以无穷小的概率，在108010^{80}1080。
在本文中，我们将直方图替换为高斯：
discrete BayesGaussianStepxˉ=x∗f(x)xˉN=xN⊕fxN(∙)Predictx=∣L⋅xˉ∣xN=L⊗xˉNUpdate\begin{array}{l|l|c} \text{discrete Bayes} & \text{Gaussian} & \text{Step}\\ \hline \bar {\mathbf {x}} = \mathbf{ x} \ast f(\mathbf {x}) & \bar {x}_{\mathcal{N}} = x_{\mathcal{N}} \oplus f_{{x_{\mathcal{N}}}}(\bullet) & \text{Predict} \\ \mathbf x = |\mathcal {L} \cdot \bar{\mathbf {x}}| & x_{\mathcal{N}} = L \otimes \bar{x}_{\mathcal{N}} & \text{Update} \end{array}discrete Bayesxˉ=x∗f(x)x=∣L⋅xˉ∣GaussianxˉN=xN⊕fxN(∙)xN=L⊗xˉNStepPredictUpdate
其中，⊕\oplus⊕和⊗\otimes⊗表示高斯上的某个未知算子。
如果进行类比：离散贝叶斯滤波器采用卷积进行预测，它使用全概率公式，最终计算出一个和，所以也许我们可以进行高斯相加；它使用乘法将观测值合并到先验值中，所以也许我们可以进行高斯相乘。能这么简单吗：
xˉ=?x+fx(∙)x=?L⋅xˉ\begin{aligned} \bar x &\stackrel{?}{=} x + f_x(\bullet) \\ x &\stackrel{?}{=} \mathcal L \cdot \bar x \end{aligned}xˉx=?x+fx(∙)=?L⋅xˉ
只有当两个高斯的和、积都是另一个高斯时，这才有效。否则在第一个循环之后，xxx就不是高斯的，这个方案就无法继续下去了。
高斯预测
我们使用牛顿运动方程，根据当前速度和先前位置计算当前位置：
xˉk=xk−1+vkΔt=xk−1+fx\bar{x}_{k} = x_{k-1} + v_{k} \Delta t = x_{k-1} + f_{x}xˉk=xk−1+vkΔt=xk−1+fx
我放弃了fx(∙)f_{x}(\bullet )fx(∙)的符号，转而使用fxf_{x}fx来保持方程的整洁。
如果当前狗在10m10m10m处，它的速度是15m/s15m/s15m/s，时间步长是2s2s2s，我们有：
xˉk=xk−1+fx=10+(15×2)=40\bar{x}_{k} = x_{k-1} + f_{x} = 10 + (15 \times 2) = 40xˉk=xk−1+fx=10+(15×2)=40
其实，它当前的位置和速度都不是很精确的值，所以单纯这样计算可不行。我们需要用高斯函数来表示不确定性。
位置很容易，我们把xxx定义为高斯函数。如果我们认为狗在10m10m10m处，距离不确定度的标准差是0.2m0.2m0.2m，我们得到x=N(10,0.22)x = N(10, 0.2^{2})x=N(10,0.22)。
我们对它行动的不确定性怎么描述？我们将fxf_{x}fx定义为高斯函数。如果狗的速度是15m/s15m/s15m/s，时间是1s1s1s，速度不确定度的标准偏差是0.7m/s0.7m/s0.7m/s，我们得到fx=N(15,0.72)f_{x} = N(15, 0.7^{2})fx=N(15,0.72)。
先验值的计算公式是：
xˉ=x+fx\bar{x} = x + f_{x}xˉ=x+fx
两个高斯的和是多少？在上一章我证明了：
μ=μ1+μ2\mu = \mu_{1} + \mu_{2}μ=μ1+μ2
σ2=σ12+σ22\sigma ^{2} = \sigma _{1}^{2} + \sigma _{2}^{2}σ2=σ12+σ22
这是个好消息：两个高斯的和就是另一个高斯！
数学上是可行的，但这有直觉意义吗？想想这个抽象方程的物理表示。我们有：
x=N(10,0.22)x = N(10, 0.2^{2})x=N(10,0.22)
fx=N(15,0.72)f_{x} = N(15,0.7^{2})fx=N(15,0.72)
利用之前的公式，我们得到：
xˉ=μx+μfx=25\bar{x} = \mu_{x} + \mu_{f_{x}} = 25xˉ=μx+μfx=25
σˉ2=σx2+σfx2=0.53\bar{\sigma}^{2} = \sigma _{x}^{2} + \sigma _{f_{x}}^{2} = 0.53σˉ2=σx2+σfx2=0.53
预测的位置是前一个位置加上移动距离，这是有意义的。方差呢？很难对此形成直觉。但是，回想一下离散贝叶斯滤波器的predict()函数，如果预测过程存在噪声，我们总是会丢失信息。我们更不知道狗移动到哪里，所以概率应该变小（方差变大）。σfx2\sigma _{f_{x}}^{2}σfx2是由于对运动的不完全预测而增加了系统中的不确定性，因此我们将把它加到现有的不确定性中。
让我们利用Python的collection模块中的namedtuple类来实现一个Gaussian对象。当然，我们也可以使用一个tuple实现一个Gaussian，其中N(10,0.04)N(10, 0.04)N(10,0.04)在Python中可以表示为g=(10,0.04)g = (10, 0.04)g=(10,0.04)。我们用g[0]取均值，用g[1]取方差。
namedtuple的工作原理与tuple相同，除了一点，namedtuple可以添加类型名和字段名。理解这一点并不重要，但我修改了__repr__方法，使用本文中的符号来显示它的值。
from collections import namedtuplegaussian = namedtuple('Gaussian', ['mean', 'var'])
gaussian.__repr__ = lambda s: '
【滤波】一维卡尔曼滤波器相关推荐
- 卡尔曼滤波器(6) -- 一维卡尔曼滤波器(例5完整模型)
This blog is translated from https://www.kalmanfilter.net/default.aspx. It's an excellent tutorial a ...
- Udacity机器人软件工程师课程笔记（三十二) - 卡尔曼滤波器 - 一维卡尔曼滤波器 - 多维卡尔曼滤波器 - 拓展卡尔曼滤波器（EKF）
卡尔曼滤波器 一.概述 二.一维高斯分布 均值和方差 三.一维卡尔曼滤波器 变量命名约定 卡尔曼滤波循环 1.测量值更新 (1)平均值计算 (2)程序实现 2.位置预测 位置预测公式 3.一维卡尔曼滤 ...
- 卡尔曼滤波器(8) -- 一维卡尔曼滤波器(例7例8)
This blog is translated from https://www.kalmanfilter.net/default.aspx. It's an excellent tutorial a ...
- android程序如何滤波,android – 卡尔曼滤波器：如何使用它没有“状态转换模型”？...
我正在从Android手机上研究加速度计.我希望过滤加速度计正在返回的可怕噪音,记录手机的动作. 我正在读卡尔曼滤波器,因为低通是不够的. 但我没有从ACCELERATION(k-1)到ACCELER ...
- android 陀螺仪滤波_android – 卡尔曼滤波器 – 指南针和陀螺仪
我正在尝试用陀螺仪,加速度计和万用表构建罗盘. 我正在将acc值与测量值融合以获得方向(使用旋转矩阵)并且它工作得非常好. 但现在我想添加陀螺仪,以便在磁传感器不准确时进行补偿.因此,我想使用卡尔曼滤 ...
- 卡尔曼滤波器简介——多维卡尔曼滤波
原文:多维卡尔曼滤波 (kalmanfilter.net) 目录 前言 基本背景 状态外推方程 示例 - 飞机 - 无控制输入 示例 - 带控制输入的飞机 示例 – 坠落物体 状态外推方程维度 线性时 ...
- 卡尔曼滤波器(4) -- α−β−γ滤波器(例3例4总结)
This blog is translated from https://www.kalmanfilter.net/default.aspx. It's an excellent tutorial a ...
- 卡尔曼滤波器(3) -- α−β−γ滤波器(例2)
This blog is translated from https://www.kalmanfilter.net/default.aspx. It's an excellent tutorial a ...
- 卡尔曼滤波器的特殊案例
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 什么是卡尔曼滤波器? 卡阿尔曼滤波器为每个结果状态找到最佳的平均因 ...
- 卡尔曼滤波器(2) -- α−β−γ滤波器(例1)
This blog is translated from https://www.kalmanfilter.net/default.aspx. It's an excellent tutorial a ...
最新文章
- DCMTK：OFStandard类中的ASCII /双转换方法的测试代码
- Strom+Kafka + redis实时计算单词出现频率的案例
- jQuery源码解析（1）—— jq基础、data缓存系统
- RocketMq发送延迟消息
- linux常用分区大小,Linux基本知识点总结——硬盘分区及LVM
- 计算机三级嵌入式系统
- 烟花php,基于HTML5 canvas的逼真烟花特效插件jquery.fireworks.js
- imageAI基本使用
- 账号权限问题导致数据泄露频发，如何破解“万豪们”的安全难题？
- windows之批处理读取注册表，得到我的文档默认路径
- 电脑桌面不显示此电脑或是计算机,电脑怎么显示出此电脑？此电脑显示的设置方法...
- iOS必备小技巧（非常的全）
- 办理营业执照注册要什么费用
- java: java mina ——基于TCP/IP、UDP/IP协议栈的通信框架
- 基于区块链的知识共享框架-Aletheia
- 【Python】字符串转换为ASCII码
- teamlab什么意思_去看炸火的teamLab大型个展前 你应该知道的事
- 谈谈唯一约束和唯一索引
- 哥德巴赫猜想计算机验证进展,哥德巴赫猜想的最新进展，不知道下文证明的是否正确，求验证...
- 【720科技实训之产品经理】产品经理视野入门之对市场几种社交软件的分析（一）
热门文章