首页 星云 工具 资源 星选 资讯 热门工具
:

PDF转图片 完全免费 小红书视频下载 无水印 抖音视频下载 无水印 数字星空

卡尔曼matblef仿真代码+数据

后端 5.38KB 17 需要积分: 1
立即下载

资源介绍:

卡尔曼matblef仿真代码+数据
clc; clear; % 初始化变量 %真实值 position_rho_measure = []; position_theta_measure = []; position_velocity_measure = []; time_measure = []; position_x_true = []; position_y_true = []; speed_x_true = []; speed_y_true = []; % x,y方向的测量值(由非线性空间转到线性空间) position_x_measure = []; position_y_measure = []; speed_x_measure = []; speed_y_measure = []; % 先验估计值 position_x_prior_est = []; % x方向位置的先验估计值 position_y_prior_est = []; % y方向位置的先验估计值 speed_x_prior_est = []; % x方向速度的先验估计值 speed_y_prior_est = []; % y方向速度的先验估计值 % 估计值和观测值融合后的最优估计值 position_x_posterior_est = []; % 根据估计值及当前时刻的观测值融合到一体得到的最优估计值x位置值存入到列表中 position_y_posterior_est = []; % 根据估计值及当前时刻的观测值融合到一体得到的最优估计值y位置值存入到列表中 speed_x_posterior_est = []; % 根据估计值及当前时刻的观测值融合到一体得到的最优估计x速度值存入到列表中 speed_y_posterior_est = []; % 根据估计值及当前时刻的观测值融合到一体得到的最优估计y速度值存入到列表中 % 打开文件 file = fopen('C:/Users/19516/Desktop/sample-laser-radar-measurement-data-2.txt'); % 读取文件并处理数据 while ~feof(file) line = fgetl(file); % 读取一行 curLine = strsplit(line, '\t'); % 使用制表符分割字符串 % 取出radar数据 if strcmp(curLine{1}, 'R') position_rho_measure(end+1) = str2double(curLine{2}); position_theta_measure(end+1) = str2double(curLine{3}); position_velocity_measure(end+1) = str2double(curLine{4}); time_measure(end+1) = str2double(curLine{5}); position_x_true(end+1) = str2double(curLine{6}); position_y_true(end+1) = str2double(curLine{7}); speed_x_true(end+1) = str2double(curLine{8}); speed_y_true(end+1) = str2double(curLine{9}); % 测量值 由非线性空间转到线性空间 % rho = str2double(curLine{2}); % theta = str2double(curLine{3}); % velocity = str2double(curLine{4}); % position_x_measure(end+1) = rho * cos(theta); % position_y_measure(end+1) = rho * sin(theta); % speed_x_measure(end+1) = velocity * cos(theta); % speed_y_measure(end+1) = velocity * sin(theta); end end % 关闭文件 fclose(file); for i = 1 : 100 position_x_measure(i) = position_rho_measure(i)*cos(position_theta_measure(i)); position_y_measure(i) = sqrt(position_rho_measure(i)^2 - position_x_measure(i)^2); speed_x_measure(i) = position_velocity_measure(i)*cos(position_theta_measure(i)); speed_y_measure(i) = position_velocity_measure(i)*sin(position_theta_measure(i)); end % --------------------------- 初始化 ------------------------- % 用第二帧测量数据初始化 X0 = [position_x_measure(2); position_y_measure(2); speed_x_measure(2); speed_y_measure(2)]; last_timestamp_ = time_measure(2); P = eye(4); X_posterior = X0; P_posterior = P; % 将初始化后的数据依次送入(即从第三帧速度往里送) for i = 3:length(time_measure) % ------------------- 下面开始进行预测和更新,来回不断的迭代 ------------------------- % 求前后两帧的时间差,数据包中的时间戳单位为微秒,除以1e6,转换为秒 delta_t = (time_measure(i) - last_timestamp_) / 1e6; last_timestamp_ = time_measure(i); % 状态转移矩阵F,上一时刻的状态转移到当前时刻 F = [1 0 delta_t 0; 0 1 0 delta_t; 0 0 1 0; 0 0 0 1]; % 外界输入矩阵B B = [delta_t^2/2 0; 0 delta_t^2/2; delta_t 0; 0 delta_t]; % 计算加速度 if i < 5 acceleration_x_posterior_est = 0; acceleration_y_posterior_est = 0; else acceleration_x_posterior_est = (speed_x_posterior_est(i-3) - speed_x_posterior_est(i-4)) / delta_t; acceleration_y_posterior_est = (speed_y_posterior_est(i-3) - speed_y_posterior_est(i-4)) / delta_t; end % 外界状态矩阵U U = [acceleration_x_posterior_est; acceleration_y_posterior_est]; % ---------------------- 预测 ------------------------- X_prior = F * X_posterior; % X_prior = F*X_posterior + B*U;% 不使用加速度 position_x_prior_est(i) = X_prior(1); position_y_prior_est(i) = X_prior(2); speed_x_prior_est(i) = X_prior(3); speed_y_prior_est(i) = X_prior(4); % Q:过程噪声的协方差 Q = [0.0001 0 0 0; 0 0.00009 0 0; 0 0 0.001 0; 0 0 0 0.001]; % Q = [0.0001 0 0 0; % 0 0.00009 0 0; % 0 0 0.001 0; % 0 0 0 0.001]; % 计算状态估计协方差矩阵P P_prior = F * P_posterior * F' + Q; % ------------------- 更新 ------------------------ % 测量的协方差矩阵R R = [0.000009 0 0; 0 0.009 0; 0 0 9]; % 状态观测矩阵H (EKF,radar) Px = X_prior(1); Py = X_prior(2); Vx = X_prior(3); Vy = X_prior(4); position_rho_ = sqrt(Px^2 + Py^2); if position_rho_ < 1e-8 position_rho_ = 1e-8; end % 线性化 H = [Px/position_rho_ Py/position_rho_ 0 0; -Py/(position_rho_^2) Px/(position_rho_^2) 0 0; Py*(Vx*Py - Vy*Px)/(position_rho_^3) Px*(Vy*Px - Vx*Py)/(position_rho_^3) Px/position_rho_ Py/position_rho_]; % 计算卡尔曼增益 K = P_prior * H' * inv(H * P_prior * H' + R); % 测量值 Z_measure = [position_rho_measure(i); position_theta_measure(i); position_velocity_measure(i)]; % z_measure = [sqrt(Px^2 + Py^2);(Px/sqrt(Px^2 + Py^2));sqrt(Vx^2 + Vy^2)]; X_posterior = X_prior + K * (Z_measure - H*X_prior); position_x_posterior_est(i) = X_posterior(1); position_y_posterior_est(i) = X_posterior(2); speed_x_posterior_est(i) = X_posterior(3); speed_y_posterior_est(i) = X_posterior(4); % 更新状态估计协方差矩阵P P_posterior = (eye(4) - K * H) * P_prior; end % 可视化显示 figure; subplot(2, 2, 1); plot(position_x_measure, 'b-', 'DisplayName', '位置x_测量值'); hold on; plot(position_x_posterior_est, 'r-', 'DisplayName', '位置x_扩展卡尔曼滤波后的值'); title('位置x'); xlabel('k'); legend; subplot(2, 2, 2); plot(position_y_measure, 'b-', 'DisplayName', '位置y_测量值'); hold on; plot(position_y_posterior_est, 'r-', 'DisplayName', '位置y_扩展卡尔曼滤波后的值'); title('位置y'); xlabel('k'); legend; subplot(2, 2, 3); plot(speed_x_measure, 'b-', 'DisplayName', '速度x_测量值'); hold on; plot(speed_x_posterior_est, 'r-', 'DisplayName', '速度x_扩展卡尔曼滤波后的值'); title('速度x'); xlabel('k'); legend; subplot(2, 2, 4); plot(speed_y_measure, 'b-', 'DisplayName', '速度y_测量值'); hold on; plot(speed_y_posterior_est, 'r-', 'DisplayName', '速度y_扩展卡尔曼滤波后的值'); title('速度y'); xlabel('k'); legend;

资源文件列表:

Kalman_Deal.zip 大约有4个文件
  1. Kalman_Deal/ekf.asv 7.31KB
  2. Kalman_Deal/ekf.m 7.58KB
  3. Kalman_Deal/untitled.m 2.01KB
  4. Kalman_Deal/
0评论
提交 加载更多评论
其他资源 【浏览器插件】扩展管理器.zip
【浏览器插件-扩展管理器】为您的Chrome浏览器带来高效便捷的插件管理体验。通过禁用那些暂时不用的插件,有效降低Chrome的资源占用。主要特性包括: 1. 置顶展示当前启用的插件,一目了然。 2. 清晰区分启用中的插件、分组插件及所有插件。 3. 一键启用或禁用插件,操作简便快捷。 4. 支持插件分组功能,可创建新的插件群组,并通过拖拽轻松管理。 5. 插件排序功能,通过拖拽即可调整插件顺序。 6. 提供文字列表、大图标列表和小图标列表等多种视图选项,满足不同用户需求。 7. 支持拆卸插件,点击拆卸按钮即可移除长期不使用的插件;对于临时不用的插件,只需禁用即可,无需拆卸。 通过“扩展管理”,您可以轻松地启用、禁用或拆卸插件,无需进入Chrome的“扩展程序”页面。此外,还提供了一系列快捷方式,方便您快速访问系统功能,如下载管理、浏览历史、扩展管理、插件设置、Chrome设置、搜索引擎配置、清除缓存、内容选项、字体与编码、语言设置以及浏览器重置等。
【浏览器插件】守候购物小助手.zip
【浏览器插件-守候购物小助手】 本插件全面支持京东、淘宝、天猫、苏宁、国美等主流电商平台。集成了快速搜索、页面跳转、复制、库存查询、价格对比、优惠券查询、促销活动识别等多功能于一体。用户可在插件选项页根据个人偏好,自由开启或关闭各项服务。此外,插件界面还隐藏着更多高级功能,等待您的发掘。您可以依据购物习惯,定制个性化的购物助手,打造专属的购物体验。守候购物小助手承诺,将持续更新,带来更多创新与强大的功能,敬请期待。 【更新日志】 - 版本v3.10.2(2023年11月9日):新增京东直播页电脑端访问功能,优化购买分析组件计算逻辑,修复多项功能问题。 - 版本v3.10.1(2023年11月3日):新增多项京东页面功能,优化历史价格显示,修复购买分析等问题。 - 版本v3.9.2至v3.9.1(2023年9月):新增库存查询、登录状态提醒,改进触屏端数据请求,修复多项功能问题。 - 更早版本更新:持续优化用户体验,修复发现的问题,新增实用功能。 注:插件目前处于积极开发阶段,部分平台功能正在完善中。具体功能操作与介绍请见插件选项页,各功能均有详细说明。
【浏览器插件】卡巴斯基保护.zip
【浏览器插件 - 卡巴斯基安全防护】这款插件让您在浏览网页时,能够便捷地管理安全防护,而不会分散您的注意力。它使您能够在浏览器窗口中直接访问安全防护管理功能。插件提供的功能取决于您的订阅计划,包括: - **安全支付**:在您进行在线支付时,保护您的数据安全,防止数据被拦截。 - **无痕浏览**:当网站尝试收集您的数据时,无痕浏览会及时通知您,并允许您根据需要限制数据收集。 - **广告拦截**:有效阻止网站上的烦人弹出广告,提升您的浏览体验。 - **安全浏览**:防止您访问潜在的危险网站,保障您的网络安全。 卡巴斯基保护插件设计用于与以下卡巴斯基应用程序无缝协作: - 卡巴斯基反病毒软件 - 卡巴斯基安全软件 - 卡巴斯基全方位安全软件 - Kaspersky Security Cloud - 免费版 - Kaspersky Security Cloud - 个人版 - Kaspersky Security Cloud - 家庭版 - Kaspersky Small Office Security - 卡巴斯基免费版 - 卡巴斯基基础版 - 卡巴斯基标准版 - 卡巴斯基 Pl
四、前后端分离通用权限系统(4)project-demo(3).zip
四、前后端分离通用权限系统(4)project_demo(3).zip
四、前后端分离通用权限系统(4)project-demo(3).zip 四、前后端分离通用权限系统(4)project-demo(3).zip 四、前后端分离通用权限系统(4)project-demo(3).zip
【浏览器插件】ReadAloud文本语音朗读助理.zip
浏览器插件-ReadAloud:让文字跃然纸上,成为您的声音助理。厌倦了长时间静坐阅读?只需一键点击,便可舒适地躺在床上,享受文章的朗读。支持PDF格式,让阅读变得更加轻松。市面上许多文本转语音扩展功能繁琐,需要手动选择文本,令人烦恼。一些热门产品甚至功能失效。因此,我为自己量身打造了这款插件,它能够自动识别并朗读内容,一切尽在一键之间。通过EPUBReader扩展,更支持EPUB格式。(epubread.com)
【浏览器插件】货币换算.zip
浏览器插件-货币换算,专为Chrome和Edge浏览器设计的Currency Converter扩展,让您在浏览网页时轻松转换货币。是否在旅途中为货币转换感到困扰?是否急需一个高效、快速的货币计算器,以便您能即时在不同货币间切换?货币转换器正是您需要的工具。它允许您在当前标签页工作时,将一种货币单位即时转换为另一种货币。 这款扩展程序功能全面,集成了多种实用特性,成为您货币转换需求的一站式解决方案。它持续更新,确保所有实时货币变动都能及时反映。对于追求精确货币价值的用户来说,它既准确又可靠。支持高达六位小数的货币转换,并提供四种货币格式选项,满足您的个性化需求。转换过程实时进行,输入时即刻显示新货币结果。 扩展内置了全球众多货币,您可通过下拉菜单选择所需货币。货币列表可根据您常转换的货币进行排序,避免重复查找的麻烦。 主要功能亮点: - **高度可定制**:选择您偏好的货币进行切换,点击图标即可快速访问USD和EUR等默认货币。通过加号或拖动按钮,轻松添加或删除感兴趣的货币,所有更改将自动保存,下次打开浏览器时恢复设置。 - **简洁界面**:用户友好的设计,让您能迅速进行货币转
【浏览器插件】Tab Wrangler.zip
浏览器插件Tab Wrangler,助你摆脱标签页成瘾。它会自动关闭那些最近未使用的标签页,让你的Chrome浏览器界面焕然一新。点击以下链接,了解Tab Wrangler如何为你的浏览器带来秩序:https://www.youtube.com/watch?v=jtP7pysqHY8 - 指定时间后自动关闭闲置标签页 - 保护你最爱的网站不被自动关闭 - 保存已关闭的标签页,便于快速重新打开 - 跨设备同步设置 - 固定标签页不会被关闭 - 锁定标签页,防止意外关闭 别担心,自动关闭的标签页将安全地保存在标签栏中,随时待命。Tab Wrangler提供高度的可配置性,功能强大,让你的电脑和大脑远离标签页过载的困扰。火狐浏览器标签管理器,点击这里下载:https://addons.mozilla.org/en-US/firefox/addon/tabwrangler/
【浏览器插件】ColorZilla.zip
ColorZilla,一款广受好评的浏览器插件,以其用户友好、功能全面、安全可靠而著称。作为Chrome和Firefox开发者扩展中的佼佼者,ColorZilla全球下载量已突破1000万次。它允许用户在浏览器的任何位置获取颜色值,快速调整颜色,并将其复制到其他程序中。ColorZilla不仅具备基本的取色功能,还拥有高级颜色选择器、CSS渐变生成器、网页颜色分析器等多种实用工具。 **功能亮点:** - 取色器:精准捕捉页面上任意像素的颜色。 - 高级颜色选择器:提供类似Photoshop的调色体验。 - 终极CSS渐变生成器:轻松创建复杂的渐变效果。 - 网页颜色分析器:一键获取任何网站的配色方案。 - 调色板查看器:内置7种预设调色板,方便颜色搭配。 - 颜色历史:记录最近选择的颜色,方便回顾。 - 多种采样大小:1x1、3x3、5x5、11x11、25x25像素,适应不同需求。 - 元素信息展示:显示元素的标签名、类、ID、尺寸等信息。 - 自动复制颜色:一键复制选中颜色到剪贴板。 - 键盘快捷键:提高操作效率。 - 色调/饱和度/值操作:直观调整颜色属性。 - 动态悬停元素