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,一经查实,立即删除!

相关文章

如何利用PHP实现爬虫监控

爬虫监控是一种用来跟踪目标网站内容变化的技术&#xff0c;通常用于网站内容更新或者数据采集。php作为一种优秀的开发语言&#xff0c;在实现爬虫监控方面也有着不俗的能力。本文将介绍如何利用php实现爬虫监控的常用方法。 设置爬取目标 在开始爬虫监控之前&#xff0c;需…

【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的视觉主题、增强对多个…

EXPLAIN--SQL执行计划各个参数含义

目录 一、简介 二、详细介绍 三、意义 一、简介 EXPLAIN是SQL语句中用于分析和显示执行计划的命令&#xff0c;主要用于帮助理解查询是如何被数据库优化器处理的。在不同的数据库系统中&#xff08;如MySQL、PostgreSQL、SQLite等&#xff09;&#xff0c;EXPLAIN的具体语法…

算法力扣刷题记录 二十三【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…

监控Linux/Windows服务器CPU、内存使用率TOP5的服务进程

主题&#xff1a;监控Linux/Windows服务器CPU、内存使用率TOP5的服务进程 内容&#xff1a; 一、Windows系统监控CPU、内存使用率TOP5的进程 1、编写脚本monitor.ps1 # 获取CPU使用率前5的服务进程,无法直接获取使用率&#xff0c;该值为“进程已用于所有处理器的处理器时间…

go语言怎么向kafka推送消息?

在Go语言中&#xff0c;你可以使用confluent-kafka-go&#xff08;也称为librdkafka的Go客户端&#xff09;或segmentio/kafka-go等第三方库来与Apache Kafka交互&#xff0c;并向其推送&#xff08;或生产&#xff09;消息。以下是使用confluent-kafka-go库向Kafka推送消息的简…

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浏览…

深入解析 Redisson分布式锁看门狗机制

一、Redisson分布式锁概述 1.1 分布式锁的意义 在分布式系统中&#xff0c;多个节点可能同时访问共享资源&#xff0c;导致数据不一致或竞态条件。分布式锁通过协调不同节点对共享资源的访问&#xff0c;确保数据的一致性和并发访问的安全性。 1.2 Redisson分布式锁的优势 …

探索iOS开发语言基础与Xcode工具:从零开始构建你的第一个iOS应用

目录 1. iOS开发语言基础 1.1 Swift语言基础 1.1.1 变量和常量 1.1.2 数据类型 1.1.3 控制流 1.1.4 函数 1.1.5 类和结构体 1.2 Objective-C语言基础 1.2.1 语法和数据类型 1.2.2 控制流 1.2.3 函数和方法 1.2.4 类和对象 2. 初探Xcode工具 2.1 Xcode的安装 2.2…

Apache Doris 2.0.12 版本正式发布

亲爱的社区小伙伴们&#xff0c;Apache Doris 2.0.12 版本已于 2024 年 6 月 27 日正式与大家见面&#xff0c;该版本提交了 99 个改进项以及问题修复&#xff0c;欢迎大家下载体验。 官网下载&#xff1a; https://doris.apache.org/download/ GitHub 下载&#xff1a; http…