标准卡尔曼滤波

news2025/3/19 6:29:30

1.状态转移方程和观测方程

  • 状态转移方程
    x k = A x k − 1 + B u k + w k x_k = A x_{k-1} + Bu_k + w_k xk=Axk1+Buk+wk
    • x k x_k xk:k时刻的 状态向量,理论上的真实状态。
    • x k − 1 x_{k-1} xk1:k-1时刻的 状态向量,理论上的真实状态。
    • A A A:状态转移矩阵,用来决定下一时刻 状态向量状态空间 中的位置
    • B , u k B, u_k B,uk:控制矩阵 和 控制向量,用来对 状态向量 的位置进行修正或控制
    • w k w_k wk:过程噪声,模型本身引入的噪声,假定其符合 正态分布 ,数学期望(均值)为0,协方差矩阵为 Q Q Q
  • 观测方程
    z k = H x k + v k z_k = Hx_k + v_k zk=Hxk+vk
    • z k z_k zk:k时刻的观测值
    • H H H:观测矩阵,用于将 x k x_k xk 映射到观测空间
    • x k x_k xk:与 状态转移方程 中的 x k x_k xk 意义相同
    • v k v_k vk:测量噪声,测量不准确引入的噪声,假定其符合 正态分布 ,数学期望(均值)为0,协方差矩阵为 Q Q Q
  • 注意:
    • 在上述两个公式中 x k x_k xk,代表 k k k 时刻的 真实值,他是一个理想值,他假定预测模型和测量是完备的,误差也是已知的,但这在现实中是不可能的,卡尔曼滤波本身也是,不断通过 预测修正 两个阶段交替进行,使计算结果不断逼近真实结果 x k x_k xk 的一种方法。
    • 也可以这么理解卡尔曼滤波:模型估计测量 都有误差,误差的 概率分布 符合 正态分布(高斯分布),也就是 估计值测量值都不准,卡尔曼滤波就是在 估计值测量值 之间找到一个最优平衡点, 也就是找到 概率密度 最高的那个点,是一种最优估计的算法。

    如果误差都是已知的,那就直接修正了,那就等于没有误差,也不需要滤波,一个公式,直接就得到真结果了,那显然是不可能的。

2.卡尔曼增益 K k K_k Kk

  • 依据上述的状态方程和观测方程,可以得到下面两个公式:
    x ^ k − = F x ^ k − 1 + B u k \hat{\mathbf{x}}_k^-=\mathbf{F} \hat{\mathbf{x}}_{k-1}+\mathbf{B} \mathbf{u}_k x^k=Fx^k1+Buk
    z k = H x ^ k m e a x ^ k m e a = H − z k \mathbf{z}_k=\mathbf{H} \hat{\mathbf{x}}_{k_{mea}} \\ \hat{\mathbf{x}}_{k_{mea}} = H^-z_k zk=Hx^kmeax^kmea=Hzk

    • x ^ k − \hat{\mathbf{x}}_k^- x^k:当前状态的 先验估计 ,通过模型计算出来的值。
    • x ^ k m e a \hat{\mathbf{x}}_{k_{mea}} x^kmea 测量出来的值。

    此时, x ^ k − \hat{\mathbf{x}}_k^- x^k x ^ k m e a \hat{\mathbf{x}}_{k_{mea}} x^kmea 都是不准的,下一步就是从这两个不准的值中找到 最优估计

  • 假设存在一个系数 G G G,决定了我们应该相信 算出来的结果 还是 测出来的结果
    x ^ k = x ^ k − + G ( H − z k − x ^ k − ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + G(H^-z_k - \hat{\mathbf{x}}_k^-) x^k=x^k+G(Hzkx^k)

    • 假设 G = 0 G = 0 G=0 , x ^ k = x ^ k − \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- x^k=x^k, 这时应该相信 算出来的结果
    • 假设 G = 1 G = 1 G=1, x ^ k = H − z k \hat{\mathbf{x}}_k = H^-z_k x^k=Hzk,这时更应该相信 测出来的结果

    x ^ k \hat{\mathbf{x}}_k x^k:后验估计,也就是当前时刻的最优估计值

  • 对这个公式进行一个变换, 是 G = k k H G = k_k H G=kkH, 带入上述公式得到
    x ^ k = x ^ k − + G ( H − z k − x ^ k − ) x ^ k = x ^ k − + k k H ( H − z k − x ^ k − ) − − − − − x ^ k = x ^ k − + k k ( z k − H x ^ k − ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + G(H^-z_k - \hat{\mathbf{x}}_k^-) \\ \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k H(H^-z_k - \hat{\mathbf{x}}_k^-) \\ -----\\ \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k (z_k - H\hat{\mathbf{x}}_k^-) \\ x^k=x^k+G(Hzkx^k)x^k=x^k+kkH(Hzkx^k)x^k=x^k+kk(zkHx^k)

    • x ^ k \hat{\mathbf{x}}_k x^k:后验估计
    • 此时 K k K_k Kk 的取值范围是 0 − H − {0 - H^-} 0H
  • K k K_k Kk 就是 卡尔曼增益, 后续主要解决 k k k_k kk 的取值

3. 误差的协方差矩阵 P k P_k Pk

  • 每个时刻都会计算出一个 最优的估计(后验估计) x ^ k \hat{\mathbf{x}}_k x^k, 通常情况下,后验估计真实值 仍然存在误差,假设误差为 e k e_k ek :
    e k = x k − x ^ k / / 真实值 − 后验估计 e_k = x_k - \hat{\mathbf{x}}_k // 真实值 - 后验估计 ek=xkx^k//真实值后验估计
  • P k P_k Pk其实就是 e k e_k ek 的协方差矩阵
    P k = E [ e k e k T ] P_k = E[e_k {e_k}^T] Pk=E[ekekT]
  • 我们的目标是让误差 e k e_k ek 最小,也可以理解为方差最小,也可以说是协方差矩阵 P k P_k Pk 对角线之和最小,也就是协方差矩阵的 最小
  • 不难理解, P k P_k Pk也是一个理想值,无法直接获得,上面描述主要是帮助理解 P k P_k Pk 是什么,卡尔曼滤波中使用下面公式估计 P k P_k Pk 的值,下面是 P k P_k Pk 的先验估计公式
    P k − = F k P k − 1 F k ⊤ + Q k \mathbf{P}_k^-=\mathbf{F}_k\mathbf{P}_{k-1}\mathbf{F}_k^\top+\mathbf{Q}_k Pk=FkPk1Fk+Qk
    • Q k Q_k Qk估计误差 w k w_k wk(状态空间方程中的 w k w_k wk) 的协方差矩阵: Q k = E [ w k w k T ] Q_k = E[w_k {w_k}^T] Qk=E[wkwkT]
  • 卡尔曼增益 k k k_k kk的公式
    K k = P k − H T H P k − H T + R k K_k=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+R_k} Kk=HPkHT+RkPkHT
    • R k R_k Rk:测量误差 v k v_k vk 的协方差矩阵: R k = E [ v k v k T ] R_k = E[v_k {v_k}^T] Rk=E[vkvkT]
    • R k R_k Rk 比较大时,也就是分母比较大, K k K_k Kk 就会比较小,当 R k R_k Rk 足够大, K k K_k Kk 就会趋近于0。
    • R k R_k Rk 比较小时, K k ≈ P k − H T H P k − H T ≈ H − 1 K_k \approx {\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}}} \approx H^{-1} KkHPkHTPkHTH1

4. 卡尔曼滤波的递推流程

  • 卡尔曼滤波器的递推流程主要分为 预测校正 两步
  • 预测部分: 包括 状态的先验估计误差协方差矩阵的先验估计
    x ^ k − = F x ^ k − 1 + B u k / / 状态的先验估计 \hat{\mathbf{x}}_k^-=\mathbf{F} \hat{\mathbf{x}}_{k-1}+\mathbf{B} \mathbf{u}_k //状态的先验估计 x^k=Fx^k1+Buk//状态的先验估计
    P k − = F k P k − 1 F k ⊤ + Q k / / 误差协方差矩阵的先验估计 \mathbf{P}_k^-=\mathbf{F}_k\mathbf{P}_{k-1}\mathbf{F}_k^\top+\mathbf{Q}_k // 误差协方差矩阵的先验估计 Pk=FkPk1Fk+Qk//误差协方差矩阵的先验估计
  • 矫正部分包括:卡尔曼增益的计算, 后验估计(当前时刻的最优估计), 更新误差的协方差矩阵
    K k = P k − H T H P k − H T + R k / / 卡尔曼增益 K_k=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+R_k} //卡尔曼增益 Kk=HPkHT+RkPkHT//卡尔曼增益
    x ^ k = x ^ k − + k k ( z k − H x ^ k − ) / / 后验估计 ( 当前时刻的最优估计 ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k (z_k - H\hat{\mathbf{x}}_k^-) //后验估计(当前时刻的最优估计) x^k=x^k+kk(zkHx^k)//后验估计(当前时刻的最优估计)
    P k = ( I − K k H ) P k − / / 更新误差的协方差矩阵 ; I : 单位矩阵 P_{k}=(I-K_{k}H)P_{k}^{-} // 更新误差的协方差矩阵 ; I:单位矩阵 Pk=(IKkH)Pk//更新误差的协方差矩阵;I:单位矩阵

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

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

基于SpringBoot+Vue的幼儿园管理系统+LW示例参考

1.项目介绍 系统角色:管理员、教师、普通用户功能模块:用户管理、教师管理、班级管理、幼儿信息管理、会议记录管理、待办事项、职工考核、请假信息、缴费信息、体检管理、资源管理、原料管理、菜品信息管理等技术选型:SpringBoot&#xff0…

案例5_3: 6位数码管静态显示

文章目录 文章介绍效果图仿真图复习知识:代码思考 文章介绍 第5章 学习数码管,使用6位数码管进行静态显示 效果图 仿真图 新建一个干净的5_3文件夹,用于存放新画的仿真图 除单片机最小系统外,新增3个元器件,分别是&…

Profinet转Modbus RTU/TCP以太网通讯处理器

Profinet转Modbus RTU/TCP以太网通讯处理器 在当今的工业自动化领域,各种通讯协议和标准层出不穷。 其中,Profinet和Modbus作为两种广泛应用的通讯协议,分别在不同的应用场景中发挥着重要作用。 然而,当需要将这两种协议进行转换…

3倍训练速度+40%显存节省!Mamba+Transformer 仅用一半时间,性能提升80%!

在人工智能领域,Mamba与Transformer的结合正在成为研究热点,为自然语言处理和多模态任务带来新的突破。 最新研究表明,通过将Mamba架构与Transformer的强大编码能力相结合,模型在处理复杂的多模态数据时的效率提升了50%&#xff…

春秋云境刷题1

CVE-2022-29464 靶标介绍: WSO2文件上传漏洞(CVE-2022-29464)是Orange Tsai发现的WSO2上的严重漏洞。该漏洞是一种未经身份验证的无限制任意文件上传,允许未经身份验证的攻击者通过上传恶意JSP文件在WSO2服务器上获得RCE。 Git…

台式机电脑组装---电源

台式机电脑组装—电源 22 33 主板供电是聚集了12V,5V,3.3V的24pin CPU供电的话主要是12V的44pin供电 44pin合并之后,就是8pin 55 SATA硬盘会使用饼io口取电,从电源获取12v,5v,3.3v的电 33

10-BST(二叉树)-建立二叉搜索树,并进行前中后遍历

题目 来源 3540. 二叉搜索树 - AcWing题库 思路 建立二叉搜索树(注意传参时用到了引用,可以直接对root进行修改),同时进行递归遍历;遍历可以分前中后三种写,也可以用标志来代替合在一起。其余详见代码。…

蓝桥杯备考:贪心问题之淘淘摘苹果

这是淘淘摘苹果普通版&#xff0c;很可爱的一道题&#xff0c;我们不多陈述&#xff0c;直接上代码 #include <iostream> using namespace std; const int N 15; int a[N]; int main() {for(int i 1;i<10;i){cin >> a[i];}int x;cin >> x;x30;int cnt …

VSTO(C#)Excel开发 系列目录 含源码发布

初级代码游戏的专栏介绍与文章目录-CSDN博客 我的github&#xff1a;codetoys&#xff0c;所有代码都将会位于ctfc库中。已经放入库中我会指出在库中的位置。 这些代码大部分以Linux为目标但部分代码是纯C的&#xff0c;可以在任何平台上使用。 源码指引&#xff1a;github源…

Git使用和原理(3)

1.远程操作 1.1分布式版本控制系统 我们⽬前所说的所有内容&#xff08;⼯作区&#xff0c;暂存区&#xff0c;版本库等等&#xff09;&#xff0c;都是在本地&#xff01;也就是在你的笔记本或者 计算机上。⽽我们的 Git 其实是分布式版本控制系统&#xff01;什么意思呢&a…

博客图床 VsCode + PigGo + 阿里云OSS

关键字 写博客&#xff0c;图床&#xff0c;VsCode&#xff0c;PigGo&#xff0c;阿里云OSS 背景环境 我想把我在本地写的markdown文档直接搬到CSDN上和博客园上&#xff0c;但是图片上传遇到了问题。我需要手动到不同平台上传文件&#xff0c;非常耗费时间和经历。 为了解决…

C++之list类及模拟实现

目录 list的介绍 list的模拟实现 定义节点 有关遍历的重载运算符 list的操作实现 &#xff08;1&#xff09;构造函数 (2)拷贝构造函数 &#xff08;3&#xff09;赋值运算符重载函数 &#xff08;4&#xff09;析构函数和clear成员函数 &#xff08;5&#xff09;尾…

SwinTransformer 改进:添加DoubleAttention模块提升上下文语义提取能力

目录 1. DoubleAttention模块 2. SwinTransformer + DoubleAttention 3. 完整代码 Tips:融入模块后的网络经过测试,可以直接使用,设置好输入和输出的图片维度即可 1. DoubleAttention模块 DoubleAttention 是一种用于计算机视觉任务的注意力机制,旨在通过双重注意力机制…

MacBook部署达梦V8手记

背景 使用Java SpringBootDM开发Web应用&#xff0c;框架有License&#xff0c;OSX加载dll失败&#xff0c;安装了Windows 11&#xff0c;只有一个C盘&#xff0c;达梦安装后因为C盘权限问题&#xff0c;创建数据库失败&#xff0c;遂采用Docker容器方式部署。 下载介质 官网在…

外贸 B2B 平台没落?多语言批发系统正在崛起

近年来&#xff0c;全球外贸行业正在发生快速变化&#xff0c;传统的 B2B 平台正面临越来越多的挑战&#xff0c;尤其是在面对新兴的多语言批发系统时。这种变化不仅影响了供应商和买家之间的交易方式&#xff0c;也正在推动外贸行业的数字化升级和转型。今天&#xff0c;让我们…

[spring] Spring JPA - Hibernate 多表联查 1

[spring] Spring JPA - Hibernate 多表联查 之前在 [spring] spring jpa - hibernate 名词解释&配置 和 [spring] spring jpa - hibernate CRUD 简单的学习了一下怎么使用 Hibernate 实现 CRUD 操作&#xff0c;不过涉及到的部分都是逻辑上比较简单的实现——只在一张表上…

鸿蒙Next开发实战教程—电影app

最近忙忙活活写了不少教程&#xff0c;但是总感觉千篇一律&#xff0c;没什么意思&#xff0c;大家如果有感兴趣的项目可以私信给幽蓝君写一写。 今天分享一个电影App。 这个项目也比较简单&#xff0c;主要是一些简单页面的开发和本地视频的播放以及横竖屏切换。 页面搭建以…

停车场停车位数据集,标注停车位上是否有车,平均正确识别率99.5%,支持yolov5-11, coco json,darknet,xml格式标注

停车场停车位数据集&#xff0c;标注停车位上是否有车&#xff0c;平均正确识别率98.0&#xff05;&#xff0c;支持yolov5-11&#xff0c; coco json&#xff0c;darknet&#xff0c;xml格式标注 数据集-识别停车场所有车辆的数据集 数据集分割 一共184张图片 训练组 89&am…

ssm框架之mybatis框架讲解

1&#xff0c;Mybatis 1.1 Mybatis概述 1.1.1 Mybatis概念 MyBatis 是一款优秀的持久层框架&#xff0c;用于简化 JDBC 开发 MyBatis 本是 Apache 的一个开源项目iBatis, 2010年这个项目由apache software foundation 迁移到了google code&#xff0c;并且改名为MyBatis 。2…

CEF 多进程模式时,注入函数,获得交互信息

CEF 控制台添加一函数,枚举 注册的供前端使用的CPP交互函数有哪些-CSDN博客 上篇文章,是在模拟环境,单进程中设置的,这篇文章,将其改到正常多进程环境中设置。 对应于工程中的 CEF_RENDER项目 一、多进程模式中,改写 修改步骤 1、注入函数 client_app_render.cpp 在…