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.rhkb.cn/news/367791.html

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

相关文章

React Native V0.74 — 稳定版已发布

嗨,React Native开发者们, React Native 世界中令人兴奋的消息是,V0.74刚刚在几天前发布,有超过 1600 次提交。亮点如下: Yoga 3.0New Architecture: Bridgeless by DefaultNew Architecture: Batched onLayout UpdatesYarn 3 for New Projects让我们深入了解每一个新亮点…

移动智能终端数据安全管理方案

随着信息技术的飞速发展&#xff0c;移动设备已成为企业日常运营不可或缺的工具。特别是随着智能手机和平板电脑等移动设备的普及&#xff0c;这些设备存储了大量的个人和敏感数据&#xff0c;如银行信息、电子邮件等。员工通过智能手机和平板电脑访问企业资源&#xff0c;提高…

【vue3】【vant】 移动端中国传统文化和民间传说案例

更多项目点击&#x1f446;&#x1f446;&#x1f446;完整项目成品专栏 【vue3】【vant】 移动端中国传统文化和民间传说案例 获取源码方式项目说明&#xff1a;其中功能包括项目包含&#xff1a;项目运行环境运行截图和视频 获取源码方式 加Q群&#xff1a;632562109项目说…

Linux_管道通信

目录 一、匿名管道 1、介绍进程间通信 2、理解管道 3、管道通信 4、用户角度看匿名管道 5、内核角度看匿名管道 6、代码实现匿名管道 6.1 创建子进程 6.2 实现通信 7、匿名管道阻塞情况 8、匿名管道的读写原子性 二、命名管道 1、命名管道 1.1 命名管道通信 …

源代码层面分析Appium-inspector工作原理

Appium-inspector功能 Appium Inspector 基于 Appium 框架&#xff0c;Appium 是一个开源工具&#xff0c;用于自动化移动应用&#xff08;iOS 和 Android&#xff09;和桌面应用&#xff08;Windows 和 Mac&#xff09;。Appium 采用了客户端-服务器架构&#xff0c;允许用户通…

C++初学者指南-3.自定义类型(第一部分)-异常

C初学者指南-3.自定义类型(第一部分)-异常 文章目录 C初学者指南-3.自定义类型(第一部分)-异常简介什么是异常&#xff1f;第一个示例用途:报告违反规则的行为异常的替代方案标准库异常处理 问题和保证资源泄露使用 RAII 避免内存泄漏&#xff01;析构函数&#xff1a;不要让异…

Taogogo Taocms v3.0.2 远程代码执行漏洞(CVE-2022-25578)

前言 CVE-2022-25578 是一个存在于 Taogogo Taocms v3.0.2 中的代码注入漏洞。此漏洞允许攻击者通过任意编辑 .htaccess 文件来执行代码注入。 漏洞详情 漏洞描述&#xff1a;攻击者可以利用此漏洞上传一个 .htaccess 文件到网站&#xff0c;并在文件中注入恶意代码&#xf…

CesiumJS【Basic】- #058 绘制网格填充多边形(Entity方式)-使用shader

文章目录 绘制网格填充多边形(Entity方式)-使用shader1 目标2 代码2.1 main.ts绘制网格填充多边形(Entity方式)-使用shader 1 目标 使用Entity方式绘制绘制网格填充多边形 - 使用shader 2 代码 2.1 main.ts import * as Cesium from cesium;// 创建 Cesium Viewer 实例…

主流国产服务器操作系统技术分析

主流国产服务器操作系统 信创 "信创"&#xff0c;即信息技术应用创新&#xff0c;作为科技自立自强的核心词汇&#xff0c;在我国信息化建设的进程中扮演着至关重要的角色。自2016年起步&#xff0c;2020年开始蓬勃兴起&#xff0c;信创的浪潮正席卷整个信息与通信技…

程序员AI提效案例:统计B站课程耗时情况

文章目录 一&#xff0c;时长统计需求二&#xff0c;一波三折三&#xff0c;终极方案 AIJava总结 今天为了写一篇博客&#xff0c;这篇博客介绍了B站的一个Java项目&#xff0c;这个项目分为三个阶段&#xff1a; 初级篇高级篇运维篇 一&#xff0c;时长统计需求 我想根据每个…

Spring+SpringMVC+MyBatis整合

目录 1.SSM介绍1.1 什么是SSM&#xff1f;1.2 SSM框架1.2.1 Spring1.2.2 SpringMVC1.2.3 MyBatis 2.SSM框架整合2.1 建库建表2.2 创建工程2.3 pom.xml2.4 log4j.properties2.5 db.properties2.6 applicationContext-dao.xml2.7.applicationContext-tx.xml2.8 applicationContex…

昇思25天学习打卡营第9天|静态图模式的深度剖析与应用指南

目录 背景介绍 动态图模式 静态图模式 静态图模式的使用场景 静态图模式开启方式 基于装饰器的开启方式 基于context的开启方式 静态图的语法约束 JitConfig配置选项 静态图高级编程技巧 背景介绍 AI 编译框架主要包含两种运行模式&#xff0c;即动态图模式与静态图模…

Docker(八)-Docker运行mysql8容器实例

1.运行mysql8容器实例并挂载数据卷 -e:配置环境变量 --lower_case_table_names1 设置忽略表名大小写一定要放在镜像之后运行mysql8容器实例之前&#xff0c;先查看是否存在mysql8镜像以及是否存在已运行的mysql实例docker run -d -p 3306:3306 --privilegedtrue -v 【宿主机日…

【windows】电脑如何关闭Bitlocker硬盘锁

如果你的硬盘显示这样的一把锁&#xff0c;说明开启了Bitlocker硬盘加密。 Bitlocker硬盘锁&#xff0c;可以保护硬盘被盗&#xff0c;加密防止打开查看数据。 方法一&#xff1a;进入“控制面板->BitLocker 驱动器加密”进行设置。或者“控制面板\系统和安全->BitLocke…

数据库对比脚本,java如何对比两个数据库的表字段的不同

因为有时候开发环境和 测试环境&#xff0c;有时候会有不同的数据库表&#xff0c;比如有些加字段了&#xff0c;所以这个脚本就实现了对比两个数据库连接的数据库到底哪里不一样&#xff0c;输出到控制台 package com.junfun.pms;import lombok.extern.slf4j.Slf4j;import ja…

SQL执行慢排查以及优化思路

数据库服务器的优化步骤 当我们遇到数据库调优问题的时候&#xff0c;该如何思考呢&#xff1f;我把思考的流程整理成了下面这张图。 整个流程划分成了观察&#xff08;Show status&#xff09;和行动&#xff08;Action&#xff09;两个部分。字母 S 的部分代表观察&#xf…

Android常用加解密算法总结

Android开发中对于数据的传输和保存一定会使用加密技术&#xff0c;加密算法是最普遍的安保手段&#xff0c;多数情况数据加密后在需要使用源数据时需要再进行解密&#xff0c;但凡是都有例外。下面从可逆加密、不可逆、不纯粹加密三种方式记录一下常见的加解密算法。 加密技术…

HDFS学习

3.5 HDFS存储原理 3.5.1 冗余数据保存 作为一个分布式文件系统&#xff0c;为了保证系统的容错性和可用性&#xff0c;HDFS采用了多副本方式对数据进行冗余存储&#xff0c;通常一个数据块的多个副本会被分布到不同的数据节点上。 如图所示&#xff0c;数据块1被分别存放到…

Eslint与Prettier搭配使用

目录 前置准备 Eslint配置 Prettier配置 解决冲突 前置准备 首先需要安装对应的插件 然后配置settings.json 点开之后就会进入settings.json文件里&#xff0c;加上这两个配置 // 保存的时候自动格式化 "editor.formatOnSave": true, // 保存的时候使用prettier进…

【Qt之·类QTableWidget】

系列文章目录 文章目录 前言一、常用属性二、成员函数2.1 左上角空白区域 三、实例演示总结 前言 一、常用属性 二、成员函数 方法描述selectRow选中行removeRow移除行insertRow插入行rowCount总行数 2.1 左上角空白区域 QTableCornerButton即不属于列表头&#xff0c;也不…