卡尔曼滤波

第一章知识点回顾

表1变量符号对照表

1.1数学期望

数学期望表示为每次可能的结果乘上结果概率的总和。

1.1.1 数学期望的性质

假设常数为 C ,随机变量 X Y ,则

1.2 方差(variance

概率论中和统计中的方差反映单个(一维)随机变量的离散程度即随机变量偏离数学期望的幅度大小,方差越大偏离数学期望的幅度越大。

总体方差:

离散型随机变量的方差

 1.2.1 方差的性质 

假设常数为A和B,随机变量X和Y,则

1.3 协方差(Covariance

协方差反映两个(二维)随机变量的相似程度,当两个变量相同时,协方差为方差。“协”是指几个变量的协同相关性。

随机变量XY的协方差表示为

如果一个变量大于该变量的期望,另一个变量也大于自身的期望,则两个变量的协方差为正值,正相关。

如果一个变量小于该变量的期望,另一个变量也小于自身的期望,则两个变量的协方差为负值,负相关。

如果一个变量等于该变量的期望,另一个变量也等于自身的期望,则两个变量的协方差为零,不相关,即两个 随机变量相互独立。

 1.3.1协方差的性质

假设常数为A和B,随机变量X和Y,且X和Y不是相互独立的,则

1.4协方差矩阵

协方差只能两个(二维)随机变量,三个(三维)及以上随机变量需要计算多个协方差,用矩阵形式表示就是协方差矩阵。协方差矩阵是一个对称非负定矩阵。

二维随机变量X和Y的协方差矩阵表示为

三维随机变量X、Y和Z的协方差矩阵表示为

1.4.1协方差矩阵的性质

假设矩阵为A,向量为b,随机变量X、Y和Z,则

 1.5高斯分布

高斯分布也称为正态分布常态分布,很多信号噪声和数据都服从高斯分布。高斯分布在线性变换后仍然是高斯分布。高斯分布的期望、平均数、中位数和众数相同。

随机变量X服从高斯分布,记为

当期望为0,标准差为1时,高斯分布为标准高斯分布

高斯分布只要确定了期望和方差就可以得到概率密度函数,表示为

期望决定高斯分布位置,标准差决定高斯分布的幅度。离期望越近概率越大,离期望越远概率越小。标准差越大分布越分散,标准差越小分布越集中。

1.5.1高斯分布的性质

假设实数为A和B,独立高斯随机变量X和Y,则

1.6微分法矩阵求导

1.6.1矩阵转置的性质

1.6.2矩阵微分运算法则 

 1.6.3矩阵迹的性质

1.6.4矩阵求导求解步骤

  1. 根据给定的f寻找df。
  2. 给df套上迹Tr,并进行化简处理。
  3. 根据导数与微分的关系得到结果。

1.6.5迹函数对矩阵求导

下面两个例子迹函数对矩阵A求导,根据求解步骤计算。

根据矩阵微分运算法则得到

根据导数和微分的关系

第二章卡尔曼滤波

2.1卡尔曼滤波定义

卡尔曼滤波是一种状态观测器、是一种最优化估计算法、是一种传感器数据融合算法

针对线性系统采用卡尔曼滤波,针对非线性系统采用扩展卡尔曼滤波。区别在于扩展卡尔曼滤波多了一个把预测和测量部分进行线性化的过程。

卡尔曼滤波假设所有变量和噪声都服从高斯分布卡尔曼滤波由两种变量构成,一种是由数学模型或经验得出的估计(预测)状态变量,另一种是由传感器测量得到的测量(观测)变量,这两种变量都是带误差的变量,卡尔曼滤波将估计状态变量和测量变量进行数据融合,根据上一时刻的变量值循环往复,不停迭代逼近真实的数据,即根据上一时刻的最优估计值预测当前时刻的最优估计值,并使用当前时刻的传感器测量值修正估计值得到最优估计值。

卡尔曼滤波的优点是将不准确的估计值和不准确的测量值做数据融合得到最优估计值,不需要很久之前的数据,只需要上一时刻的数据,运行速度快,占用系统内存小。

2.2卡尔曼滤波公式推导

 表2变量符号对照表

注:表2中的先验估计值是不完整的估计,需要用测量值修正处理;后验估计值是卡尔曼滤波输出的最优估计值;观测值是用传感器测量的测量值。

2.2.1递归思想

在时刻1,2,…,k分别测量到数据z1,z2,…,zk,在k时刻的估计值为

当测量数据的足够多时,k趋于无穷,测量值zk不再重要,估计值趋于上一次的估计值。但是当测量数据较少时,k趋于0,测量值zk很重要。

得到卡尔曼增益  

当前估计值和上一时刻估计值有关,上一时刻估计值又和上上一次估计值有关,这就是卡尔曼滤波的递归思想,只需要上一时刻的数据就可以估计出当前时刻的值。

估计误差

测量误差

卡尔曼增益

当k-1时刻的估计误差远大于k时刻的测量误差时,卡尔曼增益趋于1,k时刻估计值趋于k时刻的测量值。

当k-1时刻的估计误差远小于k时刻的测量误差时,卡尔曼增益趋于0,k时刻估计值趋于k-1时刻的估计值。

2.2.2卡尔曼公式

 2.2.3卡尔曼增益推导

系统的状态方程

系统的观测方程

过程噪声和测量噪声都是不可以直接测量的,假设过程噪声和测量噪声都服从高斯分布,过程噪声的概率密度函数是期望为0,协方差矩阵为Q;测量噪声的概率密度函数是期望为0,协方差矩阵为R。

过程噪声协方差矩阵Q的期望计算如下,同理测量噪声协方差矩阵R。

根据方差和协方差定义,且期望为0。

得到

在求解时,系统的状态方程去掉过程噪声,得到一个算出来卡尔曼预测公式—先验估计值(不完整的估计,需要用测量值修正处理)。

系统的观测方程去掉测量噪声,根据实际测量值zk,得到一个测出来的估计值

卡尔曼滤波器根据上面的不准确的计算出来的先验估计值和不准确的测出来的估计值计算出后验估计值(注意:后验估计值就是最优估计值)。下面公式根据递归和数据融合思想得到。

得到卡尔曼校正公式—后验估计

目标是计算卡尔曼增益Kk使后验估计值趋于真实值。Kk与过程噪声和测量噪声有关。引入真实值和后验估计值误差ek,ek的概率密度函数是期望为0,协方差为P的高斯分布。

当后验估计值估计越准确即越接近于真实值,ek的方差越小,协方差矩阵Pk的迹Tr(Pk)越小。目标是计算Kk使Tr(Pk)最小。

ek的协方差矩阵Pk

将后验估计和系统的观测方程代入协方差矩阵,这里使用真实测量值zk引入测量噪声进行估计。

 令

得到

 计算协方差矩阵的第2项

同理,协方差矩阵的第3项也为0。

目标是使协方差矩阵Pk的tr(Pk)最小,从而使误差ek的方差最小。

上式等号右边第二项

由互为转置的矩阵迹相同,得到

 计算迹在Kk的变化下最小值

 根据矩阵微分运算法则和矩阵迹的性质,迹函数对矩阵求导得到卡尔曼校正公式—卡尔曼增益

当测量噪声R很大时

当测量噪声R很小时

2.2.4更新误差协方差矩阵推导

误差协方差矩阵带入卡尔曼增益得到卡尔曼校正公式—更新误差协方差矩阵

2.2.5先验误差协方差矩阵推导

先验误差

带入真实值和先验估计值

 先验误差协方差矩阵定义为

判断上式中独立的项

 化简得到卡尔曼预测公式—先验误差协方差矩阵

2.3卡尔曼滤波仿真

仿真文件上传到博客中,可以下载进行调试学习。

 2.4卡尔曼滤波参数调整

2.4.1参数调整方法

通过Simulink仿真可以得到卡尔曼滤波参数调整的方法,可以用于嵌入式系统控制算法中。

Q和R不能为0。当Q等于0时,后验估计等于初值。当R等于0时,系统更相信测量值,后验估计等于测量值。

卡尔曼增益是由Q和R共同作用的,不能单独调整某一个值。同时增大Q和R相同倍数,后验估计变化不大。

减小Q,增大R,卡尔曼增益变小,后验估计收敛速度变慢,振荡小、滞后大、更稳定、更相信估计值;增大Q,减小R,卡尔曼增益变大,后验估计收敛速度快、振荡大、不稳定、更相信测量值。

2.4.2选择Q和R的数值

Q数值的选择,使用数学模型的误差或由实验测得。R数值的选择,使用传感器给定的误差或者由实验测得。

因为卡尔曼增益是由Q和R共同作用的,所以将Q数值设置为固定值,根据实际应用中传感器的变化调整R数值。

2.4.3初值设置

H初值可以设置为1。

xk初值可以设置为0。

Pk初值一般设置为1,不能设为0。多次迭代后,Pk值就趋向稳定值。

参考文献

学习B站DR_CAN的系列视频、百度百科、CSDN和知乎一些大神的博客。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/91055.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

Java进阶篇--进程和线程的区别

进程和线程 进程 在一个操作系统中,每个独立执行的程序都可称之为一个进程,也就是“正在运行的程序”。目前大部分计算机上安装的都是多任务操作系统,即能够同时执行多个应用程序,最常见的有Windows、Linux、Unix等。比如在Wind…

S波形及鱼眼扭曲源码

三角波形扭曲&#xff1a; void sinwave(cv::Mat& src,cv::Mat& dst) {dst.create(src.rows, src.cols, CV_8UC3);dst.setTo(0);src.copyTo(dst);int PI 3.1415;int RANGE dst.cols/2;for (int i 0; i < dst.rows; i) {double temp (dst.cols - RANGE) / 2 (d…

Git,分布式版本控制工具

1.为常用指令配置别名&#xff08;可选&#xff09; 打开用户目录&#xff0c;创建.bashrc文件 &#xff08;touch ~/.bashrc&#xff09; 2.往其输入内容 #用于输出git提交日志 alias git-loggit log --prettyoneline --all --graph --abbrev-commit #用于输出当前目录所有文…

意外发现Cortex-M内核带的64bit时间戳,比32bit的DWT时钟周期计数器更方便,再也不用担心溢出问题了

视频&#xff1a; https://www.bilibili.com/video/BV1Bw411D7F5 意外发现Cortex-M内核带的64bit时间戳&#xff0c;比32bit的DWT时钟周期计数器更方便&#xff0c;再也不用担心溢出问题了 介绍&#xff1a; 看参数手册的Debug章节&#xff0c;System ROM Table里面带Timestam…

首轮征稿 | 2024年第二届先进无人飞行系统国际会议(ICAUAS 2024)

会议简介 Brief Introduction 2024年第二届先进无人飞行系统国际会议(ICAUAS 2024) 会议时间&#xff1a;2024年4月5日-7日 召开地点&#xff1a;中国武汉 大会官网&#xff1a;ICAUAS 2024-2024 2nd International Conference on Advanced Unmanned Aerial Systems 由华中科技…

软件测试框架实战:Python+Slenium搭建Web自动化测试框架全教程

PythonSelenium是一种流行的Web自动化测试框架&#xff0c;可以模拟真实的用户操作&#xff0c;对网页进行功能和样式的验证。要通过selenium测试网页&#xff0c;需要以下几个步骤&#xff1a; 安装selenium库和浏览器驱动 。 使用selenium提供的方法来控制浏览器窗口大小、后…

图像检索,目标检测map的实现

一、图像检索指标Rank1,map 参考&#xff1a;https://blog.csdn.net/weixin_41427758/article/details/81188164?spm1001.2014.3001.5506 1.Rank1: rank-k&#xff1a;算法返回的排序列表中&#xff0c;前k位为存在检索目标则称为rank-k命中。 常用的为rank1&#xff1a;首…

老人摔倒智能识别检测算法

老人摔倒智能识别检测算法通过yolov8深度学习算法模型架构&#xff0c;老人摔倒智能识别检测算法能够实时监测老人的活动状态及时发现摔倒事件&#xff0c;系统会立即触发告警&#xff0c;向相关人员发送求助信号&#xff0c;减少延误救援的时间。YOLOv8 算法的核心特性和改动可…

对于pycharm 运行的时候不在cmd中运行,而是在python控制台运行的情况,如何处理?

对于pycharm 运行的时候不在cmd中运行&#xff0c;而是在python控制台运行的情况&#xff0c;如何处理&#xff1f; 比如&#xff0c;你在运行你的代码的时候 它总在python控制台运行&#xff0c;十分难受 解决方法 在pycharm中设置下即可&#xff0c;很简单 选择运行点击…

XSS攻击与防御

目录 一、环境配置 kali安装beef contos7安装dvwa 二、XSS攻击简介 三、XSS攻击的危害 四、xSS攻击的分类 五、XSS产生的原因 六、构造XSS攻击脚本 (一)基础知识 常用的html标签 常用的js脚本 (二)构造脚本的方式弹窗警告 七、自动XSS攻击 (一)BeEF简介 (二)BeEF…

亲测influxdb安装为window后台服务

InfluxDB 安装 64bit&#xff1a;https://dl.influxdata.com/influxdb/releases/influxdb-1.7.4_windows_amd64.zip 解压安装包 修改配置文件 [meta]# Where the metadata/raft database is storeddir "D:/influxdb/meta"...[data]# The directory where the TSM…

容器内执行命令

上篇文章向读者介绍了一个Nginx的例子&#xff0c;对于Nginx这样一个容器而言&#xff0c;当它启动成功后&#xff0c;我们不可避免的需要对Nginx进行的配置进行修改&#xff0c;那么这个修改要如何完成呢&#xff1f;且看下文。 依附容器 docker attach 依附容器这个主要是…

3D姿态相关的损失函数

loss_mpjpe: 计算预测3D关键点与真值之间的平均距离误差(MPJPE)。 loss_n_mpjpe: 计算去除尺度后预测3D关键点误差(N-MPJPE),评估结构误差。 loss_velocity: 计算3D关键点的速度/移动的误差,评估运动的平滑程度。 loss_limb_var: 计算肢体长度的方差,引导生成合理的肢体长度…

最新人工智能源码搭建部署教程/ChatGPT程序源码/AI系统/H5端+微信公众号版本源码

一、AI系统 如何搭建部署人工智能源码、AI创作系统、ChatGPT系统呢&#xff1f;小编这里写一个详细图文教程吧&#xff01; SparkAi使用Nestjs和Vue3框架技术&#xff0c;持续集成AI能力到AIGC系统&#xff01; 1.1 程序核心功能 程序已支持ChatGPT3.5/GPT-4提问、AI绘画、…

Python加入Excel--生产力大提高|微软的全方面办公

Python作为一种功能强大的编程语言&#xff0c;已经逐渐成为了数据分析、机器学习、Web开发等领域的主流语言之一。而将Python集成到Excel中&#xff0c;则可以为Excel用户提供更加强大的数据处理和分析能力&#xff0c;同时也可以为Python开发者提供更加便捷的数据处理和可视化…

Mysql 基本概念

数据库的组成 数据 数据是描述事务的符号记录&#xff1b;包括数字、文字、图形、图像、声音、档案记录等 以“记录“形式按统一的格式进行存储 表 将不同的记录以行和列的方式组合成了表 用来存储具体数据 数据库 它就是所有不同属性表的集合 以一定的组织方式存储的…

【随笔】- 程序员的40岁后健身计划

【随笔】- 40岁后程序员的健身计划 文章目录 【随笔】- 40岁后程序员的健身计划一、树立健身信心&#xff0c;制订坚持计划二、挑选让你舒适的方式三、调整速度&#xff0c;以间歇式训练为主四、刚开始锻炼&#xff0c;别求太快五、增加力量、柔韧性和平衡练习六、运动多样化七…

三极管NPN、PNP的区别及简单应用

目录 一、NPN、PNP三极管的概念 二、NPN、PNP三极管的区别 三、三极管NPN、PNP的简单应用 一、NPN、PNP三极管的概念 NPN型三极管&#xff1a; 由两块N型半导体和一块P型半导体组成&#xff0c;P型半导体在中间&#xff0c;两块N型半导体在两侧。 三极管是电子电路中最重要的…

低通滤波器和高通滤波器

应用于图像低通滤波器和高通滤波器的实现 需要用到傅里叶变换 #include <opencv2/opencv.hpp> #include <Eigen> #include <iostream> #include <vector> #include <cmath> #include <complex>#define M_PI 3.14159265358979323846…

【C语言】喝汽水问题

大家好&#xff01;今天我们来学习C语言中的喝汽水问题&#xff01; 目录 1. 题目内容&#xff1a; 2. 思路分析 2.1 方法一 2.2 方法二 2.3 方法三 3. 代码实现 3.1 方法一 3.2 方法二 3.3 方法三 1. 题目内容 喝汽水&#xff0c;1瓶汽水1元&#xff0c;2个空瓶可以…