利用matlab实现线型卡尔曼滤波(LKF)

2023-12-25 11:40

本文主要是介绍利用matlab实现线型卡尔曼滤波(LKF),希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

LKF算法模型

在这里插入图片描述

例题

在这里插入图片描述
在这里插入图片描述

实现

在这里插入图片描述

观测只有位置
clc;
clear;
%测量值模拟
N=100;%观测次数
t=0:1:N-1;%假定输出周期
T=1;
x=zeros(6,N);
z=zeros(3,N);
x0=[0;0;50;0;5*cos(pi/6);5*sin(pi/6)];%真值初始值
mu1=[0;0;0;0;0;0];
mu2=[0;0;0];
Q=diag(0.1*[9,9,20.25,0.01,0.01,0.0225]);
R=diag([9,9,20.25]);rng(1);
w=mvnrnd(mu1,Q,N)';
v=mvnrnd(mu2,R,N)';
x(:,1)=x0;
for k=1:N-1x1=x(1,k);x2=x(2,k);x3=x(3,k);x4=x(4,k);x5=x(5,k);x6=x(6,k);x(:,k+1)=[x1;x2+x5*T;x3+x6*T;x4;x5;x6]+w(:,k);z(:,k+1)=x(1:3,k+1)+v(:,k+1);
end
%RLS估计
xr=zeros(6,N);
xr(:,1)=[0;0;0;0;0;0];
P=diag([1000,1000,1000,100,100,100]);
H=[eye(3),zeros(3,3)];
for k=1:N-1K=P*H'/(H*P*H'+R);xr(:,k+1)=xr(:,k)+K*(z(:,k+1)-H*xr(:,k));P=P-K*H*P;
end
%LKF估计
xk=zeros(6,N);
xk(:,1)=[0;0;0;0;0;0];
Pk=diag([1000,1000,1000,100,100,100]);
Fai=[eye(3),T.*eye(3);zeros(3,3),eye(3)];
Hk=[eye(3),zeros(3,3)];
for k=1:N-1xk(:,k)=Fai*xk(:,k);Pk=Fai*Pk*Fai'+Q;Kk=Pk*Hk'/(Hk*Pk*Hk'+R);xk(:,k+1)=xk(:,k)+Kk*(z(:,k+1)-Hk*xk(:,k));Pk=(eye(6)-Kk*Hk)*Pk*(eye(6)-Kk*Hk)'+Kk*R*Kk';
end

输出结果:
真实值、观测值、递推最小二乘估计值和卡尔曼滤波法估计值曲线如下图:
在这里插入图片描述
递推最小二乘估计值和卡尔曼滤波法估计值与真实值的误差如下图:
在这里插入图片描述

观测只有速度
clc;
clear;
%测量值模拟
N=100;%观测次数
t=0:1:N-1;%假定输出周期
T=1;
x=zeros(6,N);
z=zeros(3,N);
x0=[0;0;50;0;5*cos(pi/6);5*sin(pi/6)];%真值初始值
mu1=[0;0;0;0;0;0];
mu2=[0;0;0];
Q=diag(0.1*[9,9,20.25,0.01,0.01,0.0225]);
R=diag([0.01;0.01;0.0225]);rng(1);
w=mvnrnd(mu1,Q,N)';
v=mvnrnd(mu2,R,N)';
x(:,1)=x0;
for k=1:N-1x1=x(1,k);x2=x(2,k);x3=x(3,k);x4=x(4,k);x5=x(5,k);x6=x(6,k);x(:,k+1)=[x1;x2+x5*T;x3+x6*T;x4;x5;x6]+w(:,k);z(:,k+1)=x(4:6,k+1)+v(:,k+1);
end
%RLS估计
xr=zeros(6,N);
xr(:,1)=[0;0;0;0;0;0];
P=diag([1000,1000,1000,100,100,100]);
H=[zeros(3,3),eye(3)];
for k=1:N-1K=P*H'/(H*P*H'+R);xr(:,k+1)=xr(:,k)+K*(z(:,k+1)-H*xr(:,k));P=P-K*H*P;
end
%LKF估计
xk=zeros(6,N);
xk(:,1)=[0;0;0;0;0;0];
Pk=diag([1000,1000,1000,100,100,100]);
Fai=eye(6);
Hk=[zeros(3,3),eye(3)];
for k=1:N-1xk(:,k)=Fai*xk(:,k);Pk=Fai*Pk*Fai'+Q;Kk=Pk*Hk'/(Hk*Pk*Hk'+R);xk(:,k+1)=xk(:,k)+Kk*(z(:,k+1)-Hk*xk(:,k));Pk=(eye(6)-Kk*Hk)*Pk*(eye(6)-Kk*Hk)'+Kk*R*Kk';
end

输出结果:
真实值、观测值、递推最小二乘估计值和卡尔曼滤波法估计值曲线如下图:
在这里插入图片描述
递推最小二乘估计值和卡尔曼滤波法估计值与真实值的误差如下图:
在这里插入图片描述

观测既有位置又有速度
clc;
clear;
%测量值模拟
N=100;%观测次数
t=0:1:N-1;%假定输出周期
T=1;
x=zeros(6,N);
z=zeros(6,N);
x0=[0;0;50;0;5*cos(pi/6);5*sin(pi/6)];%真值初始值
mu=[0;0;0;0;0;0];
Q=diag(0.1*[9,9,20.25,0.01,0.01,0.0225]);
R=diag([9,9,20.25,0.01,0.01,0.0225]);rng(1);
w=mvnrnd(mu,Q,N)';
v=mvnrnd(mu,R,N)';
x(:,1)=x0;
for k=1:N-1x1=x(1,k);x2=x(2,k);x3=x(3,k);x4=x(4,k);x5=x(5,k);x6=x(6,k);x(:,k+1)=[x1;x2+x5*T;x3+x6*T;x4;x5;x6]+w(:,k);z(:,k+1)=x(:,k+1)+v(:,k+1);
end
%RLS估计
xr=zeros(6,N);
xr(:,1)=[0;0;0;0;0;0];
P=diag([1000,1000,1000,100,100,100]);
H=eye(6);
for k=1:N-1K=P*H'/(H*P*H'+R);xr(:,k+1)=xr(:,k)+K*(z(:,k+1)-H*xr(:,k));P=P-K*H*P;
end
%LKF估计
xk=zeros(6,N);
xk(:,1)=[0;0;0;0;0;0];
Pk=diag([1000,1000,1000,100,100,100]);
Fai=[eye(3),T.*eye(3);zeros(3,3),eye(3)];
Hk=eye(6);
for k=1:N-1xk(:,k)=Fai*xk(:,k);Pk=Fai*Pk*Fai'+Q;Kk=Pk*Hk'/(Hk*Pk*Hk'+R);xk(:,k+1)=xk(:,k)+Kk*(z(:,k+1)-Hk*xk(:,k));Pk=(eye(6)-Kk*Hk)*Pk*(eye(6)-Kk*Hk)'+Kk*R*Kk';
end

输出结果:
真实值、观测值、递推最小二乘估计值和卡尔曼滤波法估计值曲线如下图:
在这里插入图片描述
递推最小二乘估计值和卡尔曼滤波法估计值与真实值的误差如下图:
在这里插入图片描述

这篇关于利用matlab实现线型卡尔曼滤波(LKF)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



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

相关文章

SpringBoot3实现Gzip压缩优化的技术指南

《SpringBoot3实现Gzip压缩优化的技术指南》随着Web应用的用户量和数据量增加,网络带宽和页面加载速度逐渐成为瓶颈,为了减少数据传输量,提高用户体验,我们可以使用Gzip压缩HTTP响应,... 目录1、简述2、配置2.1 添加依赖2.2 配置 Gzip 压缩3、服务端应用4、前端应用4.1 N

SpringBoot实现数据库读写分离的3种方法小结

《SpringBoot实现数据库读写分离的3种方法小结》为了提高系统的读写性能和可用性,读写分离是一种经典的数据库架构模式,在SpringBoot应用中,有多种方式可以实现数据库读写分离,本文将介绍三... 目录一、数据库读写分离概述二、方案一:基于AbstractRoutingDataSource实现动态

Python FastAPI+Celery+RabbitMQ实现分布式图片水印处理系统

《PythonFastAPI+Celery+RabbitMQ实现分布式图片水印处理系统》这篇文章主要为大家详细介绍了PythonFastAPI如何结合Celery以及RabbitMQ实现简单的分布式... 实现思路FastAPI 服务器Celery 任务队列RabbitMQ 作为消息代理定时任务处理完整

Java枚举类实现Key-Value映射的多种实现方式

《Java枚举类实现Key-Value映射的多种实现方式》在Java开发中,枚举(Enum)是一种特殊的类,本文将详细介绍Java枚举类实现key-value映射的多种方式,有需要的小伙伴可以根据需要... 目录前言一、基础实现方式1.1 为枚举添加属性和构造方法二、http://www.cppcns.co

使用Python实现快速搭建本地HTTP服务器

《使用Python实现快速搭建本地HTTP服务器》:本文主要介绍如何使用Python快速搭建本地HTTP服务器,轻松实现一键HTTP文件共享,同时结合二维码技术,让访问更简单,感兴趣的小伙伴可以了... 目录1. 概述2. 快速搭建 HTTP 文件共享服务2.1 核心思路2.2 代码实现2.3 代码解读3.

MySQL双主搭建+keepalived高可用的实现

《MySQL双主搭建+keepalived高可用的实现》本文主要介绍了MySQL双主搭建+keepalived高可用的实现,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,... 目录一、测试环境准备二、主从搭建1.创建复制用户2.创建复制关系3.开启复制,确认复制是否成功4.同

Java实现文件图片的预览和下载功能

《Java实现文件图片的预览和下载功能》这篇文章主要为大家详细介绍了如何使用Java实现文件图片的预览和下载功能,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... Java实现文件(图片)的预览和下载 @ApiOperation("访问文件") @GetMapping("

使用Sentinel自定义返回和实现区分来源方式

《使用Sentinel自定义返回和实现区分来源方式》:本文主要介绍使用Sentinel自定义返回和实现区分来源方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录Sentinel自定义返回和实现区分来源1. 自定义错误返回2. 实现区分来源总结Sentinel自定

Java实现时间与字符串互相转换详解

《Java实现时间与字符串互相转换详解》这篇文章主要为大家详细介绍了Java中实现时间与字符串互相转换的相关方法,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录一、日期格式化为字符串(一)使用预定义格式(二)自定义格式二、字符串解析为日期(一)解析ISO格式字符串(二)解析自定义

opencv图像处理之指纹验证的实现

《opencv图像处理之指纹验证的实现》本文主要介绍了opencv图像处理之指纹验证的实现,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着小编来一起学... 目录一、简介二、具体案例实现1. 图像显示函数2. 指纹验证函数3. 主函数4、运行结果三、总结一、