卡尔曼滤波器1——递归算法(笔记篇 + 代码实现)
前言
本文是观看DR_CAN老师的视频后,简单总结了一下的笔记,并根据思路写了示例代码;这里主要讲卡尔曼滤波器与递归算法。
视频地址:https://www.bilibili.com/video/BV1ez4y1X7eR
目录
一、卡尔曼滤波器
二、引言案例 测量硬币尺寸
三、卡尔曼公式
四、卡尔曼增益 Kalman Gain
五、更新估计误差
六、卡尔曼计算流程
七、实例应用卡尔曼算法
八、卡尔曼滤波-递归算法 Python版
一、卡尔曼滤波器
卡尔曼滤波器,Kalmen Filter;可理解为是一种算法:最优化 递归 数字处理 算法。它更像一种观测器,而不是一般意义的滤波器。卡尔曼滤波器应用非常广泛,主要是因为很多事物存在不确定性,不确定性体现在三个方面:
- 不存在完美的数学模型;
- 系统存在扰动,或很难建模;
- 测量传感器存在误差。
二、引言案例 测量硬币尺寸
不同的人员,测量同一个硬币,测量结果用表示,其中k是表示第k次测量;这里产生误差或不确定地方有:尺子本身的误差、不同人员测试的。
第一次测量结果, = 50.1mm;
第二次测量结果, = 50.4mm;
第三次测量结果, = 50.2mm;
如果让我们“估计真实数据”,自然会想到取三次结果的平均值。估计值,用平均值表示:
注释:红色部分是k-1次的平均值,
重新整理后得到: ,其代表的含义是:
本次的估计值 = 上一次的估计值 + 系数*(本次测量值 - 上一次的估计值)
观察一下k变化的影响,k是测量的次数:
当k很大时, 趋于0,这时估计值,即:测试的次数很多后,本次的估计值,和上一次的估计值很接近了。
当k很小时, 趋于1,这时估计值 ,即:测试的次数很少后,本次的估计值,取本次测量值更准确一些。
三、卡尔曼公式
综上所得,卡尔曼公式如下,含义也简单解释一下:
本次的估计值 = 上一次的估计值 + 系数*(本次测量值 - 上一次的估计值)
其中,Kalman Gain,表示卡尔曼增益/因素
通过公式可以看出来,本次的估计值,与上一次的估计值有关。然后上一次的估计值,由与上上次的估计值有关;这是一种递归的思想。
这也是卡尔曼滤波器的优势,它不需要追溯很久以上的数据,只需拿上一次的数据就可以了。
四、卡尔曼增益 Kalman Gain
如何计算这个卡尔曼增益,这里给出一个公式,后面有机会再详细解释和推导。在此之前,首先介绍两个概念:
估计误差,其中e是指error,误差的意思。EST是指Estimate,估计的意思。
测量误差,MEA是指Measure,测量的意思。真实值和测量值之间的误差。
卡尔曼增益公式如下:(小k是指计算次数;k-1是k的上一次。)
卡尔曼增益 = 上一次的估计误差 除以 (上一次的估计误差 + 本次的测量误差)
CSDN的公式编辑中,编辑不了多重下标,贴一下老师的手写的公式,大家对比一下:
五、更新估计误差
估计误差在每次计算中,需要更新的,具体的公式如下:
公式含义:本次的估计误差 = (1 - 卡尔曼增益)* 上一次的估计误差
六、卡尔曼计算流程
首先计算卡尔曼增益,然后计算本次的估计值,最后更新估计误差。
第一步:卡尔曼增益 = 上一次的估计误差 除以 (上一次的估计误差 + 本次的测量误差)
第二步:本次的估计值 = 上一次的估计值 + 系数*(本次测量值 - 上一次的估计值)
第三步:本次的估计误差 = (1 - 卡尔曼增益)* 上一次的估计误差
如果错误请指出,谢谢;欢迎多多交流。
七、实例应用卡尔曼算法
前面讲了这么多理论的,可能不是太清晰,下面用测量硬币的例子,观察一下卡尔曼滤波的魅力。
硬币实际长度:50mm
用尺子手动测量误差是:3mm (即:测出来的结果范围在47mm——53mm之间)
假设模型M能估计硬币的长度
首先让模型M估计一个结果,比如:认为硬币只有40mm;估计误差可能是5mm。
按照卡尔曼算法流程,首先计算卡尔曼增益,然后计算本次的估计值,最后更新估计误差。 即:
画一个表格,观察预测值,和手动测量值变化:
k | 测量值 | 测量误差 | 卡尔曼增益 | 估计值 | 估计误差 |
0 | 40 | 5 | |||
1 | 52 | 3 | 0.625 | 47.7 | 1.875 |
2 | 51 | 3 | 0.385 | 48.847 | 1.153 |
3 | 47 | 3 | 0.278 | 48.334 | 0.832 |
4 | 52 | 3 | 0.217 | 49.13 | 0.651 |
5 | 49 | 3 | 0.178 | 49.10 | 0.535 |
能看虽然手动测量不稳定,存在3mm的误差,但经过卡尔曼滤波后,输出的估计逐渐趋于真实值50,而且也比较稳定。
k =1;第一次手动测量为52mm
k =2;第二次手动测量为51mm
后面的可以自己推算一下。
八、卡尔曼滤波-递归算法 Python版
根据 卡尔曼滤波的流程,用Python写了一个代码,对应上面测量硬币的案例:
'''
卡尔曼滤波器--递归算法
Author: guopu
date: 2021-8-23
'''k = 0 # 计算次数msa = 0 # 手动测量结果
e_MEA = 0.0 # 手动测量误差X_k0 = 0.0 # 上一次估计结果
X_k = 0.0 # 本次估计结果e_EST0 = 0.0 # 上一次估计误差
e_EST = 0.0 # 本次估计误差KK = 0.0 # 卡尔曼增益'''
卡尔曼增益 = 上一次的估计误差 除以 (上一次的估计误差 + 本次的测量误差)
'''
def kalman_gain(e_EST0, e_MEA):return e_EST0/(e_EST0 + e_MEA)'''
本次的估计值 = 上一次的估计值 + 系数*(本次测量值 - 上一次的估计值)
'''
def now_estimated_value(X_k0, kk, msa):return X_k0 + kk*(msa - X_k0)'''
本次的估计误差 = (1 - 卡尔曼增益)* 上一次的估计误差
'''
def now_estimated_error(KK, e_EST0):return (1 - KK)*e_EST0# 初始化数据
true_Length = 50.0 # 硬币的真实长度
msa_list = [52, 51, 47, 52, 49] # 5次的手动测量结果
e_MEA = 3.0 # 手动测量误差
X_k = 40.0 # 初始估计结果
e_EST = 5.0 # 初始估计误差X_list = [] # 用来存放估计值for k in range(0, len(msa_list)):KK = kalman_gain(e_EST, e_MEA)X_k = now_estimated_value(X_k, KK, msa_list[k])e_EST = now_estimated_error(KK, e_EST)print("\n k:", k+1)print("测量结果:", msa_list[k])print("估计结果:", X_k)X_list.append(X_k)print("\n真实长度", true_Length)
print("\n5次测量结果:", msa_list)
print("\n5次估计结果:", X_list)
print("\n")
代码效果:
本文只提供参考和学习,谢谢。
卡尔曼滤波器1——递归算法(笔记篇 + 代码实现)相关推荐
- 七.卡尔曼滤波器开发实践之七: 无损卡尔曼滤波器(UKF)进阶-实例篇
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式 二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器 三.卡尔曼滤波器(EK ...
- 卡尔曼滤波器2——数据融合(笔记篇 + 代码实现)
前言 本文是观看DR_CAN老师的视频后,简单总结了一下的笔记,并根据思路写了示例代码:这里主要讲使用卡尔曼滤波器进行数据融合. 视频地址:[卡尔曼滤波器]2_数学基础_数据融合_协方差矩阵_状态空间 ...
- OpenCV‘s Kalman filter卡尔曼滤波器的实例(附完整代码)
OpenCV's Kalman filter卡尔曼滤波器的实例 OpenCV's Kalman filter卡尔曼滤波器的实例 OpenCV's Kalman filter卡尔曼滤波器的实例 #inc ...
- 六.卡尔曼滤波器开发实践之六: 无损卡尔曼滤波器(UKF)进阶-白话讲解篇
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式 二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器 三.卡尔曼滤波器(EK ...
- 【滤波】多元卡尔曼滤波器
%matplotlib inline #format the book import book_format book_format.set_style() 简介 我们现在准备研究和实现完整的.多元形 ...
- 四.卡尔曼滤波器(EKF)开发实践之四: ROS系统位姿估计包robot_pose_ekf详解
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式 二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器 三.卡尔曼滤波器(EK ...
- 一. 卡尔曼滤波器开发实践之一: 五大公式详解
既然标题名称是开发实践,本系列文章将主要介绍如何在工程实践中使用卡尔曼滤波器,至于卡尔曼滤波器的五大公式如何推导而来,网上有很多大拿们写的都很精彩,这里不再叙述.可以参考了下面两篇博文: 1. 卡尔曼 ...
- 五.卡尔曼滤波器(EKF)开发实践之五: 编写自己的EKF替换robot_pose_ekf中EKF滤波器
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式 二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器 三.卡尔曼滤波器(EK ...
- 卡尔曼滤波与组合导航原理_图解卡尔曼滤波器,无需深厚的数学知识也易懂(第五部分:多维卡尔曼滤波器)...
译者前言:因第1-4部分(基础)已经学习完毕,接下来需要学习多维卡尔曼滤波器与扩展的卡尔曼滤波器(EKF),但是1-4部分的作者对接下来要学习的内容还没有更新,因此我们将从华盛顿大学教授的教材开始学习 ...
最新文章
- c语言增强,C语言提高-day5
- Python3爬虫之爬取某一路径的所有html文件
- word rectangle
- 1029:计算浮点数相除的余
- input文本框自动填充背景色黄色解决办法
- MVC的初步认识理论
- java中RIGHT的值_java循环遍历, reduce(),reduceRight()方法和Object,keys遍历对象的属性...
- python对于设计师有什么用-如果你有设计师朋友,请对TA好一些...
- matlab bwdist
- linux 0891端口,凯立德导航专用版本号及分辨率、端口、波特率
- mdx 医学词典_一些西医学方面词典的介绍
- Ftp站点访问及FileZilla客户端使用
- Win10 虚拟桌面
- 自制homekit开关
- 没有音响,把手机当作电脑音响的操作。
- CSS的三种布局方式
- 组合类和友员类的差异有哪些(C++)
- 点云配准 3- icp-交互式ICP点云配准
- 【GlobalMapper精品教程】021:利用控制点校正栅格图像
- 【机器学习】p3客户细分中的参考资料skewed distribution