14 卡尔曼滤波及代码实现

文章目录

    • 14 卡尔曼滤波及代码实现
      • 14.0 基本概念
      • 14.1 公式推导
      • 14.2 代码实现

14 卡尔曼滤波及代码实现

14.0 基本概念

卡尔曼滤波是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。

通俗来说就是,线性数学模型算出预测值+传感器测量值=更准确的测量值。根据数学模型,由第 k k k 时刻的值递推得到第 k + 1 k+1 k+1 时刻的预测值,结合第 k + 1 k+1 k+1 时刻的观测值,得到第 k + 1 k+1 k+1 时刻更精准的值。

在这里插入图片描述

卡尔曼滤波主要用于 线性高斯系统

14.1 公式推导

(1)线性高斯系统表达

状态方程:

x k = A x k − 1 + B u k + w k \boldsymbol{x}_k = \boldsymbol{A}\boldsymbol{x}_{k-1}+\boldsymbol{B}\boldsymbol{u}_k+\boldsymbol{w}_k xk=Axk1+Buk+wk

观测方程:

z k = H x k + v k \boldsymbol{z}_k = \boldsymbol{H}\boldsymbol{x}_k+\boldsymbol{v}_k zk=Hxk+vk

其中, x k \boldsymbol{x}_k xk 为状态量, z k \boldsymbol{z}_k zk 为观测量, A \boldsymbol{A} A 为状态转移矩阵, B k \boldsymbol{B}_k Bk 为控制输入矩阵, H \boldsymbol{H} H 为状态观测矩阵。

w k \boldsymbol{w}_k wk 是过程噪声,服从高斯分布, w k \boldsymbol{w}_k wk 是观测噪声,也服从高斯分布,即:

w k ∼ N ( 0 , Q ) \boldsymbol{w}_k \sim N(0, \boldsymbol{Q}) wkN(0,Q)

v k ∼ N ( 0 , R ) \boldsymbol{v}_k \sim N(0, \boldsymbol{R}) vkN(0,R)

其中 Q \boldsymbol{Q} Q 是过程噪声的协方差, R \boldsymbol{R} R 是观测噪声的协方差。

卡尔曼滤波包括预测和更新两步。

(2)预测(先验)

预测是根据上一时刻的状态量,由状态方程预测出下一时刻的状态量 x ^ k − \hat{\boldsymbol{x}}_k^{-} x^k ,以及状态量误差协方差的先验估计矩阵 P k − \boldsymbol{P}_k^{-} Pk。这是没有加观测值的。

x ^ k − = A x ^ k − 1 + B u k \hat{\boldsymbol{x}}_k^{-} = \boldsymbol{A}\hat{\boldsymbol{x}}_{k-1}+\boldsymbol{B}\boldsymbol{u}_k x^k=Ax^k1+Buk

P k − = A P k − 1 A T + Q \boldsymbol{P}_k^{-}=\boldsymbol{AP}_{k-1}\boldsymbol{A}^T+\boldsymbol{Q} Pk=APk1AT+Q

其中, A x ^ k − 1 \boldsymbol{A}\hat{\boldsymbol{x}}_{k-1} Ax^k1 是上一时刻的最优估计。

(3)更新(后验)

加入观测,对预测值进行更新校正,得到最优后验估计。

首先计算增益矩阵

K k = P k − H T ( H P k − H T + R ) − 1 \boldsymbol{K}_k=\boldsymbol{P}_k^{-}\boldsymbol{H}^T(\boldsymbol{H}\boldsymbol{P}_k^{-}\boldsymbol{H}^T+\boldsymbol{R})^{-1} Kk=PkHT(HPkHT+R)1

更新状态量及其协方差矩阵

x ^ k = x ^ k − + K k ( z k − H x ^ k − ) \hat{\boldsymbol{x}}_k = \hat{\boldsymbol{x}}_k^{-} + \boldsymbol{K}_k(\boldsymbol{z}_k-\boldsymbol{H}\hat{\boldsymbol{x}}_k^{-}) x^k=x^k+Kk(zkHx^k)

P k = ( I − K k H ) P k − \boldsymbol{P}_k=(\boldsymbol{I}-\boldsymbol{K}_k\boldsymbol{H})\boldsymbol{P}_k^{-} Pk=(IKkH)Pk

在这里插入图片描述

14.2 代码实现

以雷达追踪目标为背景,系统的状态方程为

[ x y V x V y a x a y ] k + 1 = [ 1 0 δ t 0 0.5 δ t 2 0 0 1 0 δ t 0 0.5 δ t 2 0 0 1 0 δ t 0 1 0 0 1 0 δ t 0 0 0 0 1 0 0 0 0 1 0 1 ] [ x y V x V y a x a y ] k \begin{bmatrix}x\\y\\Vx\\Vy\\ax\\ay\end{bmatrix}_{k+1}=\begin{bmatrix}1&0&\delta_t&0&0.5\delta_t^2&0\\0&1&0&\delta_t&0&0.5\delta_t^2\\0&0&1&0&\delta_t&0\\1&0&0&1&0&\delta_t\\0&0&0&0&1&0\\0&0&0&1&0&1\end{bmatrix}\begin{bmatrix}x\\y\\Vx\\Vy\\ax\\ay\end{bmatrix}_k xyVxVyaxay k+1= 100100010000δt010000δt01010.5δt20δt01000.5δt20δt01 xyVxVyaxay k

观测方程

[ x y ] k + 1 = [ 1 0 0 0 0 0 0 1 0 0 0 0 ] [ x y V x V y a x a y ] k \begin{bmatrix}x\\y\end{bmatrix}_{k+1}=\begin{bmatrix}1&0&0&0&0&0\\0&1&0&0&0&0\end{bmatrix}\begin{bmatrix}x\\y\\Vx\\Vy\\ax\\ay\end{bmatrix}_k [xy]k+1=[100100000000] xyVxVyaxay k

/***********************************************************                                          *
* Time: 2023/11/26
* Author: xiaocong
* Function: 卡尔曼滤波
***********************************************************/
#ifndef KALMANFILTER_H
#define KALMANFILTER_H#include <eigen3/Eigen/Dense>
#include <iostream>using namespace Eigen;
using namespace std;class KalmanFilter
{
public:KalmanFilter(int stateSize, int measSize, int uSize);               // 构造函数void init(VectorXd& x, MatrixXd& P, MatrixXd& R, MatrixXd& Q);      // 初始化void predict(MatrixXd& A);void predict(MatrixXd& A, MatrixXd& B, VectorXd& u);            // 重载,针对有控制输入的情况VectorXd update(MatrixXd& H, VectorXd z_meas);                      // 更新~KalmanFilter();                                                    // 析构函数private:VectorXd x_;         // 状态变量VectorXd z_;         // 观测变量MatrixXd A_;         // 状态转移矩阵MatrixXd B_;         // 控制矩阵VectorXd u_;         // 控制变量MatrixXd P_;         // 状态值的协方差矩阵MatrixXd H_;         // 观测矩阵MatrixXd R_;         // 观测噪声协方差矩阵MatrixXd Q_;         // 过程噪声协方差矩阵
};#endif //KALMANFILTER_H
#include "../inlude/KalmanFilter.h"// 构造函数
KalmanFilter::KalmanFilter(int stateSize, int measSize, int uSize)
{if (stateSize == 0 || measSize == 0){std::cerr << "Error, State size and measurement size must bigger than 0" << endl;}x_.resize(stateSize);x_.setZero();A_.resize(stateSize, stateSize);A_.setIdentity();u_.resize(uSize);u_.setZero();B_.resize(stateSize, uSize);B_.setZero();P_.resize(stateSize, stateSize);P_.setIdentity();H_.resize(measSize, stateSize);H_.setZero();Q_.resize(stateSize, stateSize);Q_.setIdentity();R_.resize(measSize, measSize);R_.setIdentity();
}void KalmanFilter::init(VectorXd& x, MatrixXd& P, MatrixXd& R, MatrixXd& Q)
{x_ = x;P_ = P;R_ = R;Q_ = Q;
}void KalmanFilter::predict(MatrixXd& A)         // 没有控制输入u
{A_ = A;x_ = A * x_;P_ = A_ * P_ * A_.transpose() + Q_;
}void KalmanFilter::predict(MatrixXd& A, MatrixXd& B, VectorXd& u)       // 有控制输入u
{A_ = A;B_ = B;u_ = u;x_ = A * x_ + B * u_;P_ = A_ * P_ * A_.transpose() + Q_;
}VectorXd KalmanFilter::update(MatrixXd& H, VectorXd z_meas)      // 更新
{H_ = H;MatrixXd temp = H_ * P_ * H_.transpose() + R_;MatrixXd K = P_ * H_.transpose() * temp.inverse();x_ = x_ + K * (z_meas - H_ * x_);               // 更新 x_kMatrixXd I = MatrixXd::Identity(x_.rows(), x_.rows());P_ = (I - K * H_) * P_;return x_;
}KalmanFilter::~KalmanFilter()
{}
#include "../inlude/KalmanFilter.h"
#include <fstream>#define N 1000
#define T 0.01double data_x[N], data_y[N];// 模型函数
double sample(double x0, double v0, double acc, double t)
{return x0 + v0 * t + 0.5 * acc * t * t;
}double getRand()
{return 0.5 * rand() / RAND_MAX - 0.25;   //[-0.25, 0.25)
}int main()
{ofstream fout;fout.open("../data/data.txt");// 生成观测值double t;for (int i = 0; i < N; i++){t = T * i;data_x[i] = sample(0, -4.0, 0.1, t) + getRand();data_y[i] = sample(0.1, 2.0, 0, t) + getRand();}int stateSize = 6;int measSize = 2;int uSize = 0;KalmanFilter kf(stateSize, measSize, uSize);Eigen::MatrixXd A(stateSize, stateSize);A << 1, 0, T, 0, 1 / 2 * T * T, 0,0, 1, 0, T, 0, 1 / 2 * T * T,0, 0, 1, 0, T, 0,0, 0, 0, 1, 0, T,0, 0, 0, 0, 1, 0,0, 0, 0, 0, 0, 1;Eigen::MatrixXd B(0, 0);Eigen::MatrixXd H(measSize, stateSize);H << 1, 0, 0, 0, 0, 0,0, 1, 0, 0, 0, 0;Eigen::MatrixXd P(stateSize, stateSize);P.setIdentity();Eigen::MatrixXd R(measSize, measSize);R.setIdentity() * 0.01;Eigen::MatrixXd Q(stateSize, stateSize);Q.setIdentity() * 0.001;Eigen::VectorXd x(stateSize);Eigen::VectorXd u(0);Eigen::VectorXd z_meas(measSize);z_meas.setZero();Eigen::VectorXd res(stateSize);         // 存储预测结果for (int i = 0; i < N; i++){if (i == 0)         // 初始值{x << data_x[i], data_y[i], 0, 0, 0, 0;kf.init(x, P, R, Q);continue;}kf.predict(A);                         // 预测z_meas << data_x[i], data_y[i];        // 观测res << kf.update(H, z_meas);           // 更新fout << data_x[i] << " " << data_y[i] << " " << res[0] << " " << res[1] << " " << res[2] << " " << res[3] << " " << res[4] << " " << res[5] << endl;}fout.close();return 0;}

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

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

相关文章

【Linux】服务器被work32病毒入侵CPU占用99%

文章目录 一、问题发现二、问题解决2.1 清楚病毒2.2 开启防火墙2.3 修改SSH端口2.4 仅使用凭据登录&#xff08;可选&#xff09; 一、问题发现 我的一台海外服务器&#xff0c;一直只运行一项服务&#xff08;你懂的&#xff09;&#xff0c;但是前不久我发现CPU占用99%。没在…

PTA:7-12 斐波那契数列

斐波那契数列 (FibonacciSequence)&#xff0c;又称黄金分割数列&#xff0c;因数学家莱昂纳多斐波那契 (LeonardoFibonacci) 以兔子繁殖为例子而引入&#xff0c;故又称为“兔子数列”&#xff0c;指的是这样一个数列&#xff1a;1,1,2,3,5,8,13,21,⋯ 在数学上&#xff0c;斐…

如何用Go语言,实现基于宏系统的解释器?

目录 一、Go语言介绍二、什么是宏系统三、什么是解释器四、如何用Go语言实现一个基于宏系统的解释器&#xff1f; 一、Go语言介绍 Go语言&#xff0c;又称为Golang&#xff0c;是一种由谷歌公司开发并开源的编程语言。Go语言的设计目标是提高程序员的生产力&#xff0c;同时具…

MFC扩展库BCGControlBar Pro v35.0新版亮点 - 工具栏、菜单全新升级

BCGControlBar库拥有500多个经过全面设计、测试和充分记录的MFC扩展类。 我们的组件可以轻松地集成到您的应用程序中&#xff0c;并为您节省数百个开发和调试时间。 BCGControlBar专业版 v35.0已全新发布了&#xff0c;这个版本改进类Visual Studio 2022的视觉主题、增强对多个…

算法力扣刷题记录 二十三【151.翻转字符串里的单词】

前言 字符串篇&#xff0c;继续。 记录 二十三【151.翻转字符串里的单词】 – 一、题目阅读 给你一个字符串 s &#xff0c;请你反转字符串中 单词 的顺序。 单词 是由非空格字符组成的字符串。s 中使用至少一个空格将字符串中的 单词 分隔开。 返回 单词 顺序颠倒且 单词…

【04】从0到1构建AI生成思维导图应用 -- 创建 AI 工作流

【04】从0到1构建AI生成思维导图应用 – 创建 AI 工作流 大家好&#xff01;最近自己做了一个完全免费的AI生成思维导图的网站&#xff0c;支持下载&#xff0c;编辑和对接微信公众号&#xff0c;可以在这里体验&#xff1a;https://lt2mind.zeabur.app/ 上一章&#xff1a;h…

centos7安装mysql8-zabbix6.4

MySQL rpm -qa | grep mysql #查看是否已经安装 Mysql rpm -qa | grep mariadb #查看是否已经安装 mariadb,CentOS 7可视化安装会默认安装该数据库,安装MySQL前需要卸载该数据库 rpm -e --nodeps mariadb-libs #删除mariadb数据库找到对应linux的版本进行下载 […

从零开始:Spring Boot 中使用 Drools 规则引擎的完整指南

规则引擎作用 规则引擎主要用于将业务逻辑从应用程序代码中分离出来&#xff0c;提高系统的灵活性和可维护性。规则引擎通过预定义的规则来处理输入数据并做出相应的决策&#xff0c;从而实现业务逻辑的自动化和动态调整。 例如 门店信息校验&#xff1a;美团点评在门店信息…

【高中数学之基本不等式】已知:a,b皆为正实数且1/a+1/(b+2)=1/2 求:a+b的最小值?

解&#xff1a;先从1/a1/(b2)1/2 入手&#xff0c;看能否化二为一&#xff08;将两变量化成一个变量&#xff09; 由1/a1/(b2)1/2 两边通分得(b2a)/a/(b2)1/2 交叉相乘得2a2b4ab2a 最后得到a24/b 所以ab24/bb 此时已经可以用基本不等式了 ab24/bb>22*根号下(4/b*b)22…

SpringBoot 3.3.1 + Minio 实现极速上传和预览模式

统一版本管理 <properties><minio.version>8.5.10</minio.version><aws.version>1.12.737</aws.version><hutool.version>5.8.28</hutool.version> </properties><!--minio --> <dependency><groupId>io.m…

Arduino - TM1637 4 位 7 段显示器

Arduino - TM1637 4 位 7 段显示器 Arduino-TM1637 4 位 7 段显示器 A standard 4-digit 7-segment display is needed for clock, timer and counter projects, but it usually requires 12 connections. The TM1637 module makes it easier by only requiring 4 connectio…

有哪些防爬虫的方法

防爬虫的方法有robots.txt文、user-agent过滤、ip限制、验证码、动态页面生成、频率限制、动态url参数和反爬虫技术等。详细介绍&#xff1a;1、robots.txt文件&#xff0c;用于告诉搜索引擎爬虫哪些页面可以访问&#xff0c;哪些页面禁止访问&#xff1b;2、ip限制&#xff0c…

关于vs code中Live Server插件安装后无法打开的问题

一、问题情况 安装好Live Server插件之后&#xff0c;点击open with live server只会出现界面右下角落的提示&#xff0c;但是不会跳转到浏览器的页面&#xff1a;如下所示&#xff1a; 二&#xff1a;解决步骤 1、首先进行扩展设置&#xff0c;默认将浏览器的设置为chrome浏览…

vue组件全局注册

描述&#xff1a; vue组件的注册分为局部和全局注册两部分&#xff0c;局部注册相对容易&#xff0c;不做赘述&#xff1b;而不同框架的注册方法又有所不同&#xff0c;下面针对vite框架和vue-cli框架的注册分别进行说明 vue组件全局注册 一、vite框架中全局组件注册二、Vue-cl…

-bash: /snap/bin/docker: 没有那个文件或目录

-bash: /snap/bin/docker: 没有那个文件或目录 解决办法 export PATH$PATH:/usr/bin/docker然后&#xff0c;重新加载配置文件 source ~/.bashrc

线程的等待通知机制

等待通知机制 之前所学到的join是等待线程结束,而此时的等待通知,等待代码给我们提示进行显示的通知(并不一定要结束),可以更加精细控制线程之间的执行顺序,在系统内部,线程是抢占式执行,随机调度,但是程序员也是有手段可以进行干预的,我们可以通过"等待"的方式让线…

【学术日记】关于读博,目标院校,意向导师,毕业要求,重要时间点

文章目录 一、目标院校二、重要时间点西安交通大学意向导师 华南理工大学意向导师 本文记录博主的科研日记。如果对博主的其他文章感兴趣&#xff0c;可以看这篇文章【CSDN文章】晚安66博客文章索引。 首次修改时间&#xff1a;2024年5月12日。当前修改时间&#xff1a;2024年5…

C : 线性规划例题求解

Submit Page TestData Time Limit: 1 Sec Memory Limit: 128 Mb Submitted: 93 Solved: 49 Description 求解下述线性规划模型的最优值min &#xfffd;1&#xfffd;1&#xfffd;2&#xfffd;2&#xfffd;3&#xfffd;3&#xfffd;.&#xfffd;. &…

Spring Cloud LoadBalancer基础入门与应用实践

官网地址&#xff1a;https://docs.spring.io/spring-cloud-commons/reference/spring-cloud-commons/loadbalancer.html 【1】概述 Spring Cloud LoadBalancer是由SpringCloud官方提供的一个开源的、简单易用的客户端负载均衡器&#xff0c;它包含在SpringCloud-commons中用…

前端vue使用onlyoffice控件实现word在线编辑、预览(仅列出前端部分需要做的工作,不包含后端部分)

简介 ONLYOFFICE 文档 是一个开源办公套件&#xff0c;包括文本文档、电子表格、演示文稿和可填写表单的编辑器。 它提供以下功能&#xff1a; 创建、编辑和查看文本文档、电子表格、演示文稿和可填写表单&#xff1b; 与其他队友实时协作处理文件。 基于这个控件&#xff0c;…