【卡尔曼滤波】 Kalman Filter 原理详解与公式推导

于 2024-10-01 11:29:51 发布
阅读量549 收藏 16
点赞数 11
分类专栏： 控制算法 文章标签： 单片机 stm32 c语言 嵌入式硬件
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/AbaAbaxxx_/article/details/142596142
版权

【卡尔曼滤波】 Kalman Filter 原理详解与公式推导

前言

笔者最近在学习卡尔曼滤波，因个人水平实在有限，花了许久才理解卡尔曼滤波的思想。故现以通俗易懂的语言，总结我对卡尔曼滤波原理的理解

参考资料：本笔记基于Blog
中的实例与图解，并推荐大家阅读这篇文章

引例：什么是卡尔曼滤波？

现在，你开发了一个可以在森林里到处跑的小机器人。
这个机器人带有GPS，精度大约10m
森林里有很多沟壑和悬崖，如果机器人走错了一步，就有可能掉下悬崖。因此我们需要基于已有信息，推测出比GPS更精确的位置
在这里插入图片描述
状态矩阵
我们可以说小机器人有个zhuang
ps:这个状态只是这个小机器人的一系列基本属性，在这里是位置与速度。在其他场景也可以是机器人的重量、温度等

已知信息：
状态信息：通过估计得到的机器人的位置、速度。电机的预期转速以及机器人的运动方向等
测量信息：GPS测得的位置

但是，两种信息都有各自的不确定性
状态信息可能受到阻力、地面不平等影响；而GPS测量的信息则因其精度不足而有误差（测量误差）。
故我们既不能完全相信状态信息，也不能完全相信预测信息

需要注意，两种信息的不确定性也分大小。若GPS精度特别低，则预测信息的不确定性会更大。若运动过程中受到的阻力等干扰过多，则状态方程也难以很好估计机器人状态

卡尔曼滤波的引入：
卡尔曼滤波可以帮助我们在不确定性中融合信息，提取精确信息
简单来说：卡尔曼滤波算法以确定性为权重/即确定性高的信息权重更大，更相信；确定性低的信息权重较小，更不相信。
他通过两种信息的权重，加权融合信息，得到最优估计

图解卡尔曼滤波

引入

我们引入二维状态变量的概率密度分布图，来形象的表现系统二维状态(如小机器人的位置与速度)的可能性的分布。

概率密度分布图
在这里插入图片描述
越亮的区域表示越大的可能性

二维状态变量的均值与方差
两个状态变量均服从高斯分布，故每个变量都有一个均值 μ，共同表示随机分布的中心(最可能的状态)；以及一个方差𝜎^2，协方差表示不确定性(中心向两侧的偏移量)
在这里插入图片描述

二维状态变量的相关性
事实上，二维状态变量常常是存在相关性的。就以位置与速度而言，若当下速度很大，则下此位置与这次位置相比，大概率偏差很大。二维状态变量的相关性以协方差的形式体现
以图表示
在这里插入图片描述

从宏观上理解卡尔曼融合

概述
卡尔曼滤波基于已知的状态信息（由状态方程预测）和观测信息（如传感器测量），
分别得到先验状态矩阵（靠状态信息的运动状态方程预测）和观测状态矩阵(传感器测量到的直接或间接状态值)
这两者的重叠区域，即融合后的后验状态矩阵。
我们通过密度连乘计算后验矩阵密度分布分析，可以提取出其中心（即均值或最优估计）和边长（即协方差）。
最优估计即后验状态矩阵的中心

先验状态矩阵与观测状态矩阵
在这里插入图片描述
后验状态矩阵(最优估计矩阵)
在这里插入图片描述
分析后验状态矩阵
卡尔曼滤波融合先验状态矩阵和观测状态矩阵后得到的后验状态矩阵，也即融合得到最优估计
在这里插入图片描述

从微观上理解状态矩阵

先验状态矩阵

无外部控制的状态转移(基于状态转移方程
在这里插入图片描述
我们用状态转移矩阵Fk来表示状态的转移，并预测这个过程
在这里插入图片描述
实际上，状态转移矩阵Fk使得中心(均值)与边长(协方差)均进行变换，体现为状态矩阵的矩阵变换
有外部控制的状态转移
在这里插入图片描述
噪声的参与
噪声通常服从均值为 0、方差为 Q 的高斯分布。
因此，噪声不会改变状态矩阵的均值，只会影响方差。即不改变先验状态矩阵的中心，只改变状态矩阵的边长。
因此，在状态转移方程中，无需考虑噪声的影响。但在协方差转移方程中，需加入噪声协方差 Q，以表示噪声增加了系统的不确定性和离散性。

观测状态矩阵

我们有时不直接测量状态值。
如陀螺仪中观测量为角速度，而状态量为角度，前者为后者的低一阶物理量，因此观测矩阵需要将测量值映射为状态值。
在这里插入图片描述

观测状态矩阵映射为同物理意义的状态矩阵。实际上也是中心(均值)与边长(协方差)的映射。
注意，观测噪声(传感器精度)同样会使得观测状态矩阵的不确定性和离散性增加。

卡尔曼滤波公式理解

在这里我们只较为浅显的理解卡尔曼滤波的公式，若想了解详细的公式推导，可跳跃至b站DR_CAN
卡尔曼滤波公式
在这里插入图片描述

参数说明

预测阶段（Prediction）

在这里插入图片描述

更新阶段（update）

在这里插入图片描述

公式理解

我们现在来一一对比卡尔曼滤波图解，来理解卡尔曼滤波公式

预测阶段（Prediction）

在这里插入图片描述

更新阶段（update）

在这里插入图片描述

卡尔曼增益系数与推导过程

我们先以一维高斯分布来分析，具有方差 这里写图片描述 和 μ 的高斯曲线可以用下式表示：
在这里插入图片描述
而卡尔曼滤波就是将两个服从高斯分布的概率密度函数(预测概率密度、测量概率密度)相乘融合，并求融合高斯分布的均值(最优估计)与方差
在这里插入图片描述
化简可得融合高斯分布的均值与方差：
在这里插入图片描述
定义卡尔曼增益系数K
实际上这个K还不是真正的卡尔曼增益系数，卡尔曼增益系数其实包含了Hk-的逆矩阵的作用，也即测量值到预测值的映射作用
在这里插入图片描述
改写为高维矩阵形式
在这里插入图片描述
公式整合
在这里插入图片描述

卡尔曼滤波器框架

在这里插入图片描述

Abaaba+
关注
专栏目录
滤波笔记一：卡尔曼滤波（Kalman Filtering）详解
scoutee的博客
07-02 10万+
卡尔曼滤波法的详解，包括公式推导，算法迭代步骤以及python源代码。
详解卡尔曼滤波原理
热门推荐
清风莞尔的博客
03-18 41万+
详解卡尔曼滤波原理　　在网上看了不少与卡尔曼滤波相关的博客、论文，要么是只谈理论、缺乏感性，或者有感性认识，缺乏理论推导。能兼顾二者的少之又少，直到我看到了国外的一篇博文，真的惊艳到我了，不得不佩服作者这种细致入微的精神，翻译过来跟大家分享一下，
卡尔曼滤波(Kalman Filter)概念介绍及详细公式推导
泽渊的博客
05-29 2万+
卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。​ 卡尔曼滤波的算法是二步骤的程序。
卡尔曼滤波【Kalman Filter】详解
weixin_44606353的博客
04-20 1730
对于卡尔曼滤波器，我们几乎可以下这么一个定论：只要是存在不确定信息的动态系统，卡尔曼滤波就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰，卡尔曼滤波通常也能很好的弄清楚究竟发生了什么，找出现象间不易察觉的相关性。 因此卡尔曼滤波非常适合不断变化的系统，它的优点还有内存占用较小（只需保留前一个状态）、速度快，是实时问题和嵌入式系统的理想选择。
卡尔曼滤波（Kalman Filter）原理与公式推导
我究竟在写什么
04-12 4049
一、背景---卡尔曼滤波的意义 随着传感技术、机器人、自动驾驶以及航空航天等技术的不断发展，对控制系统的精度及稳定性的要求也越来越高。卡尔曼滤波作为一种状态最优估计的方法，其应用也越来越普遍，如在无人机、机器人等领域均得到了广泛应用。 对于Kalman Filter的理解，用过的都知道“黄金五条”公式，且通过“预测”与“更新”两个过程来对系统的状态进行最优估计，但完整的推导过程却不一定能写出来...
【学习笔记】卡尔曼滤波 Kalman Filter
一些笔记
02-21 2199
对卡尔曼滤波算法的系统学习
详解卡尔曼滤波（Kalman Filter）
weixin_55252589的博客
12-30 3564
黑盒（Black Box）思想最早由维纳（Wiener）在1939年提出，即假定我们对从数据到估计中间的映射过程一无所知，仅仅用线性估计（我们知道在高斯背景下，线性估计能达到克拉美劳下界，是最优估计）来掩盖我们的无知。但是，到了二十年以后的1960年卡尔曼的年代，我们对于红框内的事（prior knowledge），很有可能是知道的，并且知道得很详细，很清楚。再这样的情况下，我们还不能有效地利用这一部分先验知识来帮助我们把估计做的更好就没有任何道理了。卡尔曼就是从红框入手，将这部分先验信息表达出来。至于怎么
卡尔曼滤波（Kalman Filter）原理与公式推导【转载】
xiaoyaolangwj的博客
09-02 1291
克尔曼滤波器
卡尔曼滤波(kalman filter)超详细推导
littlefrogyq的博客
07-23 1816
1. 概率论相关知识 这一节主要叙述概率论的一些相关基础知识，包括全概率公式、贝叶斯公式、协方差矩阵、二维高斯分布等等，对这些熟悉的可以直接跳到第二节 1.1 全概率公式与贝叶斯公式 1.2 协方差矩阵 1.3 二维高斯分布 2. 贝叶斯(Bayes)滤波 卡尔曼滤波是基于贝叶斯滤波的一种改进，这里先说明一下贝叶斯滤波的相关原理。 2.1 变量定义 运动过程一般需要用到的变量主要有3个： xi\boldsymbol x_ixi​表示iii时刻的状态。比如在运动的机器人中，表示机器人当前的位姿 zi\bold
通俗讲解卡尔曼滤波Kalman Filter原理及Python实现教程
qq_28077617的博客
04-21 3235
Python编程实践卡尔曼滤波 import numpy as np # 模拟数据 t = np.linspace(1,100,100) a = 0.5 position = (a * t**2)/2 position_noise = position+np.random.normal(0,120,size=(t.shape[0])) import matplotlib.pyplot as plt plt.plot(t,position,label='truth position') pl...
卡尔曼滤波（Kalman Filter）原理与公式推导.pdf
12-23
卡尔曼滤波（Kalman Filter）原理与公式推导.pdf
Kalman Filter卡尔曼滤波 java实现
08-09
KalmanFilter,KalmanFilter3,KalmanFilter5 一共7种,以上三种实现保证可用,其余待调试,调试函数写好,直接运行即可 KalmanFilter KalmanFilter2 KalmanFilter3 KalmanFilter4 KalmanFilter5 KalmanFilter6 Kalman...
卡尔曼滤波_Kalmanfilter_amsyk_卡尔曼滤波_verilog_卡尔曼verilog
09-10
标题中的"卡尔曼滤波_Kalmanfilter_amsyk_卡尔曼滤波_verilog_卡尔曼verilog"暗示了这个项目是关于使用Verilog硬件描述语言实现卡尔曼滤波器。Verilog是一种广泛用于数字电路设计的语言，它可以用来描述和模拟数字...
基于单片机的温湿度检测判断系统
夜间去看海的博客
09-27 730
基于STC89C52单片机，采用dht11温湿度传感器检测温湿度， 通过lcd1602显示屏各个参数，四个按键分别可以增加温湿度的阈值， 如果超过阈值，则电机启动打开风扇
TI DSP TMS320F280025 Note13：CPUtimer定时器原理分析与使用
weixin_52849254的博客
09-30 496
当系统时钟（SYSCLKOUT） 来一个脉冲， PSCH： PSC 预定标计数器减 1， 当 PSCH： PSC 预定标计数器减到 0 的时候， 预定标计数器产生下溢后向定时器的 32 位计数器 TIMH:TIM 借位， 即 TIMH:TIM 计数器减 1， 同时 PSCH： PSC 可以重载定时器分频寄存器（TDDRH： TDDR） 的值；每个系统时钟都会使定时器16位的预定标计数器减一，当16位的预定标计数器减到0时，16位的预定标计数器重置，同时32位计数器减一。（3） 定时器的设置。
STM32 MCU学习资源
qq_50930131的博客
09-19 820
作者: @bushuo联系方式: 3513566868@qq.com。
TI DSP TMS320F280025 Note15：串口SCI的使用
最新发布
weixin_52849254的博客
09-30 555
发送器和接收器具有单独的中断启用位。⑨可编程的中断级： 发送和接收 FIFO 都能产生 CPU 中断， 只要发送 FIFO 状态位 TXFFST（位 12-8） 与中断触发优先级 TXFFIL（位 4-0） 相匹配， 就产生一个中断触发， 从而为 SCI 的发送和接收提供一个可编程的中断触发逻辑。如果ISR在下一个起始位开始之前没有完成(在RX线再次变低之前)，SCI模块在错误的位置开始读取起始位，因此可能会错误地读取所有位，直到下一个正确对齐的起始位(当ISR有足够的时间在再次开始位之前处理)。
STM32常见配置
m0_66746512的博客
09-27 523
PWM模式：设置输出比较模式为PWM模式1.在PWM模式1中，输出将在计数器值小于TIM_Pulse时为高电平，大于时为低电平。I2C_Mode_SMBus：用于SMBus（系统管理总线），更具特色，适合特定应用。I2C_Mode_I2C：标准I2C模式，用于一般应用。I2C_DutyCycle：指定在快速模式下的时钟高低电平的比例。I2C_Init：将配置应用于指定的I2C接口（如I2C1）I2C_ClockSpeed：设置I2C总线的时钟频率；I2C_OwnAddress1：设置从设备的I2C地址。
卡尔曼滤波(kalman filter)原理与公式推导
08-24
卡尔曼滤波是一种用于通过融合多个传感器测量数据来估计物体状态的优化算法。它在估计中考虑了系统的动态模型以及传感器的测量噪声，能够在噪声和不确定性环境下提供最优的状态估计。 卡尔曼滤波的原理可以通过以下步骤进行推导： 1. 假设系统是线性的，可以用以下状态方程描述：x(k)=Ax(k-1)+Bu(k)+w(k)，其中x(k)是状态向量，A是状态转移矩阵，B是输入控制矩阵，u(k)是控制向量，w(k)是系统过程噪声。 2. 假设测量数据是线性的，可以用以下观测方程描述：z(k)=Hx(k)+v(k)，其中z(k)是观测向量，H是观测矩阵，v(k)是测量噪声。 3. 假设状态和测量噪声都是高斯分布，即w(k)~N(0,Q)，v(k)~N(0,R)，其中Q是系统过程噪声协方差矩阵，R是测量噪声协方差矩阵。 4. 初始化卡尔曼滤波器的状态估计值和协方差矩阵。 5. 递归执行以下步骤： a. 预测步骤：根据当前状态估计值和模型方程，计算预测状态和预测协方差矩阵。即x^- = Ax+Bu，P^- = APA^T + Q。 b. 更新步骤：根据测量值和观测方程，计算卡尔曼增益、估计当前状态和协方差矩阵。即K = P^-HT(HP^-HT + R)^-1，x = x^- + K(z - Hx^-)，P = (I - KH)P^-。 6. 返回估计的状态。 卡尔曼滤波是通过不断更新状态估计值和协方差矩阵来提供最优状态估计的。预测步骤利用系统的动态模型来预测下一个状态，更新步骤则将预测值和测量值进行融合，根据测量值的准确性来修正预测值。通过迭代执行预测和更新步骤，卡尔曼滤波能够提供最优的状态估计值。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值