当前位置： 首页 > article >正文

标准卡尔曼滤波

article 2026/3/6 7:00:45

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

  • 状态转移方程
    x k = A x k − 1 + B u k + w k x_k = A x_{k-1} + Bu_k + w_k xk​=Axk−1​+Buk​+wk​
    • x k x_k xk​：k时刻的 状态向量，理论上的真实状态。
    • x k − 1 x_{k-1} xk−1​：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^k−1​+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^kmea​​x^kmea​​=H−zk​

    • 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(H−zk​−x^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​=H−zk​，这时更应该相信 测出来的结果

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

  • 对这个公式进行一个变换， 是 G = k k H G = k_k H G=kk​H, 带入上述公式得到
    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(H−zk​−x^k−​)x^k​=x^k−​+kk​H(H−zk​−x^k−​)−−−−−x^k​=x^k−​+kk​(zk​−Hx^k−​)

    • x ^ k \hat{\mathbf{x}}_k x^k​：后验估计
    • 此时 K k K_k Kk​ 的取值范围是 0 − H − {0 - H^-} 0−H−

  • 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​=xk​−x^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[ek​ek​T]
  • 我们的目标是让误差 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−​=Fk​Pk−1​Fk⊤​+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[wk​wk​T]
  • 卡尔曼增益 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​=HPk−​HT+Rk​Pk−​HT​
    • 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[vk​vk​T]
    • 当 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} Kk​≈HPk−​HTPk−​HT​≈H−1

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^k−1​+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−​=Fk​Pk−1​Fk⊤​+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​=HPk−​HT+Rk​Pk−​HT​//卡尔曼增益
    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​(zk​−Hx^k−​)//后验估计(当前时刻的最优估计)
    P k = ( I − K k H ) P k − / / 更新误差的协方差矩阵 ; I : 单位矩阵 P_{k}=(I-K_{k}H)P_{k}^{-} // 更新误差的协方差矩阵 ; I:单位矩阵 Pk​=(I−Kk​H)Pk−​//更新误差的协方差矩阵;I:单位矩阵

相关文章：

标准卡尔曼滤波

1.状态转移方程和观测方程 状态转移方程 x k A x k − 1 B u k w k x_k A x_{k-1} Bu_k w_k xk​Axk−1​Buk​wk​ x k x_k xk​：k时刻的 状态向量，理论上的真实状态。 x k − 1 x_{k-1} xk−1​：k-1时刻的 状态向量，理论…...

编程日记 2026/2/14 6:48:47

主流区块链

文章目录 主流链1. Solana特点：适用场景：工具链： 2. Binance Smart Chain (BSC)特点：适用场景：工具链： 3. Avalanche特点：适用场景：工具链： 4. Polkadot特点：…...

编程日记 2026/3/2 7:05:42

pytorch中有哪些损失函数

L1Loss 计算预测值 ypred 和真实值 ytrue 之间的平均绝对误差（MAE），公式为 L ( y p r e d , y t r u e ) 1 n ∑ i 1 n ∣ y p r e d i − y t r u e i ∣ L(y_{pred},y_{true})\frac1n\sum^n_{i1}|y^i_{pred}-y^i_{true}| L(ypred​,ytru…...

编程日记 2026/3/2 14:32:28

【设计模式有哪些】

一、创建型模式（Creation Patterns） 1. 单例模式（Singleton） 核心思想：保证一个类仅有一个实例，并提供全局访问点。实现方式：public class Singleton {// 1. 私有静态实例，volatil…...

编程日记 2026/2/21 14:05:38

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

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

编程日记 2026/3/4 5:56:54

默认参数 d = {} 的陷阱

默认参数 d {} 的陷阱 问题需求思路代码实现默认参数d {}的陷阱解决办法1、在函数外为每个字符串创建空字典统计词频2、函数改为每次调用时创建新字典，避免数据污染 举一反三 问题需求 统计两个字符串的中文词语出现次数 思路 先使用jieba库分词功能处理字符串…...

编程日记 2026/2/14 2:14:33

Python 常用内建模块-argparse

目录 argparse 小结 argparse 在命令行程序中，经常需要获取命令行参数。Python内置的sys.argv保存了完整的参数列表，我们可以从中解析出需要的参数： # copy.py import sys print(sys.argv) source sys.argv[1] target sys.argv[2] # TOD…...

编程日记 2026/2/14 4:59:30

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

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

编程日记 2026/2/18 18:07:24

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

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

编程日记 2026/3/1 13:13:22

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

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

编程日记 2026/3/4 20:38:00

春秋云境刷题1

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

编程日记 2026/2/26 6:58:24

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

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

编程日记 2026/3/5 18:45:48

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

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

编程日记 2026/2/22 19:46:12

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

这是淘淘摘苹果普通版&#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 …...

编程日记 2026/2/26 2:23:43

VSTO（C#）Excel开发 系列目录 含源码发布

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

编程日记 2026/3/4 22:11:27

在 Ubuntu 下通过 Docker 部署 Nginx+PHP-FPM 服务器

引言 大家好&#xff0c;今天我们来聊聊如何在 Ubuntu 下通过 Docker 部署 Nginx 和 PHP-FPM 服务器。Docker 是一个开源的容器化平台&#xff0c;可以轻松地打包、分发和管理应用程序。而 Nginx 是一个高性能的 HTTP 服务器和反向代理服务器&#xff0c;PHP-FPM 则是 PHP 的一…...

编程日记 2026/2/25 0:20:34

Git使用和原理（3）

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

编程日记 2026/2/19 0:47:20

【ELK】节省存储 之 压缩存储方式调整

目录 集群版本&#xff1a; 7.17.6 解释几个概念&#xff1a; 段&#xff08;Segment&#xff09; 合并(Merge) 索引设置&#xff1a; 压缩方式(index.codec)&#xff1a; 测试设置前提条件 对比 在创建的时候指定压缩类型&#xff08;index.codec&#xff09; 对比 在…...

编程日记 2026/3/6 0:42:31

导出的使用

在web开发中&#xff0c;导出是很常见的一个功能&#xff0c;当我进行个人项目练习的时候&#xff0c;导出的时候无法控制列宽以及居中样式&#xff0c;后续发现导出插件无法进行修改&#xff0c;整个插件较为简便易懂的同时&#xff0c;对于EX的控制较为简陋&#xff0c;很多东…...

编程日记 2026/2/15 14:00:32

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

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

编程日记 2026/2/28 15:44:00

鸿蒙开源硬件：重构万物智联生态的底层基座与未来机遇

一、从生态裂变到产业重构&#xff1a;开源鸿蒙的崛起之路 自 2020 年开源至今&#xff0c;OpenHarmony 社区以惊人的发展速度重塑智能终端操作系统格局。数据显示&#xff0c;其代码量已从初始的 700 万行激增至 1.2 亿行&#xff0c;汇聚超 8200 名开发者及 70 余家核心共建…...

编程日记 2026/3/2 7:05:50

C++之list类及模拟实现

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

编程日记 2026/3/5 4:43:37

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

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

编程日记 2026/3/2 19:46:57

在Electron中实现实时下载进度显示的完整指南

在开发Electron应用时&#xff0c;提供良好的用户体验至关重要&#xff0c;尤其是在下载大文件时。用户需要知道下载进度、预计完成时间以及当前下载速度。本文将详细介绍如何在Electron应用中实现实时下载进度显示功能&#xff0c;从主进程到渲染进程的完整流程。 技术栈是ele…...

编程日记 2026/2/14 13:25:22

java生成一个可以下载的word文件

在 Java 里&#xff0c;你能够借助 Apache POI 库来生成 Word 文件&#xff0c;并且实现文件下载功能。下面为你详细介绍实现步骤和示例代码。 1. 添加依赖 若使用 Maven 项目&#xff0c;需在 pom.xml 里添加 Apache POI 的依赖&#xff1a; <dependencies><depen…...

编程日记 2026/2/16 7:01:19

MacBook部署达梦V8手记

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

编程日记 2026/2/22 18:21:11

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

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

编程日记 2026/3/3 0:00:24

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

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

编程日记 2026/2/25 2:27:08

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

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

编程日记 2026/3/5 6:29:49

共享栈 线程局部存储 线程互斥 线程同步 消费者生产者模型

共享栈 第一个主线程会在栈区 而当其他线程创建时实在共享区动态申请的栈区 线程局部存储 __thread 关键字 与编译有关 全局变量是被线程共享的 每个线程都能看到 修改 但是如果对该全局变量加上__thread关键字后 该全局变量就不会被共享 将变量在库中的每一个线程的属…...

编程日记 2026/2/22 10:19:58