使用卡尔曼滤波估计自由落体时的位置和速度

2023-10-18 09:59

本文主要是介绍使用卡尔曼滤波估计自由落体时的位置和速度,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

问题:通过传感器可以测量出自由落体时的加速度、速度和位置,通过卡尔曼滤波估计速度和位置。

坐标系:向下为正,初始位置和速度都为0

先上卡尔曼滤波公式:

然后上代码:

# -*- coding: utf-8 -*
# 向下为正方向import numpy as np
import matplotlib.pyplot as pltdef main():# 时间共1s,采样周期10msdt = 0.01t = [i * dt for i in range(0, 100)]g = 9.8# 真实值x_true_mat = np.mat(0.5 * g * np.multiply(np.array(t), np.array(t)))v_true_mat = g * np.mat(t)u_true_mat = np.mat([g for i in range(0, 100)])# 噪声x_noise = np.round(np.random.normal(0, 0.1, 100), 2)v_noise = np.round(np.random.normal(0, 0.1, 100), 2)u_noise = np.round(np.random.normal(0, 0.01, 100), 2)x_noise_mat = np.mat(x_noise)v_noise_mat = np.mat(v_noise)u_noise_mat = np.mat(u_noise)# 测量值x_z_mat = x_true_mat + x_noise_matv_z_mat = v_true_mat + v_noise_matu_mat = u_true_mat + u_noise_mat# 定义x的初始状态x_mat = np.mat([[0], [0]])# 定义初始状态协方差矩阵p_mat = np.mat([[1, 0], [0, 1]])# 状态转移矩阵f_mat = np.mat([[1, dt], [0, 1]])# 控制矩阵b_mat = np.mat([[0.5 * dt * dt], [dt]])# 定义状态转移协方差矩阵,这里我们把协方差设置的很小,因为觉得状态转移矩阵准确度高q_mat = np.mat([[1.0 * 1.0 * dt * dt, 0], [0, 1.0 * 1.0 * dt * dt]])# 定义观测矩阵h_mat = np.mat([[1, 0], [0, 1]])# 定义观测噪声协方差r_mat = np.mat([[1.0 * 1.0, 0], [0, 2.5 * 2.5]])for i in range(100):x_predict = f_mat * x_mat + b_mat * u_mat[0, i]p_predict = f_mat * p_mat * f_mat.T + q_matk = p_predict * h_mat.T * (h_mat * p_predict * h_mat.T + r_mat).Izt = np.mat([[x_z_mat[0, i]], [v_z_mat[0, i]]])x_mat = x_predict + k * (zt - h_mat * x_predict)p_mat = (p_mat - k * h_mat) * p_predictplt.plot(t[i], x_z_mat[0, i], 'ro', markersize=1)plt.plot(t[i], v_z_mat[0, i], 'ro', markersize=1)plt.plot(t[i], x_mat[0, 0], 'bo', markersize=1)plt.plot(t[i], x_mat[1, 0], 'bo', markersize=1)plt.show()if __name__ == '__main__':main()

结果如下:

图中的红色点分别是观测的位置和速度,蓝色点为估计出的位置和速度。

这篇关于使用卡尔曼滤波估计自由落体时的位置和速度的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/231836

相关文章

C++中assign函数的使用

《C++中assign函数的使用》在C++标准模板库中,std::list等容器都提供了assign成员函数,它比操作符更灵活,支持多种初始化方式,下面就来介绍一下assign的用法,具有一定的参考价... 目录​1.assign的基本功能​​语法​2. 具体用法示例​​​(1) 填充n个相同值​​(2)

Spring StateMachine实现状态机使用示例详解

《SpringStateMachine实现状态机使用示例详解》本文介绍SpringStateMachine实现状态机的步骤,包括依赖导入、枚举定义、状态转移规则配置、上下文管理及服务调用示例,重点解... 目录什么是状态机使用示例什么是状态机状态机是计算机科学中的​​核心建模工具​​,用于描述对象在其生命

使用Python删除Excel中的行列和单元格示例详解

《使用Python删除Excel中的行列和单元格示例详解》在处理Excel数据时,删除不需要的行、列或单元格是一项常见且必要的操作,本文将使用Python脚本实现对Excel表格的高效自动化处理,感兴... 目录开发环境准备使用 python 删除 Excphpel 表格中的行删除特定行删除空白行删除含指定

深入理解Go语言中二维切片的使用

《深入理解Go语言中二维切片的使用》本文深入讲解了Go语言中二维切片的概念与应用,用于表示矩阵、表格等二维数据结构,文中通过示例代码介绍的非常详细,需要的朋友们下面随着小编来一起学习学习吧... 目录引言二维切片的基本概念定义创建二维切片二维切片的操作访问元素修改元素遍历二维切片二维切片的动态调整追加行动态

prometheus如何使用pushgateway监控网路丢包

《prometheus如何使用pushgateway监控网路丢包》:本文主要介绍prometheus如何使用pushgateway监控网路丢包问题,具有很好的参考价值,希望对大家有所帮助,如有错误... 目录监控网路丢包脚本数据图表总结监控网路丢包脚本[root@gtcq-gt-monitor-prome

Python通用唯一标识符模块uuid使用案例详解

《Python通用唯一标识符模块uuid使用案例详解》Pythonuuid模块用于生成128位全局唯一标识符,支持UUID1-5版本,适用于分布式系统、数据库主键等场景,需注意隐私、碰撞概率及存储优... 目录简介核心功能1. UUID版本2. UUID属性3. 命名空间使用场景1. 生成唯一标识符2. 数

SpringBoot中如何使用Assert进行断言校验

《SpringBoot中如何使用Assert进行断言校验》Java提供了内置的assert机制,而Spring框架也提供了更强大的Assert工具类来帮助开发者进行参数校验和状态检查,下... 目录前言一、Java 原生assert简介1.1 使用方式1.2 示例代码1.3 优缺点分析二、Spring Fr

Android kotlin中 Channel 和 Flow 的区别和选择使用场景分析

《Androidkotlin中Channel和Flow的区别和选择使用场景分析》Kotlin协程中,Flow是冷数据流,按需触发,适合响应式数据处理;Channel是热数据流,持续发送,支持... 目录一、基本概念界定FlowChannel二、核心特性对比数据生产触发条件生产与消费的关系背压处理机制生命周期

java使用protobuf-maven-plugin的插件编译proto文件详解

《java使用protobuf-maven-plugin的插件编译proto文件详解》:本文主要介绍java使用protobuf-maven-plugin的插件编译proto文件,具有很好的参考价... 目录protobuf文件作为数据传输和存储的协议主要介绍在Java使用maven编译proto文件的插件

SpringBoot线程池配置使用示例详解

《SpringBoot线程池配置使用示例详解》SpringBoot集成@Async注解,支持线程池参数配置(核心数、队列容量、拒绝策略等)及生命周期管理,结合监控与任务装饰器,提升异步处理效率与系统... 目录一、核心特性二、添加依赖三、参数详解四、配置线程池五、应用实践代码说明拒绝策略(Rejected