控制算法学习 一、卡尔曼滤波（1）以小车为例
控制算法学习 一、卡尔曼滤波（1）以小车为例
- 前言
- 线性系统
- 状态方程和观测方程
- 举例
- 卡尔曼滤波
- 状态预测
- 状态更新
- 卡尔曼滤波流程图
前言
卡尔曼滤波（Kalman Filter, KF）是最经典的传感器滤波/去噪/估计/预测算法，适用于线性系统的最优状态估计，并且衍生出扩展卡尔曼滤波（EKF）、迭代卡尔曼滤波（IKF），无迹卡尔曼滤波（UKF）和容积卡尔曼滤波（CKF）等。
本文将非常简单的介绍和推导卡尔曼滤波，加深KF的理解记忆。
线性系统
什么是线性系统？控制领域中，系统输入输出满足叠加原理（叠加性和齐次性）就是线性系统。
一个简单的例子，一维路径上运行的匀速小车，时间（输入）和位置（输出）就是线性的。
状态方程和观测方程
如果只知道小车的初始速度，怎么获得小车在任一时刻的位置呢？
第一种方法是直接通过匀速计算： p = v ∗ t p=v*t p=v∗t，但是现实中路面可能会有崎岖，导致结果出现偏差；
第二种方法是在小车上加传感器（比如GPS），但GPS也会有测量误差；
因此以上两种方法就分别可以用状态方程和观测方程来表示：
x k = A x k − 1 + B u k − 1 + w k − 1 , w k ∼ [ 0 , Q ] z k = H x k + v k , v k ∼ [ 0 , R ] \begin{aligned} & x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}, w_k\sim [0, Q] \\ & z_k = Hx_k+v_{k} ,v_k\sim[0, R] \end{aligned} xk=Axk−1+Buk−1+wk−1,wk∼[0,Q]zk=Hxk+vk,vk∼[0,R]
A , B , H A,B,H A,B,H分别表示状态转移矩阵，控制（输入）转换矩阵，观测转换矩阵。
w k , v k w_k,v_k wk,vk都是高斯白噪声，表示了系统误差（过程噪声）和测量误差。
x k , z k x_k,z_k xk,zk是向量，表明k时刻的系统状态。
举例
对于匀速运动的小车而言，可以这样建模：
状态方程
[ p v ] t = [ 1 Δ t 0 1 ] [ p v ] t − 1 + w k − 1 \left [ \begin{matrix} p \\ v \\ \end{matrix} \right ]_{t}= \left [ \begin{matrix} 1 & \Delta t \\ 0 & 1 \end{matrix} \right ] \left [ \begin{matrix} p \\ v \end{matrix} \right ]_{t-1} \ + w_{k-1} [pv]t=[10Δt1][pv]t−1 +wk−1
观测方程（z是GPS读数）
[ z ] t = [ 1 ] [ p ] t + v k \left [ \begin{matrix} z \\ \end{matrix} \right ]_{t}= \left [ \begin{matrix} 1 \\ \end{matrix} \right ] \left [ \begin{matrix} p \end{matrix} \right ]_{t} \ + v_{k} [z]t=[1][p]t +vk
卡尔曼滤波
KF有两个流程，先预测，再更新，其实就是把预测得到的状态与观测得到的状态，以某种方式进行加权更新，得到状态的最优估计。
状态预测
预测过程有两个公式：
x ~ k = A x k − 1 + B u k − 1 ( 1 ) P ~ k = A P k − 1 A T + Q ( 2 ) x k = [ p v ] P k = [ σ p p σ p v σ v p σ v v ] \begin{aligned} & \tilde x_k = Ax_{k-1}+Bu_{k-1} &\quad (1)\\ & \tilde P_k=AP_{k-1}A^T+Q &\quad (2)\\ & x_k= \left [ \begin{matrix} p \\ v \\ \end{matrix} \right ] \quad P_{k}= \left [ \begin{matrix} \sigma_{pp} & \sigma_{pv} \\ \sigma_{vp} & \sigma_{vv} \\ \end{matrix} \right ] \end{aligned} x~k=Axk−1+Buk−1P~k=APk−1AT+Qxk=[pv]Pk=[σppσvpσpvσvv](1)(2)
这里增加了一个状态协方差矩阵 P P P（对称矩阵），用来描述状态量之间的关系。
这两个公式描述了：
(1) 将k-1时刻的状态 x k − 1 x_{k-1} xk−1，通过状态转移矩阵和控制转换矩阵，预测k时刻的状态 x k x_k xk;
(2) 将k-1时刻的状态协方差矩阵 P k − 1 P_{k-1} Pk−1，通过状态转移矩阵和过程噪声，预测k时刻的状态协方差矩阵 P k − 1 P_{k-1} Pk−1；
公式（1）容易理解，公式（2）需要从随机过程上解释：
X = [ x 1 , x 2 , … , x n ] T , Y = [ y 1 , y 2 , … , y n ] T X=[x_1,x_2,\dots,x_n]^T, Y=[y_1,y_2,\dots,y_n]^T X=[x1,x2,…,xn]T,Y=[y1,y2,…,yn]T， X , Y X,Y X,Y独立，则有 P ( A X + Y ) = A P X A T + P Y P(AX+Y)=AP_XA^T+P_Y P(AX+Y)=APXAT+PY
还是用小车作为例子，公式（1）其实就是在用匀速运动模型，通过上一时刻小车的位置预测当前小车的位置。公式（2）则是通过上一时刻小车的速度和位置关系，预测当前速度和位置的关系，并且叠加过程噪声带来的不确定性 Q Q Q
状态更新
更新过程有三个公式：
K k = P ~ k H T ( H P k H T + R ) − 1 x k = x ~ k + K k ( z k − H x ~ k ) P k = ( E − K k H ) P ~ k \begin{aligned} & K_k = \tilde P_kH^T(HP_kH^T+R)^{-1} \\ & x_k = \tilde x_k + K_k(z_k - H \tilde x_k) \\ & P_k = (E-K_kH)\tilde P_k \end{aligned} Kk=P~kHT(HPkHT+R)−1xk=x~k+Kk(zk−Hx~k)Pk=(E−KkH)P~k
这里增加了卡尔曼增益 K k K_k Kk，实际上就是预测和观测权重。
这三个公式描述了：
（1）当前卡尔曼增益的计算；
（2）通过卡尔曼增益，进行预测\观测值的加权；
（3）通过卡尔曼增益，更新协方差矩阵；
本质上，以上三个公式是对预测状态和观测状态这两个高斯分布进行相乘，获得的高斯分布，就是最优状态估计。所谓的卡尔曼增益，就是预测状态的高斯分布与融合后的高斯分布的不确定性（协方差矩阵）的比值。
卡尔曼滤波流程图
最后用一张图来描述预测和更新过程：
控制算法学习 一、卡尔曼滤波（1）以小车为例相关推荐
- 控制算法学习 四、扩展卡尔曼滤波EKF
控制算法学习 四.扩展卡尔曼滤波EKF 前言 非线性系统 状态/观测方程线性化 扩展卡尔曼滤波EKF 后记 前言 经典卡尔曼滤波的使用场景是线性系统,但现实应用时,大多数系统都是非线性的.扩展卡尔曼滤 ...
- 【按键精灵学习记录】以DOTA2人机为例
[按键精灵学习记录]以DOTA2人机为例 一.写在前面 二.准备工作 1.环境准备 2.需求分析 3.流程图 三.代码实现 1.命令分析 2.代码实现 状态判断 主循环 英雄选择 游戏中 3.实测 四 ...
- 【Pixhawk】PX4源码控制逻辑详解(以UGV小车为例)
PX4启动流程 主体控制逻辑 commander:飞行模式控制 stickmapper:摇杆映射 position_estimator:从GPS获得的位置估计 navigator:读取航点,产生期望位 ...
- 【论文写作】Latex命令学习，以ACL2020论文模板为例
[论文写作]Latex命令学习,以ACL2020论文模板为例 1.项目准备工作 2.项目文件熟悉 2.1 .tex文件说明 2.2 .bib文件说明 2.3 .cls文件说明 2.4 .sty文件说明 ...
- 学习OpenCV2——卡尔曼滤波(KalmanFilter)详解
本文将简要回顾一下卡尔曼滤波理论,然后详细介绍如何在OpenCV中使用卡尔曼滤波进行跟踪,最后给两个程序实例. 1. 卡尔曼滤波理论回顾 对于一个动态系统,我们首先定义一组状态空间方程 状态方程: ...
- 《probabilistic robotics》学习笔记——卡尔曼滤波
机器人系列文章目录 第一章 ubuntu16.04系统及相关软件安装 第二章 Linux操作系统入门笔记 第三章 ROS系统架构及程序编写过程 第四章 <probabilistic roboti ...
- DRL深度强化学习代码实战1——MountainCar-v0小车成功登顶
如果你是刚刚接触DRL的同学,那么本文的内容就是引你入胜的第一个小实验. DRL的学习离不开理论知识,但更不能缺少实践!废话不多说我们直接开始. 0. 实验环境和包版本 ① PyCharm 2022 ...
- 机器人学习--扩展卡尔曼滤波算法用于机器人定位
论文链接:leonard_ieeetroa19.pdf-互联网文档类资源-CSDN下载https://download.csdn.net/download/GGY1102/61742571 基于EKF ...
- 毕业论文 | 基于MPU6050及卡尔曼滤波的平衡小车设计（源代码与设计文档）
博主github:https://github.com/MichaelBeechan 博主CSDN:https://blog.csdn.net/u011344545 预告:代码及文档下载 一.文档主要 ...
最新文章
- Springmvc+Easyui 搜索，新增，删除，修改
- mysql 按条件导出数据_mysql命令行按任意查询条件导出指定数据
- 8个实用的Python内置函数
- P6154 游走 概率dp
- P1307 [NOIP2011 普及组] 数字反转（python3实现）
- 使用live555制作rtsp客户端，捕获h264等解码
- GlusterFS分布式文件系统
- python定义一个空列表lt_4. Python 基础之阶段测试（一）
- 阿里云盾证书服务助力博客装逼成功
- PostgreSQL数据类型（中文手册）
- PMP/高项 项目范围管理实践
- 按键精灵脚本：采集鼠标当前所在坐标的颜色
- max7456 C语言,如何使用8位单片机对16位INT型数据进行操作
- 关于Navicat 连接 RDS数据库
- USB设备被识别流程
- 电脑组成部件介绍图解之电脑由哪些部件组成？各个部件作用又是什么 ？主要功能是什么？
- 深入了解区块链的漏洞之1：介绍篇
- MobileNet相关知识整理
- 酷睿计算机系统吗,酷睿i3和i5的区别是什么？电脑处理器i3和i5的区别介绍
- 攻防世界（PWN）forgot