卡尔曼滤波详细推导

2023-05-16

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。卡尔曼滤波器的本质是线性最小均方误差估计，而均方误差是协方差矩阵的迹。卡尔曼滤波有好几种公式推导方法，本文从最小二乘估计的方法推导卡尔曼滤波过程。

卡尔曼滤波有以下几个特点：

(1)卡尔曼滤波处理的对象时随机信号

(2)被处理信号无有用和干扰之分，滤波的目的是要估计出所有被处理信号

(3)系统的白噪声激励和量测噪声并不是需要滤除的对象，它们的统计特性正式估计过程中需要利用的信息

所以确切的说卡尔曼滤波是属于最优估计理论，与常规滤波器具有完全不同的概念和含义，就实际运用而言卡尔曼滤波实质上

是由一套数字计算机实现的递推算法，每个递推周期中包含对被估计量的时间更新和量测更新过程。

1、系统的变量定义

$x(k)$ 为系统的真值

$z(k)$ 为系统的测量值

$k(k)$ 为卡尔曼滤波增益

$A$ 系统的状态转移矩阵

$B$ 为系统的输入增益矩阵

$H$ 为系统的量测矩阵

$P(k|k)$ 为最优估计协方差矩阵

$P(k|k-1)$ 为预测值协方差矩阵

$x(k|k)$ 为k时刻的最优估计值

$x(k|k-1)$ 为k时刻的预测值，系统的后验概率估计

$x(k-1|k-1)$ 为k-1时刻的最优估计值

$u(k)$ 为系统的测量噪声，其均值 $E(u(k)) = 0$ ，协方差矩阵为R且服从正态分布的测量噪声

$w(k)$ 为系统的过程噪声，其均值 $E(w(k)) = 0$ ，协方差矩阵为Q且服从正态分布的过程噪声

2、系统的状态方程

$x(k) = Ax(k-1) + Bu(k) + w(k)$ ......................................................................(1)

$z(k) = Hx(k) + u(k)$ .......................................................................................(2)

$x(k|k-1) = Ax(k-1|k-1) + Bu(k)$ .........................................................(3)

$x(k|k) = x(k|k-1) + k(k)(z(k) - Hx(k|k-1))$ ........................................(4)

公式(1)为系统状态转移方程，(2)为系统的测量方程，(3)为预测方程（后验概率估计），(4)为最佳估计方程

卡尔曼滤波器最终从上述公式求解出最优估计值 $x(k|k)$ ，在求解最优估计值时首先需要推导出公式(4)的卡尔

曼滤波增益 $k(k)$ ，具体的公式推导见下节

3、卡尔曼滤波推导

遵循误差最小原则，定义误差 $e(k) = x(k) - x(k|k)$ 求误差最小化， $P(k|k)$ 为误差的协方差矩阵

$P(k|k) = E[(x(k) - x(k|k))(x(k)-x(k|k))^{T}]$ ................................................(5)

把误差 $e(k) = x(k) - x(k|k)$ 中的最优估计值 $x(k|k)$ 用公式(4)替代

$e(k) = x(k) - \left \{ x(k|k-1) + k(k)(z(k) - Hx(k|k-1)) \right \}$ .............................(6)

上述误差方程中的 $z(k)$ 用公式(2) $z(k) = Hx(k) + u(k)$ 代替得出：

$e(k) = x(k) - \left \{ x(k|k-1) + k(k)(Hx(k) + u(k) - Hx(k|k-1)) \right \}$

$= [x(k) - k(k)Hx(k)) -(x(k|k-1) - k(k)Hx(k|k-1)] - k(k)u(k)$

$= (I - k(k)H)x(k) - (I - k(k)H)x(k|k-1) - k(k)u(k)$

$= [I - k(k)H][x(k) - x(k|k-1)] - k(k)u(k)$ ................................................(7)

$e(k)e(k)^{T} = [I - k(k)H][x(k) - x(k|k-1)][x(k) - x(k|k-1)]^{T}[I - k(k)H]^{T}$

$- [I - k(k)H][x(k) - x(k|k-1)]u(k)^{T}k(k)^{T}$

$-k(k)u(k)[x(k)-x(k|k-1)]^{T}[I-Hx(k)]^{T}+k(k)u(k)u(k)^{T}k(k)^{T}$

由于 $[x(k) - x(k|k-1)]$ 与 $u(k)$ 无关，并且 $E[u(k)] = 0$

$E[[x(k) - x(k|k-1)][x(k) - x(k|k-1)]^{T}] = P(k|k-1)$ 所以 $P(k|k)$ 协方差矩阵如下：

$P(k|k) = E[e(k)e(k)^{T}]= [I - k(k)H]P(k|k-1)[I - k(k)H]^{T} + k(k)Rk(k)^{T}$ ...........(8)

综上所示我们得到了公式(8)，我们需对公式(8)求 $k(k)$ 的导数，使之 $P(k|k)$ 最小。

我们需将公式(8)展开如下所示：

$P(k|k) = P(k|k-1) - (k(k)HP(k|k-1))^{T} - k(k)HP(k|k-1) \\+ k(k)[HP(k|k-1)H + R]k(k)^{T}$ ....................(9)

卡尔曼滤波器的本质是线性最小均方误差估计，而均方误差是协方差矩阵的迹，所以我们

只需对 $P(k|k)$ 的迹求导 $\frac{\partial tr(P(k|k))}{\partial k(k)}$ 。由于 $tr(A^{T}) = tr(A)$ ，所以 $tr(P(k|k-1))$ 如下：

$tr(P(k|k-1)) = tr(P(k|k-1)) - 2tr[k(k)HP(k|k-1)] \\+ tr[k(k)[HP(k|k-1)H + R]k(k)^{T}]$ .................................(10)

对迹求导我们需要知道以下几个矩阵迹求导公式：

$\frac{\partial tr(AB)}{\partial A} = B^{T}$ 若矩阵B是对称阵则 $\frac{\partial tr(ABA^{T})}{\partial A} = 2AB$ 其中tr表示矩阵的迹。

$\frac{\partial tr(P(k|k))}{\partial k(k)} = - 2P(k|k-1)^{T}H^{T} + 2k(k)[HP(k|k-1)H + R]$ ...............................(11)

令 $\frac{\partial tr(P(k|k))}{\partial k(k)} = 0$ 得出： $k(k) = \frac{P(k|k-1)H^{T}}{HP(k|k-1)H^{T} + R}$ ..........................................(12)

把公式(12)的表达式替换公式(9)最后一项 $k(k)[HP(k|k-1)H + R]k(k)^{T}$ 的 $k(k)$ 得到以下式子：

$P(k|k) = P(k|k-1) - (k(k)HP(k|k-1))^{T} - k(k)HP(k|k-1) \\+ P(k|k-1)H^{T}k(k)^{T}$

$= P(k|k-1) - (k(k)HP(k|k-1))^{T} - k(k)HP(k|k-1) \\+ (k(k)HP(k|k-1))^{T}$

$= P(k|k-1) - k(k)HP(k|k-1) = (I - k(k)H)P(k|k-1)$ ..................................(13)

当 $k(k)$ 的取值等于公式(12)时，最优估计值的协方差矩阵 $P(k|k)$ 方差最小。但该公式中有一个

未知的量 $P(k|k-1)$ 预测协方差矩阵，接下去我们将就继续推导 $P(k|k-1)$ 的表达式。

定义预测误差 ${e}' = x(k) - x(k|k-1)$ ，预测协方差矩阵 $p(k|k-1)=e^{'}(e^{'})^{T}$

式子 $x(k)$ 用公式(1)替代， $x(k|k-1)$ 用公式(3)替代得出：

${e}' = Ax(k-1) + Bu(k) + w(k) - Ax(k-1|k-1) - Bu(k)$

$= Ax(k-1) - Ax(k-1|k-1) + w(k)$

$p(k|k-1)=e^{'}(e^{'})^{T}$ $= A(x(k-1) - x(k-1|k-1))(x(k-1) - x(k-1|k-1))^{T}A^{T}$

$+ A(x(k-1) - x(k-1|k-1))w(k)^{T} + w(k)(x(k-1) - x(k-1|k-1))^{T}A^{T}$

$+ w(k)w(k)^{T}$

由于 $w(k)$ 与 $(x(k-1) - x(k-1|k-1))$ 无关，且 $E[w(k)] = 0$ 所以：

$p(k|k-1)=A[x(k-1)-x(k-1|k-1)][x(k-1)-x(k-1|k-1)]^{T}A^{T}$

$+w(k)w(k)^{T}$

又 $p(k-1|k-1)=[x(k-1)-x(k-1|k-1)][x(k-1)-x(k-1|k-1)]^{T}$

$w(k)w(k)^{T}=Q$

所以 $p(k|k-1)=Ap(k-1|k-1)A^{T}+Q$ ......................................................(14)

综上所示卡尔曼滤波的核心公式如下：

$x(k|k-1) = Ax(k-1|k-1) + Bu(k)$ 预测方程

$x(k|k) = x(k|k-1) + k(k)(z(k) - Hx(k|k-1))$ 最优估计方程

$k(k) = \frac{P(k|k-1)H^{T}}{HP(k|k-1)H^{T} + R}$ 卡尔曼滤波增益

$P(k|k) = (I - k(k)H)P(k|k-1)$ 最优估计协方差矩阵递推公式

$P(k|k-1) = AP(k-1|k-1)A^{T} + Q$ 预测协方差矩形递推公式

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

卡尔曼滤波详细推导的相关文章

已知一条线段的两端点A(x1,y1),B(x2,y2)及线段外的任意一点P(px,py),设计一个程序找出线段中距离p最近的点

include lt stdio h gt typedef struct float x float y Point typedef struct Point p1 Point p2 Line segment void initLine L
LL库实现定时器输出4路不同频率pwm（输出比较方法）

最近有用到定时器的输出功能 xff0c 可以通过这个功能使1个定时器输出4路不同频率的pwm xff0c 这里做一下记录 xff0c 防止以后自己忘记了下面直接看初始化 xff1a 这里我是使用定时器1通道1 xff0c 也就是PA8引脚
网络摄像头100万.200万.300万.400万.500万分辨率多少？

常见的分辨率大致有以下几种 xff1a 500W 2560 1920 300W 2048 1536 200W 1920 1080 1080P 200W 1600 1200 130W 1280 960 100W 1280 720 1 1920
ROS CmakeLists.txt解读

CMakeLists txt原本是Cmake编译系统的规则文件 xff0c 而Catkin编译系统基本沿用了CMake的编译风格 xff0c 只是针对ROS工程添加了一些宏定义所以在写法上 xff0c catkin的CMakeLists
虚拟机中加载urdf模型到gazebo中，却不显示模型？

取消使用该3D加速就可以正常显示
gazebo执行Save World As 保存世界界面卡住不动的解决办法

背景 xff1a 在学习ROS Gazebo仿真 xff0c gazebo中加入任意模型 xff0c 然后点击save world as然后卡死的问题一直无法解决现象 xff1a 点save as world屏幕变暗 xff0c 无窗口弹
rviz中机器人的位姿和gazebo中机器人的位姿不一致

当在运行ROS机器人gazebo和rviz联合仿真的时候 xff0c rviz中机器人的位姿和gazebo中机器人的位姿不一致 xff0c 解决方案 xff1a 可以使用rviz上方工具栏中的2D Pose estimate 工具 xff
零基础CMake入门（基于ROS）

cmake实践实战示例项目 xff1a 实战示例项目1 xff1a 简单项目实战示例项目2 xff1a 复杂项目 xff0c 附源码基础知识铺垫 xff1a GNU C 和 C 43 43 编译器分别称为gcc和g 43 43 xff
Gazebo仿真，模型东倒西歪、上天、乱动的异常问题

可以修改一下机器人模型的各部分的质量 xff0c 有时候导入模型中的参数mass过大会导致标题中出现的问题 xff0c 如果对mass无过高的要求 xff0c 可以改小一点单位为kg xff0c 即可正常显示我改的是机器人上方激光的质量
Git系列七：Git下载指定分支、Git下载历史版本

Git下载指定分支 b 43 分支名称 43 仓库地址 git clone b melodic devel https github com ros planning navigation Git下载历史版本分两步 xff1a 1 查看历
Ubuntu18.04+ROS安装Kinect V2步骤及问题解决

参考鸣谢 xff1a http www cnblogs com hitcm p 5118196 html https blog csdn net wuguangbin1230 article details 77184032 安装Kinec
一文搞懂结构体变量占用多大内存空间（详细）

一首先做一道例题感受一下请问下面定义的结构体变量aa在计算机内存中占用多少字节 xff1f span class token macro property span class token directive hash span spa
10_22笔试题

1 以下关于进程线程协程的的说法正确的是 xff1f xff08 xff09 不定项选择题 A 进程是操作系统能够进行运算调度的最小单位 B 线程是堆共享 xff0c 栈私有 C 线程是堆私有 xff0c 栈共享 D 现代操作系统中 x
echarts和highcharts的区别

ECharts 官网地址 https echarts apache org examples zh index html Apache ECharts incubating 是由百度捐给 Apache 开源基金会的开源项目 xff0c 目前
ROS-7.3.创建和运行服务服务器与客户端节点

7 3创建和运行服务服务器与客户端节点介绍创建功能包修改功能包配置文件修改构建配置文件创建服务文件创建服务服务器节点创建服务客户端节点构建节点运行服务服务器运行服务客户端rosservice call命令的用法GUI工具Service c
【物联网 · 嵌入式】使用 ESP8266 接入 Blinker 物联网平台

一介绍 Blinker 是一个简单易用的物联网平台 xff0c 只需要几行代码 xff0c 就可以开发出物联网应用这里我们使用 NodeMCU xff08 一种基于esp8266的开发板 xff09 接入Blinker平台 NodeMC
【电路收藏夹】AMS1117稳压电路

电路图说明 10uF 61 10622uF 61 226100nF 61 104 AMS1117管脚图注意 AMS1117最大输入电压为15V 致正在学习嵌入式的小伙伴们 xff1a 嵌入式的学习是要基础知识 43 动手实践同步进行的
【Python】利用Python拟合函数曲线

使用Python拟合函数曲线需要用到一些第三方库 xff1a numpy xff1a 科学计算的基础库 xff08 例如 xff1a 矩阵 xff09 matplotlib xff1a 绘图库 scipy xff1a 科学计算库如果没有安
【嵌入式 · 单片机】一文带你搞懂电机驱动模块

文章目录 1 基本原理1 1 控制电机转速1 2 控制电机旋转方向 2 H桥驱动2 1 96 H桥驱动 96 名称的由来2 2 H桥驱动控制电机旋转方向2 3 H桥驱动 3 电机驱动模块3 1 两个控制引脚的电机驱动3 2 三个控制引脚的电
【Python】基于OpenCV与UDP实现的视频流传输

文章目录前言原理代码服务端客户端运行效果参考资料前言 2021年电赛的测量题 xff08 如下 xff09 需要实现局域网视频传输 xff0c 我们的方案是使用gst rtsp server 搭建 RTSP 服务器进行视频推流电赛

随机推荐

【Bug收集箱 · 前端】Vite报错：Failed to load module script: Expected a JavaScript module script but the...

错误信息使用 Vite 打包的网页白屏 xff0c 控制台报错 Failed to load module script Expected a JavaScript module script but the server respond
Ubuntu18.04安装PX4并与ROS联合实验

1 如果没有安装ROS xff0c 可以去PX4官网使用脚本链接将ROS和PX4一起安装我不推荐这样 xff0c 因为装不全 xff0c 所以建议先自己装好ROS xff0c 然后参考本教程 2 如果已安装ROS melodic xff0
【计算机视觉 · OpenCV】使用 OpenCV 调用手机摄像头

一前言 Droidcam 是一款可以将手机变成网络摄像头的工具 xff0c 我们可以利用 Droidcam 让 OpenCV 拥有调用手机摄像头的能力二步骤 2 1 安装 DroidCam 在手机和电脑上分别安装 DroidCam 的
【嵌入式 · 机器人】在 Linux 下安装 ROS2（机器人操作系统）

安装要求安装 ROS2 需要保证 Linux 系统的发行版是 Ubuntu xff0c 并且不同版本的 Ubuntu 只能安装对应版本的 ROS2 系统不是 Ubuntu 怎么办 xff1f 系统是 Mac OS 或 Windows xf
【踩坑日记 · 前端】为 Excalidraw 添加中文手写字体

文章目录前言中文手写字体步骤1 下载源代码和中文字体2 注册字体3 预加载字体资源4 增加字体枚举5 添加字体切换按钮部署测试参考资料前言 Excalidraw 是一款完全免费的手绘风格绘图在线应用 xff0c 能快速画出漂亮的流程图
【嵌入式·单片机】老生常谈：学习单片机和嵌入式是否需要学习算法？

文章目录前言什么是算法嵌入式是否需要算法如何学习算法参考资料前言刚学习单片机或嵌入式的同学可能会听说过算法这个词 xff0c 部分电子信息类专业的同学也会在自己的培养计划上看到数据结构与算法这门课程 xff0c 这个时候我们可
【程序员的数学】从几何角度理解矩阵

以下笔记笔记只是简单记录了自己对矩阵的理解 xff0c 主要参考了 3Blue1Brown 的线性代数的本质 xff0c 想学习更多相关内容建议观看此视频 xff08 官方双语合集线性代数的本质系列合集哔哩哔哩 bilibili
【计算机视觉·OpenCV】使用Haar+Cascade实现人脸检测

前言人脸检测的目标是找出图像中所有的人脸对应的位置 xff0c 算法的输出是人脸的外接矩形在图像中的坐标使用 haar 特征和 cascade 检测器进行人脸检测是一种传统的方式 xff0c 下面将给出利用 OpenCV 中的 haar
TX2查看设备信息命令汇总

内存 free m 系统内核 uname a CPU信息 lscpu USB设备 lsusb CPU占用情况 sudo apt get install htop htop 查看cudnn版本 xff1a cat usr include cu
蓝牙：蓝牙协议

蓝牙协议学习整理 xff08 一 xff09 蓝牙的概述转自 xff1a https blog csdn net guoxiaolongonly article details 78414870 传送门 xff1a xff08 一 xff
Thinkphp 6.0数据库的时间查询

本节课我们要单独学习一下时间的所有查询方式 xff0c 包括传统式快捷方式和固定查询等一 xff0e 传统方式 1 可以使用 gt lt gt 61 lt 61 来筛选匹配时间的数据 xff1b Db name 39 user 39 g
Java中基本数据类型和包装类型的区别

1 包装类是对象 xff0c 拥有方法和字段 xff0c 对象的调用都是通过引用对象的地址 xff1b 基本类型不是 2 包装类型是引用的传递 xff1b 基本类型是值的传递 3 声明方式不同 xff1a 基本数据类型不需要new关键字 x
git diff如何退出

git diff 对比两次文件修改了什么但如何退出呢 xff1f 按q即可
数据结构，计算机网络，数据库，计算机组成原理，操作系统有哪些好的网课值得推荐?

大家好 xff0c 我是小林哥作为自学CS过来的老学长 xff0c 看过中国mooc b站网易云课堂很多视频 xff0c 期间踩了不少坑 xff0c 这次掏心掏肺前来跟分享下 xff0c 网上的资源是免费的 xff0c 但是找到质量好的
MATLAB中im2bw函数-将图像转换为二值图像

matlab中DIP工具箱函数im2bw使用阈值 xff08 threshold xff09 变换法把灰度图像 xff08 grayscale image xff09 转换成二值图像所谓二值图像 xff0c 一般意义上是指只有纯黑 xff
Ubuntu18.04使用RPLIDAR A2M12雷达出错的解决办法

最近领导要我用A2M12雷达搞SLAM xff0c 但是用电脑连上这个雷达捣鼓了两三天才能够拿到数据就把踩的坑记录一下软硬件平台 Nvidia Jetson Nano xff08 4GB版本的 xff09 Ubuntu 18 04 报错
workerman 连接失败可能的原因

刚开始使用workerman时很常见的一个问题是客户端连接服务端失败原因一般如下 xff1a 1 服务器防火墙包括云服务器安全组阻止了连接 xff08 50 几率是这个 xff09 2 客户端和服务端使用的协议不一致 xff08 30
排序算法：冒泡排序和选择排序的思路，区别与优缺点。

一 xff0c 冒泡排序 xff1a 冒泡排序的定义就不提了 xff0c 总结起来就一句话 xff08 划重点 xff09 xff1a xff0c 从左到右 xff0c 数组中相邻的两个元素进行比较 xff0c 将较大的放到后面算法思路
ROS创建功能包并自定义消息

ROS有时需要自定义消息 xff0c 本文叙述如何通过创建功能包并自定义消息创建ROS工作空间具体实现 xff1a https blog csdn net qq 34911636 article details 100103448 创建一
卡尔曼滤波详细推导

卡尔曼滤波 xff08 Kalman filtering xff09 是一种利用线性系统状态方程 xff0c 通过系统输入输出观测数据 xff0c 对系统状态进行最优估计的算法 xff0c 由于观测数据中包括系统中的噪声和干扰的影响 xff

卡尔曼滤波详细推导

卡尔曼滤波详细推导 的相关文章

随机推荐

热门标签

卡尔曼滤波详细推导的相关文章