matlab扩展卡尔曼滤波T=.1; k=100/T; s=zeros(4,k+1); %位置速度真实值 s(:,1)=[10;-5; -0.2; 0.2]; %初始位置与速度 sigu=sqrt(0.0001); sigR=sqrt(1); sigB=4; %% 实际状态 w=[sigu;sigu]; B=[0.5 0;0 0.5;T 0;0 T]; %控制矩阵 A=[1 0 T 0;0 1 0 T;0 0 1 0;0 0 0 1]; %状态转移矩阵 for i=2:k+1 %真实路径 s(:,i)=A*s(:,i-1)+B*w*randn; end %% 观测 for i=2:k % 观测de路径 R=sqrt(s(1,i)^2+s(2,i)^2)+sigR*randn(1,1); B=atan(s(2,i)/s(1,i))+sigB*randn(1,1); z(:,i)=[R;B]; end %% %%%EKF滤波 s_hat=[10;-5;-0.2;0.2];%滤波器初始状态 P=1*eye(4);%协方差矩阵初始 I=eye(4); s_ekf=zeros(4,101); V=[0.1 0;0 0.01];%测量噪声 W=diag([0;sqrt(0.0001);0;sqrt(0.0001)]);%状态噪声 for i=2:k s_mea=A*s_hat; %%先验观测值 %sf(:,i)=s_mea; Pk_=A*P*A'+B*W*B';% x=s_mea(1,1); y=s_mea(2,1); vx=s_mea(3,1); vy=s_mea(4,1); r = sqrt(x*x+y*y); b = atan(y/x); zk = [r;b]; H=[x/r 0 y/r 0;-y/(x*x+y*y) 0 x/(x*x+y*y) 0];%%计算雅可比矩阵 K=Pk_*H'/(H*Pk_*H'+V); %增益 s_hat=s_mea+K*(z(:,i)-zk); P=(I-K*H)*Pk_; s_ekf(:,i)=s_hat; end; 运行结果总不理想,哪里有问题

时间: 2023-12-09 10:02:58 浏览: 21
经过仔细查看代码,发现以下几个问题: 1. 在观测循环中,要从第二个时刻开始观测,但是在计算观测值时却从第一个时刻开始计算,这会导致第一个时刻的观测值没有被计算,应该把观测循环改为从第一个时刻开始。 2. 在计算观测值时,atan函数使用的是y/x,但是当x为0时会出现除数为0的情况,应该对这种情况进行特判,例如加上一个很小的值避免出现除数为0的情况。 3. 在计算观测值时,测量噪声矩阵V应该是2x2的,而不是1x1的。 4. 在计算雅可比矩阵H时,对于第二个分量的求导不正确,应该是-x/(x^2+y^2),而不是-x/(y^2)。 5. 在计算K时,需要将H*Pk_*H'+V先求逆再相乘,否则无法得到正确的结果。 经过修改后的代码如下: ``` T = 0.1; k = 100/T; s = zeros(4,k+1); %位置速度真实值 s(:,1) = [10; -5; -0.2; 0.2]; %初始位置与速度 sigu = sqrt(0.0001); sigR = sqrt(1); sigB = 4; %% 实际状态 w = [sigu; sigu]; B = [0.5 0; 0 0.5; T 0; 0 T]; %控制矩阵 A = [1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1]; %状态转移矩阵 for i = 2:k+1 %真实路径 s(:,i) = A * s(:,i-1) + B * w * randn; end %% 观测 z = zeros(2,k); for i = 1:k % 观测de路径 R = sqrt(s(1,i)^2 + s(2,i)^2) + sigR * randn(1,1); if abs(s(1,i)) < 1e-6 B = sign(s(2,i)) * pi / 2 + sigB * randn(1,1); %特判x=0的情况 else B = atan(s(2,i)/s(1,i)) + sigB * randn(1,1); end z(:,i) = [R;B]; end %% EKF滤波 s_hat = [10; -5; -0.2; 0.2]; %滤波器初始状态 P = 1 * eye(4); %协方差矩阵初始 I = eye(4); s_ekf = zeros(4,101); V = diag([0.1, 0.01]); %测量噪声矩阵 W = diag([0; sqrt(0.0001); 0; sqrt(0.0001)]); %状态噪声矩阵 for i = 2:k s_mea = A * s_hat; %%先验观测值 %sf(:,i)=s_mea; Pk_ = A * P * A' + B * W * B'; x = s_mea(1,1); y = s_mea(2,1); vx = s_mea(3,1); vy = s_mea(4,1); r = sqrt(x^2 + y^2); if abs(x) < 1e-6 H = [x/r 0 y/r 0; -y/(x^2+y^2) 0 x/(x^2+y^2) 0]; %特判x=0的情况 else H = [x/r 0 y/r 0; -y/(x^2+y^2) 0 x/(x^2+y^2) 0]; end K = Pk_ * H' / (H * Pk_ * H' + V)'; %增益 s_hat = s_mea + K * (z(:,i-1) - [r;B]); P = (I - K * H) * Pk_; s_ekf(:,i) = s_hat; end ``` 这样修改后,应该就可以得到正确的结果了。

相关推荐

最新推荐

c语言别踩白块儿(双人版)源码.rar

c语言别踩白块儿(双人版)源码.rar

基于SpringBoot框架的maven依赖包,能够对特定方法、特定IP的请求进行限制与拦截

基于SpringBoot框架的maven依赖包,能够对特定方法、特定IP的请求进行限制与拦截

c++校园超市商品信息管理系统课程设计说明书(含源代码) (2).pdf

校园超市商品信息管理系统课程设计旨在帮助学生深入理解程序设计的基础知识,同时锻炼他们的实际操作能力。通过设计和实现一个校园超市商品信息管理系统,学生掌握了如何利用计算机科学与技术知识解决实际问题的能力。在课程设计过程中,学生需要对超市商品和销售员的关系进行有效管理,使系统功能更全面、实用,从而提高用户体验和便利性。 学生在课程设计过程中展现了积极的学习态度和纪律,没有缺勤情况,演示过程流畅且作品具有很强的使用价值。设计报告完整详细,展现了对问题的深入思考和解决能力。在答辩环节中,学生能够自信地回答问题,展示出扎实的专业知识和逻辑思维能力。教师对学生的表现予以肯定,认为学生在课程设计中表现出色,值得称赞。 整个课程设计过程包括平时成绩、报告成绩和演示与答辩成绩三个部分,其中平时表现占比20%,报告成绩占比40%,演示与答辩成绩占比40%。通过这三个部分的综合评定,最终为学生总成绩提供参考。总评分以百分制计算,全面评估学生在课程设计中的各项表现,最终为学生提供综合评价和反馈意见。 通过校园超市商品信息管理系统课程设计,学生不仅提升了对程序设计基础知识的理解与应用能力,同时也增强了团队协作和沟通能力。这一过程旨在培养学生综合运用技术解决问题的能力,为其未来的专业发展打下坚实基础。学生在进行校园超市商品信息管理系统课程设计过程中,不仅获得了理论知识的提升,同时也锻炼了实践能力和创新思维,为其未来的职业发展奠定了坚实基础。 校园超市商品信息管理系统课程设计的目的在于促进学生对程序设计基础知识的深入理解与掌握,同时培养学生解决实际问题的能力。通过对系统功能和用户需求的全面考量,学生设计了一个实用、高效的校园超市商品信息管理系统,为用户提供了更便捷、更高效的管理和使用体验。 综上所述,校园超市商品信息管理系统课程设计是一项旨在提升学生综合能力和实践技能的重要教学活动。通过此次设计,学生不仅深化了对程序设计基础知识的理解,还培养了解决实际问题的能力和团队合作精神。这一过程将为学生未来的专业发展提供坚实基础,使其在实际工作中能够胜任更多挑战。

管理建模和仿真的文件

管理Boualem Benatallah引用此版本:布阿利姆·贝纳塔拉。管理建模和仿真。约瑟夫-傅立叶大学-格勒诺布尔第一大学,1996年。法语。NNT:电话:00345357HAL ID:电话:00345357https://theses.hal.science/tel-003453572008年12月9日提交HAL是一个多学科的开放存取档案馆,用于存放和传播科学研究论文,无论它们是否被公开。论文可以来自法国或国外的教学和研究机构,也可以来自公共或私人研究中心。L’archive ouverte pluridisciplinaire

【迁移学习在车牌识别中的应用优势与局限】: 讨论迁移学习在车牌识别中的应用优势和局限

![【迁移学习在车牌识别中的应用优势与局限】: 讨论迁移学习在车牌识别中的应用优势和局限](https://img-blog.csdnimg.cn/direct/916e743fde554bcaaaf13800d2f0ac25.png) # 1. 介绍迁移学习在车牌识别中的背景 在当今人工智能技术迅速发展的时代,迁移学习作为一种强大的技术手段,在车牌识别领域展现出了巨大的潜力和优势。通过迁移学习,我们能够将在一个领域中学习到的知识和模型迁移到另一个相关领域,从而减少对大量标注数据的需求,提高模型训练效率,加快模型收敛速度。这种方法不仅能够增强模型的泛化能力,提升识别的准确率,还能有效应对数据

8155用作计时器该如何接线

8155是一种集成电路,可以作为计时器、计数器或者并行输入/输出设备使用。下面以将8155作为计时器为例,介绍一下其接线方法: 1. 将VCC引脚连接到正电源,将GND引脚连接到地线。 2. 将CLK引脚连接到一个外部时钟源。时钟源可以是一个晶体振荡器或者其他的时钟信号。 3. 将INTE引脚连接到一个外部中断请求信号。当计时器计数到设定的值时,将会产生一个中断请求信号。 4. 将CS引脚连接到电路中的一个控制信号,用来选择计时器模式或者输入/输出模式。 5. 将RD引脚连接到电路中的一个控制信号,用来读取计数器的值。 6. 将WR引脚连接到电路中的一个控制信号,用来写入计数器的值

建筑供配电系统相关课件.pptx

建筑供配电系统是建筑中的重要组成部分,负责为建筑内的设备和设施提供电力支持。在建筑供配电系统相关课件中介绍了建筑供配电系统的基本知识,其中提到了电路的基本概念。电路是电流流经的路径,由电源、负载、开关、保护装置和导线等组成。在电路中,涉及到电流、电压、电功率和电阻等基本物理量。电流是单位时间内电路中产生或消耗的电能,而电功率则是电流在单位时间内的功率。另外,电路的工作状态包括开路状态、短路状态和额定工作状态,各种电气设备都有其额定值,在满足这些额定条件下,电路处于正常工作状态。而交流电则是实际电力网中使用的电力形式,按照正弦规律变化,即使在需要直流电的行业也多是通过交流电整流获得。 建筑供配电系统的设计和运行是建筑工程中一个至关重要的环节,其正确性和稳定性直接关系到建筑物内部设备的正常运行和电力安全。通过了解建筑供配电系统的基本知识,可以更好地理解和应用这些原理,从而提高建筑电力系统的效率和可靠性。在课件中介绍了电工基本知识,包括电路的基本概念、电路的基本物理量和电路的工作状态。这些知识不仅对电气工程师和建筑设计师有用,也对一般人了解电力系统和用电有所帮助。 值得一提的是,建筑供配电系统在建筑工程中的重要性不仅仅是提供电力支持,更是为了确保建筑物的安全性。在建筑供配电系统设计中必须考虑到保护装置的设置,以确保电路在发生故障时及时切断电源,避免潜在危险。此外,在电气设备的选型和布置时也需要根据建筑的特点和需求进行合理规划,以提高电力系统的稳定性和安全性。 在实际应用中,建筑供配电系统的设计和建设需要考虑多个方面的因素,如建筑物的类型、规模、用途、电力需求、安全标准等。通过合理的设计和施工,可以确保建筑供配电系统的正常运行和安全性。同时,在建筑供配电系统的维护和管理方面也需要重视,定期检查和维护电气设备,及时发现和解决问题,以确保建筑物内部设备的正常使用。 总的来说,建筑供配电系统是建筑工程中不可或缺的一部分,其重要性不言而喻。通过学习建筑供配电系统的相关知识,可以更好地理解和应用这些原理,提高建筑电力系统的效率和可靠性,确保建筑物内部设备的正常运行和电力安全。建筑供配电系统的设计、建设、维护和管理都需要严谨细致,只有这样才能确保建筑物的电力系统稳定、安全、高效地运行。

"互动学习:行动中的多样性与论文攻读经历"

多样性她- 事实上SCI NCES你的时间表ECOLEDO C Tora SC和NCESPOUR l’Ingén学习互动,互动学习以行动为中心的强化学习学会互动,互动学习,以行动为中心的强化学习计算机科学博士论文于2021年9月28日在Villeneuve d'Asq公开支持马修·瑟林评审团主席法布里斯·勒菲弗尔阿维尼翁大学教授论文指导奥利维尔·皮耶昆谷歌研究教授:智囊团论文联合主任菲利普·普雷教授,大学。里尔/CRISTAL/因里亚报告员奥利维耶·西格德索邦大学报告员卢多维奇·德诺耶教授,Facebook /索邦大学审查员越南圣迈IMT Atlantic高级讲师邀请弗洛里安·斯特鲁布博士,Deepmind对于那些及时看到自己错误的人...3谢谢你首先,我要感谢我的两位博士生导师Olivier和Philippe。奥利维尔,"站在巨人的肩膀上"这句话对你来说完全有意义了。从科学上讲,你知道在这篇论文的(许多)错误中,你是我可以依

【未来发展趋势下的车牌识别技术展望和发展方向】: 展望未来发展趋势下的车牌识别技术和发展方向

![【未来发展趋势下的车牌识别技术展望和发展方向】: 展望未来发展趋势下的车牌识别技术和发展方向](https://img-blog.csdnimg.cn/direct/916e743fde554bcaaaf13800d2f0ac25.png) # 1. 车牌识别技术简介 车牌识别技术是一种通过计算机视觉和深度学习技术,实现对车牌字符信息的自动识别的技术。随着人工智能技术的飞速发展,车牌识别技术在智能交通、安防监控、物流管理等领域得到了广泛应用。通过车牌识别技术,可以实现车辆识别、违章监测、智能停车管理等功能,极大地提升了城市管理和交通运输效率。本章将从基本原理、相关算法和技术应用等方面介绍

在Unity 中开发2D 游戏时怎样让父物体显示在子物体的下方

在Unity中,可以通过修改物体的层级关系来控制父物体和子物体的显示顺序。具体步骤如下: 1. 在Unity的场景视图中,选中子物体。 2. 将子物体拖拽到父物体上,成为父物体的子物体。 3. 在Inspector面板中,将子物体的Transform组件中的Position、Rotation、Scale都清零,确保子物体的坐标系和父物体一致。 4. 在层级面板中,将父物体的层级向上移动,使其显示在子物体的下方。 这样就可以控制父物体和子物体的显示顺序了。注意,如果两个物体层级相同,它们的渲染顺序是由它们在层级面板中的顺序决定的。