卡尔曼滤波公式通俗理解

2024-09-01 18:08

本文主要是介绍卡尔曼滤波公式通俗理解,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

本文需要配合博客卡尔曼滤波详解进行理解

1.简单介绍

参考卡尔曼滤波详解
简单介绍
上面可简化理解为
简化理解

2.主要过程

主要过程还是参考卡尔曼滤波详解
图1
图2
图3

3.实例

这里以线性运动为例

3.1 前期定义状态和变量
3.1.1分析运动情况

已知线性运动上一状态和当前状态的关系,假设没有噪声干扰,为
{ x ′ = x + v x Δ t y ′ = y + v y Δ t \begin{cases} x'=x+v_x \Delta t \\ y'=y+v_y \Delta t \\ \end{cases} {x=x+vxΔty=y+vyΔt

3.1.2 定义跟踪状态和状态转移矩阵

[ x y v x v y ] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right] xyvxvy
则根据3.1.1可以得到状态转移关系
[ x ′ y ′ v x ′ v y ′ ] = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] [ x y v x v y ] \left[ \begin{matrix} x' \\ y' \\ v_x' \\ v_y' \\ \end{matrix} \right]= \left[ \begin{matrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right] xyvxvy=10000100Δt0100Δt01xyvxvy

A = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] A=\left[ \begin{matrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] A=10000100Δt0100Δt01
加上状态转移噪声
x k = A x k − 1 + w x_k=Ax_{k-1}+w xk=Axk1+w

3.1.3 定义状态到观测转移矩阵

由于在实际过程中,我们只观测到位置 [ z x z y ] \left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right] [zxzy]所以我们要得到从跟踪状态 [ x y v x v y ] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right] xyvxvy [ z x z y ] \left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right] [zxzy]的转换矩阵,假设我们观察的位置没有噪声干扰则:
[ z x z y ] = [ 1 0 0 0 0 1 0 0 ] [ x y v x v y ] \left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right]= \left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{matrix} \right] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right] [zxzy]=[10010000]xyvxvy

H = [ 1 0 0 0 0 1 0 0 ] H=\left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{matrix} \right] H=[10010000]
加上测量噪声可得到
z k = H x k + v z_k=Hx_k+v zk=Hxk+v
由上面三部我们就基本把所需要的变量定义完了,下面就需要进行预测和矫正了

3.4 卡尔曼滤波过程

假设我们来了一帧数据
[ z x z y t ] \left[ \begin{matrix} z_x \\ z_y \\ t \\ \end{matrix} \right] zxzyt
其中 [ z x z y ] \left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right] [zxzy]为观测数据, t t t为当前时间,则进行下面步骤:

  • (预测)根据 t t t和上一帧位置,预测当前帧位置,同时计算预测误差 P k ‾ \overline{P_k} Pk
  • (矫正)求出卡尔曼增益,计算最优位置估计,计算当前估计误差 P k P_k Pk
3.2.1 预测

求出 Δ t = t − t p r e \Delta t=t-t_{pre} Δt=ttpre,根据3.1.1估算当前位置
[ x ′ y ′ v x ′ v y ′ ] = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] [ x y v x v y ] \left[ \begin{matrix} x' \\ y' \\ v_x' \\ v_y' \\ \end{matrix} \right]= \left[ \begin{matrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right] xyvxvy=10000100Δt0100Δt01xyvxvy
x k ^ ‾ = A x k − 1 ^ \overline{\hat{x_k}}=A\hat{x_{k-1}} xk^=Axk1^
计算当前估计误差,结合基本公式(过程噪声 Q Q Q)
P k ‾ = A P k − 1 A T + Q \overline{P_k}=AP_{k-1}A^T+Q Pk=APk1AT+Q

3.2.2 矫正

结合基本公式
计算卡尔曼增益(测量噪声 R R R)
K k = P k ‾ H T ( H P k ‾ H T + R ) − 1 K_k=\overline{P_k}H^T(H\overline{P_k}H^T+R)^{-1} Kk=PkHT(HPkHT+R)1
计算最优位置估计
x k ^ = x k ^ ‾ + K k ( z k − H x k ^ ‾ ) \hat{x_k}=\overline{\hat{x_k}}+K_k(z_k-H\overline{\hat{x_k}}) xk^=xk^+Kk(zkHxk^)
计算估计误差
P k = ( I − K k H ) P k ‾ P_k=(I-K_kH)\overline{P_k} Pk=(IKkH)Pk

3.2.3 实际过程

综上,我们在实际过程中,只要设置好

  • 跟踪变量 x x x
  • 状态转移矩阵 A A A
  • 状态到观测矩阵 H H H
  • 状态转移噪声 Q Q Q
  • 测量噪声 R R R
  • 初始估计误差 P k P_k Pk(设置不能太小)
  • 第一次需要设置 x k ^ \hat{x_k} xk^的值
    就可以了

这篇关于卡尔曼滤波公式通俗理解的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/1127673

相关文章

一文带你理解Python中import机制与importlib的妙用

《一文带你理解Python中import机制与importlib的妙用》在Python编程的世界里,import语句是开发者最常用的工具之一,它就像一把钥匙,打开了通往各种功能和库的大门,下面就跟随小... 目录一、python import机制概述1.1 import语句的基本用法1.2 模块缓存机制1.

深入理解C语言的void*

《深入理解C语言的void*》本文主要介绍了C语言的void*,包括它的任意性、编译器对void*的类型检查以及需要显式类型转换的规则,具有一定的参考价值,感兴趣的可以了解一下... 目录一、void* 的类型任意性二、编译器对 void* 的类型检查三、需要显式类型转换占用的字节四、总结一、void* 的

深入理解Redis大key的危害及解决方案

《深入理解Redis大key的危害及解决方案》本文主要介绍了深入理解Redis大key的危害及解决方案,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着... 目录一、背景二、什么是大key三、大key评价标准四、大key 产生的原因与场景五、大key影响与危

深入理解C++ 空类大小

《深入理解C++空类大小》本文主要介绍了C++空类大小,规定空类大小为1字节,主要是为了保证对象的唯一性和可区分性,满足数组元素地址连续的要求,下面就来了解一下... 目录1. 保证对象的唯一性和可区分性2. 满足数组元素地址连续的要求3. 与C++的对象模型和内存管理机制相适配查看类对象内存在C++中,规

认识、理解、分类——acm之搜索

普通搜索方法有两种:1、广度优先搜索;2、深度优先搜索; 更多搜索方法: 3、双向广度优先搜索; 4、启发式搜索(包括A*算法等); 搜索通常会用到的知识点:状态压缩(位压缩,利用hash思想压缩)。

【生成模型系列(初级)】嵌入(Embedding)方程——自然语言处理的数学灵魂【通俗理解】

【通俗理解】嵌入(Embedding)方程——自然语言处理的数学灵魂 关键词提炼 #嵌入方程 #自然语言处理 #词向量 #机器学习 #神经网络 #向量空间模型 #Siri #Google翻译 #AlexNet 第一节:嵌入方程的类比与核心概念【尽可能通俗】 嵌入方程可以被看作是自然语言处理中的“翻译机”,它将文本中的单词或短语转换成计算机能够理解的数学形式,即向量。 正如翻译机将一种语言

【C++高阶】C++类型转换全攻略:深入理解并高效应用

📝个人主页🌹:Eternity._ ⏩收录专栏⏪:C++ “ 登神长阶 ” 🤡往期回顾🤡:C++ 智能指针 🌹🌹期待您的关注 🌹🌹 ❀C++的类型转换 📒1. C语言中的类型转换📚2. C++强制类型转换⛰️static_cast🌞reinterpret_cast⭐const_cast🍁dynamic_cast 📜3. C++强制类型转换的原因📝

hdu 4565 推倒公式+矩阵快速幂

题意 求下式的值: Sn=⌈ (a+b√)n⌉%m S_n = \lceil\ (a + \sqrt{b}) ^ n \rceil\% m 其中: 0<a,m<215 0< a, m < 2^{15} 0<b,n<231 0 < b, n < 2^{31} (a−1)2<b<a2 (a-1)^2< b < a^2 解析 令: An=(a+b√)n A_n = (a +

深入理解RxJava:响应式编程的现代方式

在当今的软件开发世界中,异步编程和事件驱动的架构变得越来越重要。RxJava,作为响应式编程(Reactive Programming)的一个流行库,为Java和Android开发者提供了一种强大的方式来处理异步任务和事件流。本文将深入探讨RxJava的核心概念、优势以及如何在实际项目中应用它。 文章目录 💯 什么是RxJava?💯 响应式编程的优势💯 RxJava的核心概念

如何通俗理解注意力机制?

1、注意力机制(Attention Mechanism)是机器学习和深度学习中一种模拟人类注意力的方法,用于提高模型在处理大量信息时的效率和效果。通俗地理解,它就像是在一堆信息中找到最重要的部分,把注意力集中在这些关键点上,从而更好地完成任务。以下是几个简单的比喻来帮助理解注意力机制: 2、寻找重点:想象一下,你在阅读一篇文章的时候,有些段落特别重要,你会特别注意这些段落,反复阅读,而对其他部分