记录:卡尔曼滤波推导

参考:古月居-卡尔曼滤波

一、高斯分布(Gaussian Distribution)

  • 一维高斯分布:随机变量 X X X服从数学期望 μ \mu μ、方差 σ 2 \sigma^2 σ2的正态分布,概率密度函数为 p ( x ) p(x) p(x)
    X ∼ N ( μ , σ 2 ) X \sim N(\mu, \sigma^2) XN(μ,σ2)
    p ( x ) = 1 2 π σ e x p ( − 1 2 ( x − μ ) 2 σ 2 ) p(x) = \frac{1}{\sqrt{2\pi}\sigma}exp(-\frac{1}{2}\frac{(x-\mu)^2}{\sigma^2}) p(x)=2π σ1exp(21σ2(xμ)2)
  • 高维高斯分布:数学期望 μ \mu μ 1 × n 1×n 1×n的矩阵, Σ \Sigma Σ n × n n×n n×n的协方差矩阵。
    X ∼ N ( μ , Σ ) X \sim N(\mu, \Sigma) XN(μ,Σ)
    p ( x ) = 1 ( 2 π ) n d e t ( Σ ) e x p ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) p(x) = \frac{1}{\sqrt{(2\pi)^ndet(\Sigma)}}exp(-\frac{1}{2}(x - \mu)^T\Sigma^{-1}(x-\mu)) p(x)=(2π)ndet(Σ) 1exp(21(xμ)TΣ1(xμ))

注:

  1. 总体方差 σ 2 \sigma^2 σ2
    x i x_i xi是表示第 i i i个样本的观测值, N N N是总体样本数量, n n n是采样样本数量, μ \mu μ是总体均值( μ = 1 N Σ i = 1 N x i \mu=\frac{1}{N}{\Sigma}_{i=1}^{N}x_i μ=N1Σi=1Nxi), x ˉ \bar{x} xˉ是采样样本均值( x ˉ = 1 n Σ i = 1 n x i \bar{x}=\frac{1}{n}{\Sigma}_{i=1}^{n}x_i xˉ=n1Σi=1nxi)。根据n个采样样本的方差估计总体方差,有如下两种情况。
    (1)如果总体均值 μ \mu μ已知,那么总体方差:
    σ 2 = Σ i = 1 N ( x i − μ ) 2 N (1) \sigma^2=\frac{{\Sigma}_{i=1}^N(x_i-\mu)^2}{N} \tag{1} σ2=NΣi=1N(xiμ)2(1)
    (2)在总体均值未知的情况下,根据样本方差估计总体方差,则分为有偏估计和无偏估计。
  • 有偏估计:
    σ 2 = Σ i = 1 n ( x i − x ˉ ) 2 n (2) \sigma^2=\frac{{\Sigma}_{i=1}^n(x_i-\bar{x})^2}{n} \tag{2} σ2=nΣi=1n(xixˉ)2(2)
  • 无偏估计
    σ 2 = Σ i = 1 n ( x i − x ˉ ) 2 n − 1 (3) \sigma^2=\frac{{\Sigma}_{i=1}^n(x_i-\bar{x})^2}{n-1} \tag{3} σ2=n1Σi=1n(xixˉ)2(3)
  1. 协方差
    在无偏估计的基础上,协方差的定义如下,其中 x ˉ \bar{x} xˉ y ˉ \bar{y} yˉ分别表示两个随机变量对应的观测样本均值。
    σ ( x , y ) = 1 n − 1 Σ i = 1 n ( x i − x ˉ ) ( y i − y ˉ ) ) (4) \sigma(x,y)=\frac{1}{n-1}\Sigma_{i=1}^{n}(x_i-\bar{x})(y_i-\bar{y})) \tag{4} σ(x,y)=n11Σi=1n(xixˉ)(yiyˉ))(4)
    则公式(3)可以写成如下格式,表示随机变量 x x x关于其自身的协方差。
    σ 2 = Σ i = 1 n ( x i − x ˉ ) 2 n − 1 = σ ( x , x ) \sigma^2=\frac{{\Sigma}_{i=1}^n(x_i-\bar{x})^2}{n-1} =\sigma(x,x) σ2=n1Σi=1n(xixˉ)2=σ(x,x)
  2. 协方差矩阵
    给定d个随机变量 X = { x 1 , x 2 , x 3 , . . . , x d } X=\{x_1, x_2, x_3, ..., x_d\} X={x1,x2,x3,...,xd},有 x k ∈ X , k = 1 , 2 , . . . , d x_k\in{X},k=1,2,...,d xkX,k=1,2,...,d,则这些随机变量的方差为:
    σ ( x k , x k ) = 1 n − 1 Σ i = 1 n ( x k i − x ˉ k ) \sigma(x_k, x_k)=\frac{1}{n-1}\Sigma_{i=1}^n(x_{ki}-{\bar{x}}_k) σ(xk,xk)=n11Σi=1n(xkixˉk)
    其中, x k i x_{ki} xki表示第 k k k个随机变量 x k x_{k} xk的第 i i i个观测值, n n n表示采样样本数量,每个随机变量均采样了 n n n个样本。对于这些随机变量,求两两之间的协方差,则有:
    σ ( x m , x k ) = 1 n − 1 Σ i = 1 n ( x m i − x ˉ m ) ( x k i − x ˉ k ) \sigma(x_m, x_k)=\frac{1}{n-1}\Sigma_{i=1}^n(x_{mi}-{\bar{x}}_m)(x_{ki}-{\bar{x}}_k) σ(xm,xk)=n11Σi=1n(xmixˉm)(xkixˉk)
    所以,协方差矩阵为:
    Σ = [ σ ( x 1 , x 1 ) . . . σ ( x 1 , x d ) ⋮ ⋱ ⋮ σ ( x d , x 1 ) . . . σ ( x d , x d ) ] ∈ R d × d \Sigma = \begin{bmatrix} \sigma(x_1, x_1) & ... & \sigma(x_1, x_d)\\ \vdots & \ddots & \vdots \\ \sigma(x_d, x_1) & ... & \sigma(x_d, x_d)\end{bmatrix} \in{\R^{d×d}} Σ= σ(x1,x1)σ(xd,x1)......σ(x1,xd)σ(xd,xd) Rd×d

参考:总体方差的有偏与无偏估计、高斯分布、如何理解多维高斯分布?

二、卡尔曼滤波

经典线性卡尔曼滤波推导

【参考:一文看懂卡尔曼滤波、卡尔曼滤波算法详细推导】

  • 建模
    已知一辆汽车直线行驶,已知它在 t − 1 t-1 t1时刻的位置是 p t − 1 p_{t-1} pt1,速度是 v t − 1 v_{t-1} vt1,加速度是 u u u t t t时刻与 t − 1 t-1 t1时刻的时间差为 Δ t \Delta_{t} Δt。理想情况下,根据速度、位移、加速度之间的关系,它在 t t t时刻的位置 p t p_{t} pt和速度 v t v_{t} vt 可以表述为:
    v t = v t − 1 + u Δ t (1) v_t=v_{t-1}+u\Delta_{t} \tag{1} vt=vt1+uΔt(1)
    p t = p t − 1 + u t − 1 Δ t + 1 2 u ( Δ t ) 2 (2) p_t=p_{t-1}+u_{t-1}\Delta_{t}+\frac{1}{2} u{(\Delta_{t})}^2\tag{2} pt=pt1+ut1Δt+21u(Δt)2(2)

直接根据上一时刻状态预测下一时刻的状态得到的预测结果没有考虑过程误差和传感器测量误差。实际情况误差是存在的,卡尔曼滤波即是通过观测信息寻求一个最优的状态估计。上式写成矩阵形式:
[ p t v t ] = [ 1 Δ t 0 1 ] [ p t − 1 v t − 1 ] + [ ( Δ t ) 2 2 Δ t ] u (3) \begin{bmatrix} p_t \\ v_t \end{bmatrix}=\begin{bmatrix} 1 & {\Delta}_t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{t -1}\\ v_{t -1} \end{bmatrix} + \begin{bmatrix} \frac{({\Delta}_t)^2}{2} \\ {\Delta}_t \end{bmatrix}u \tag{3} [ptvt]=[10Δt1][pt1vt1]+[2(Δt)2Δt]u(3)
即可写成:
x t = F x t − 1 + B u (4) x_t=Fx_{t-1}+Bu \tag{4} xt=Fxt1+Bu(4)
考虑过程误差 w t w_t wt,上式可写成:
x t = F x t − 1 + B u + w t (5) x_t=Fx_{t-1}+Bu+w_{t} \tag{5} xt=Fxt1+Bu+wt(5)
通过传感器观测可对 t t t时刻的速度和位置进行测量,假设直接预测结果与传感器观测的结果之间是线性变换关系,并考虑传感器测量误差 v t v_t vt,则观测方程可写为:
z t = H x t + v t (6) z_t=Hx_t+v_t \tag{6} zt=Hxt+vt(6)
一般假设过程误差和观测误差服从高斯分布,即 p ( W ) ∼ N ( 0 , Q ) p(W)\sim{N(0,Q)} p(W)N(0,Q) p ( V ) ∼ N ( 0 , R ) p(V)\sim{N(0,R)} p(V)N(0,R) Q Q Q R R R为协方差矩阵,即 Q = E [ W W T ] Q=E[WW^T] Q=E[WWT] R = E [ V V T ] R=E[VV^T] R=E[VVT] Q Q Q R R R为超参数,根据经验设定,推导过程中认为已知。

  • 变量定义
    x t x_t xt:状态真实值
    x ~ t − {\tilde{x}_t^-} x~t:直接根据上一时刻的状态预测的结果(没有考虑观测信息,先验)
    x ~ t \tilde{x}_t x~t:根据观测结果进行优化得到的最优估计值(考虑观测信息,后验)
    先验误差: e t − = x t − x ~ t − {e_t^-}=x_t-{\tilde{x}_t^-} et=xtx~t
    后验误差: e t = x t − x ~ t e_t=x_t-\tilde{x}_t et=xtx~t
    先验误差协方差矩阵: P t − = E [ e t − ( e t − ) T ] {P_t^-}=E[e_t^-({{e}_t^-)^T}] Pt=E[et(et)T]
    后验误差协方差矩阵: P t = E [ e t e t T ] {P_t}=E[e_t{{e}_t^T}] Pt=E[etetT]
  • 公式推导
  1. 真实状态空间
    状态真实值: x t = F x t − 1 + B u + w t x_t = Fx_{t-1}+Bu+w_t xt=Fxt1+Bu+wt,其中 p ( W ) ∼ N ( 0 , Q ) , Q = E [ W W T ] p(W)\sim{N(0,Q)}, Q=E[WW^T] p(W)N(0,Q),Q=E[WWT]
    状态观测值: z t = H x t + v t z_t=Hx_t+v_t zt=Hxt+vt,其中 p ( V ) ∼ N ( 0 , R ) , R = E [ V V T ] p(V)\sim{N(0,R)}, R=E[VV^T] p(V)N(0,R),R=E[VVT]
  2. 实际状态空间
    直接预测值: x ~ t − = F x ~ t − 1 + B u \tilde{x}_t^-=F\tilde{x}_{t-1}+Bu x~t=Fx~t1+Bu
    校正后状态: x ~ t = x ~ t − + K t ( z t − H x ~ t − ) \tilde{x}_t=\tilde{x}_t^-+{K_t}(z_t-H\tilde{x}_t^-) x~t=x~t+Kt(ztHx~t),其中 K t ( z t − H x ~ t − ) {K_t}(z_t-H\tilde{x}_t^-) Kt(ztHx~t)表示对预测值的修正

寻找 K t K_t Kt使 x ~ t \tilde{x}_t x~t尽可能接近 x t x_t xt,即使后验误差 e t = x t − x ~ t e_t=x_t-\tilde{x}_t et=xtx~t最小化。

。。。待敲公式

卡尔曼增益:
后验估计:
更新误差协方差:

扩展卡尔曼理解参考:卡尔曼滤波器之扩展卡尔曼滤波

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

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

相关文章

前言:为什么学习鸿蒙设备开发

HarmonyOS 是什么?众所周知,HarmonyOS 鸿蒙操作系统是由华为研发的一款面向未来、面向全场景的分布式操作系统,它旨在为各种设备提供统一的操作系统解决方案,从智能手机、平板电脑到智能穿戴设备、智能家居、汽车等各类终端设备都…

Go语言数值类型教程

Go语言提供了丰富的数值类型,包括整数类型、浮点类型和复数类型。每种类型都有其特定的用途和存储范围。下面将详细介绍这些类型,并附带示例代码。 原文链接: Go语言数值类型教程 - 红客网-网络安全与渗透技术 1. 整数类型 原文链接&#xf…

字符画生成网站 ascii字符画

_____ / ___/__ ___ / /__/ _ \/ _ \ \___/ .__/ .__//_/ /_/ font推荐:1.Slant 2.Small 3.Small slant https://patorjk.com/software/taag/#pdisplay&fSmall%20Slant&tCpp https://www.kammerl.de/ascii/AsciiSignature.php https://asciia…

MySQL索引设计遵循一系列原则

高频查询与大数据量表:对查询频次较高且数据量较大的表建立索引。这是因为索引主要是为了加速查询过程,对于经常需要访问的表和数据,索引的效果最为显著。 选择合适索引字段:从WHERE子句中提取最佳候选列作为索引字段&#xff0c…

社交媒体数据恢复:华为畅连

尊敬的用户您好,以下是关于社交软件华为畅连的聊天记录数据恢复教程。在华为手机中,我们可以通过华为云服务和第三方软件来恢复删除的聊天记录。以下是详细的步骤: 第一步:登录华为云服务 请在您的华为手机上找到并打开“云服务”…

网络编程:服务器模型-并发服务器-多进程

并发服务器概念: 并发服务器同一时刻可以处理多个客户机的请求 设计思路: 并发服务器是在循环服务器基础上优化过来的 (1)每连接一个客户机,服务器立马创建子进程或者子线程来跟新的客户机通信 (accept之后…

SpringBoot报空指针错:java.lang.NullPointerException

虽然报空指针错误的原因可能有很多种,但是我还是写上我的报错原因,以此与各位共勉~ 在这里提前说一句,AI虽然强大,但是还是要谨慎使用啊(血的教训)~ 这里先截图我错误的地方: 前端能成功传进来值,后台控制…

图像质量评价指标:了解图像质量的度量方式

图像质量评价指标:了解图像质量的度量方式 在图像处理和计算机视觉领域,评价图像质量的准确性对于许多应用至关重要。通过合适的评价指标,我们可以量化图像的质量,从而更好地了解图像处理算法的效果和改进空间。本文将介绍图像质…

英语学习笔记12——名词所有格的运用

Whose is this … ? This is my/your/his/her … 这……是谁的?这是我的 / 你的 / 他的 / 她的…… Whose is that … ? That is my/your/his/her … 那……是谁的?那是我的 / 你的 / 他的 / 她的…… 词汇 Vocabulary father n. 爸爸 口语&#xf…

2024统计建模成品论文39页(附带完整数据集和代码)

2024统计建模成品论文完整版一等奖论文【1.5w字全网最佳】2024统计建模大赛高质量成品论文39页配套完整代码运行全套数据集https://www.jdmm.cc/file/2710661/

在idea中使用vue

一、安装node.js 1、在node.js官网(下载 | Node.js 中文网)上下载适合自己电脑版本的node.js压缩包 2、下载完成后进行解压并安装,一定要记住自己的安装路径 一直点击next即可,这部选第一个 3、安装成功后,按住winR输入…

如何使用 ArcGIS Pro 计算容积率

容积率是指地上建筑物的总面积与用地面积的比率,数值越小越舒适,这里为大家介绍一下如何使用ArcGIS Pro 计算容积率,希望能对你有所帮助。 数据来源 教程所使用的数据是从水经微图中下载的建筑和小区数据,除了建筑和小区数据&am…

【408精华知识】速看!各种排序的大总结!

文章目录 一、插入排序(一)直接插入排序(二)折半插入排序(三)希尔排序 二、交换排序(一)冒泡排序(二)快速排序 三、选择排序(一)简单选…

【Arduino】数字I/O的使用

目录 1、引脚工作模式 2、写入引脚digitaWrite() 3、读取引脚digitalRead(pin); 4、示例 跑马灯 1、引脚工作模式 Arduino通过pinMode()设置引脚的io工作模式,一共有4种模式 工作模式 Mode 说明 输出模式 OUTPUT 引脚为低阻抗状态&…

YOLOv9改进策略目录 | 包含卷积、主干、检测头、注意力机制、Neck上百种创新机制

👑 YOLOv9有效涨点专栏目录 👑 专栏视频介绍:包括专栏介绍、得到的项目文件、模型二次创新、权重文件的使用问题,点击即可跳转。 前言 Hello,各位读者们好 本专栏自开设两个月以来已经更新改进教程50余篇其中包含Re…

foxmail简介

Foxmail是一款电子邮件客户端软件,它最初由中国的程序员张小龙开发,后来被腾讯公司收购并继续开发。Foxmail以其轻量级、易于使用和强大的邮件管理功能而受到用户的喜爱。 1. **起源**:Foxmail在1997年由张小龙开发,最初是作为一…

自动攻丝机进出料激光检测 进料出料失败报警循环手动及关闭报警退出无限循环

/**************进料检测********************/ /***缺料无限次循环 手动退出 超时报警*******/ void check_Pon() // { zstatus0; //报警计数器归零 Signauto1; …

Pytorch 与 Tensorflow:深度学习的主要区别(1)

引言 目前,Python 深度学习领域已经涌现出多个由科技界巨头如 Google、Facebook 和 Uber 等公司公开发布的框架,这些框架旨在帮助开发者构建先进的计算架构。对于刚接触这一领域的你来说,深度学习是计算机科学中的一个分支,它通过…

如何查看centos7是否安装nginx

要查看 CentOS 7 系统上是否安装了 Nginx,您可以使用多种方法来检查。以下是一些常见的方法: 通过 RPM 包管理器查询 在 CentOS 系统上,可以使用 RPM 包管理器来查询已安装的软件包。要查看是否安装了 Nginx,您可以在终端中运行以…

福建在线教育系统搭建,培训班如何宣传招生?有几种方法?

培训行业近年发展的十分快速,现在很多孩子都会报几个兴趣培训班,教培行业门槛比较低,市场需求比较大,越来越多的人涌入这个行业,培训班之间竞争也越来越激烈,那培训班如何宣传招生? 传统线下招生…