基于matlab实现的卡尔曼滤波匀加速直线运动仿真

完整程序:

clear
clc
%% 初始化参数
delta_t = 0.1;  %采样时间
T = 8;          %总运行时长
t = 0:delta_t:T;  %时间序列
N = length(t);  %序列的长度  
x0 = 0;  %初始位置
u0 = 0;  %初速度
U = 10;  %控制量、加速度

F = [1 delta_t
    0 1];   %状态转移矩阵
B = [0.5*delta_t^2  
    delta_t];  %控制矩阵
H = [1 0];  %观测矩阵
W = [0;3];  %过程噪声
V = [70];  %量测噪声

%分配空间 
XP = zeros(2,N);%预测值
XP(:,1) = [x0;u0];
XR = zeros(2,N);%真实值
XR(:,1) = [x0;u0];
Z = zeros(1,N);%观测值
Z(1) = [0];

for i=2:N
    XP(:,i) = F*XP(:,i-1)+B*U;  %预测值
    XR(:,i) = F*XR(:,i-1)+B*U+sqrt(W)*randn;  %真实值
    Z(i) = H*XR(:,i)+sqrt(V)*randn;  %观测值
end

%% 卡尔曼滤波%%
%初始化参数
Xk = zeros(2,N);  %最优估计值
Xk(:,1) = [0;0];
P = [1,0;0,1];  %均方误差
I = eye(2);
Q = [0 0;0 0.1];
R = 10;
for i=2:N
    
    %时间更新
    X_pre = F*Xk(:,i-1)+B*U;  %状态预测值
    P_pre = F*P*F'+Q;     %预测均方误差
    
    %量测更新
    Kg = P_pre*H'*inv(H*P_pre*H'+R);  %计算卡尔曼增益
    Xk(:,i) = X_pre+Kg*(Z(:,i)-H*X_pre);  %状态估计值
    P = (I-Kg*H)*P_pre;  %均方误差
end

%% 结果
figure(1)
plot(t,XP(1,:),'K');hold on
grid on
plot(t,XR(1,:),'r');hold on
plot(t,Z(:),'b');
legend('预测值', '真实值','量测值');
title('位置')
xlabel('时间 [sec]')
ylabel('位置 [m]')
hold on
plot(t,Xk(1,:),'g');
legend('预测值', '真实值','量测值','kalman估计值');

figure(2)
x_error = abs(XR-Xk(1,:));
x_error1 = abs(XR-Z(1,:));
plot(t,x_error(1,:),'b'),grid on;hold on 
plot(t,x_error1(1,:),'r');
legend('估计值误差', '量测值误差');
title('位置误差')
xlabel('时间 [sec]')
ylabel('位置均方根误差 [m]')
grid on
hold off;

figure(3)
plot(t,Xk(2,:),'r'),grid on;
title('实际速度 ')
legend('实际速度')
xlabel('时间 [sec]')
ylabel('速度 [m/sec]')
hold off;
    

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

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

相关文章

【c#-Nuget 包“在此源中不可用”】 Nuget package “Not available in this source“

标题c#-Nuget 包“在此源中不可用”…但 VS 仍然知道它吗? (c# - Nuget package “Not available in this source”… but VS still knows about it?) 听起来您的公司有一个发布包的内部 NuGet feed,而不是公共 NuGet.org feed。您应该向您的同事询问…

CentOS 7 安装踩坑

CentOS与Ubuntu并称为Linux最著名的两个发行版,但由于笔者主要从事深度学习图像算法工作,Ubuntu作为谷歌和多数依赖库的亲儿子占据着最高生态位。但最近接手的一个项目里,甲方指定需要在CentOS7上运行项目代码,笔者被迫小小cos了一…

在华为云服务器上CentOS 7安装单机版Redis

https://redis.io/是官网地址。 点击右上角的Download。 可以进入https://redis.io/download/——Redis官网下载最新版的网址。 然后在https://redis.io/download/页面往下拉,点击下图超链接这里。 进入https://download.redis.io/releases/下载自己需要的安装…

【二叉树】的顺序存储(堆的实现)

📙作者简介: 清水加冰,目前大二在读,正在学习C/C、Python、操作系统、数据库等。 📘相关专栏:C语言初阶、C语言进阶、C语言刷题训练营、数据结构刷题训练营、有感兴趣的可以看一看。 欢迎点赞 &#x1f44d…

64位Ubuntu20.04.5 LTS系统安装32位运行库

背景: 在ubutu(版本为20.04.5 LTS)中运行./arm-none-linux-gnueabi-gcc -v 后提示“no such device”。 经多方查证,是ubutu的版本是64位的,而需要运行的编译工具链是32位的,因此会不兼容。 解决方法就是在…

十分钟理解OSPF路由协议

十分钟理解OSPF路由协议 1.RIP的缺陷以跳数为度量值最大跳数为15更新路由表采用全更新收敛速度慢 2.RIP与OSPF比较OSPF概述运行OSPF协议之前运行OSPF协议之后 3.OSPF协议工作过程1.发现邻居2.建立邻接关系3.传递链路状态信息4.计算路由 4.OSPF分区域管理 有RIP协议,…

Bootstrap 框架学习笔记(基础)

来自于 Twitter,基于 HTML、CSS、JavaScript。 有关网站:Bootstrap中文网Bootstrap是Twitter推出的一个用于前端开发的开源工具包。它由Twitter的设计师Mark Otto和Jacob Thornton合作开发,是一个CSS/HTML框架。目前,Bootstrap最…

Java Semaphore使用例子和流程

目录 Semaphore例子代码和输出semaphore.acquire();semaphore.release(); Semaphore semaphore : 英[ˈseməfɔː(r)] 美[ˈseməfɔːr] n. 旗语; 信号标; v. 打旗语; (用其他类似的信号系统)发信号; [例句]Semaphore was widely used at sea, before the advent of electr…

ssh登录时间久或登陆后报错

情况1 问题描述: ssh登录时间很久,登录后出现abrt-cli status timed out 的报错 问题原因: .lock文件被锁导致 执行systemctl status abrtd.service可以看到被锁的.lock 处理方式: ps -ef | grep pid 找到被锁的进程kill掉…

图片格式大全

青春不能回头,青春也没有终点。 大全介绍 图片格式有多种,每种格式都有其独特的特性和用途。以下是一些常见的图片格式以及它们的介绍: JPEG(Joint Photographic Experts Group): 文件扩展名:…

1786_MTALAB代码生成把通用函数生成独立文件

全部学习汇总: GitHub - GreyZhang/g_matlab: MATLAB once used to be my daily tool. After many years when I go back and read my old learning notes I felt maybe I still need it in the future. So, start this repo to keep some of my old learning notes…

计算机竞赛 深度学习 python opencv 火焰检测识别

文章目录 0 前言1 基于YOLO的火焰检测与识别2 课题背景3 卷积神经网络3.1 卷积层3.2 池化层3.3 激活函数:3.4 全连接层3.5 使用tensorflow中keras模块实现卷积神经网络 4 YOLOV54.1 网络架构图4.2 输入端4.3 基准网络4.4 Neck网络4.5 Head输出层 5 数据集准备5.1 数…

QT记事本+登陆界面的简单实现

主体头文件 #ifndef JSB_H #define JSB_H#include <QMainWindow> #include <QMenuBar>//菜单栏 #include <QToolBar>//工具栏 #include <QStatusBar>//状态栏 #include <QTextEdit>//文本 #include <QLabel>//标签 #include <QDebug&g…

Android之MediaCodec::PostAndAwaitResponse消息原理(四十三)

简介: CSDN博客专家,专注Android/Linux系统,分享多mic语音方案、音视频、编解码等技术,与大家一起成长! 优质专栏:Audio工程师进阶系列【原创干货持续更新中……】🚀 人生格言: 人生从来没有捷径,只有行动才是治疗恐惧和懒惰的唯一良药. 更多原创,欢迎关注:Android…

企业内部安全与风控管理图解

企业内部安全说外部安全&#xff0c;企业领导者都非常关注&#xff0c;由于各方面原因&#xff0c;。。。力不从心&#xff0c;妥协&#xff01; 方向&#xff1a; 1、制度 结合企业实情&#xff0c;编制企业安全管理制度 2、硬件 处理常规硬件外观&#xff0c;加壳与锁定、…

【力扣每日一题】2023.9.10 打家劫舍Ⅳ

目录 题目&#xff1a; 示例&#xff1a; 分析&#xff1a; 代码&#xff1a; 题目&#xff1a; 示例&#xff1a; 分析&#xff1a; 题目翻译有些烂&#xff0c;我来二次翻译一下&#xff0c;找出数组中k个两两互不相邻的数&#xff0c;求出它们的最大值。要求最大值尽可…

Mac 错误zsh: command not found: brew解决方法

打开iterm或其他shell终端&#xff0c;执行命令&#xff1a; /bin/zsh -c "$(curl -fsSL https://gitee.com/cunkai/HomebrewCN/raw/master/Homebrew.sh)" 选择下载brew的源&#xff0c;输入1~6任意都行 根据提示输入Y及开机密码 最后执行&#xff1a;source ~/.z…

ChatGLM 大模型应用构建 Prompt 工程

文章目录 一、大模型简介1.1 大模型基础知识1.2 大模型分类 二、如何构建大模型应用2.1 To B、To C场景应用区别2.2 大模型在To B场景中的应用原则2.3 大模型应用技巧2.3.1 大模型应用技巧2.3.2 大模型的应用方法2.3.3 案例&#xff1a;车险问答系统 三、指令工程&#xff08;p…

macOS 中 Apple Distribution 与 Apple Development 证书不受信任解决方法

造成这个现象的原因是 Worldwide Developer Relations 中间关系证书缺失 我们只需要将相关证书下载并导入到「系统」级钥匙串即可 首先访问网站&#xff1a;https://www.apple.com/certificateauthority 下载右侧 Apple Intermediate Certificates 下方的如下证书 Develope…

安理【2022】

关键字&#xff1a; 出栈序列s2固定、快速排序2趟、next数组、二分查找比较次数log2n向上取整、 一、选择 二、填空 三、应用