利用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

相关文章

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

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

Spring Boot 结合 WxJava 实现文章上传微信公众号草稿箱与群发

《SpringBoot结合WxJava实现文章上传微信公众号草稿箱与群发》本文将详细介绍如何使用SpringBoot框架结合WxJava开发工具包,实现文章上传到微信公众号草稿箱以及群发功能,... 目录一、项目环境准备1.1 开发环境1.2 微信公众号准备二、Spring Boot 项目搭建2.1 创建

IntelliJ IDEA2025创建SpringBoot项目的实现步骤

《IntelliJIDEA2025创建SpringBoot项目的实现步骤》本文主要介绍了IntelliJIDEA2025创建SpringBoot项目的实现步骤,文中通过示例代码介绍的非常详细,对大家... 目录一、创建 Spring Boot 项目1. 新建项目2. 基础配置3. 选择依赖4. 生成项目5.

Linux下删除乱码文件和目录的实现方式

《Linux下删除乱码文件和目录的实现方式》:本文主要介绍Linux下删除乱码文件和目录的实现方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录linux下删除乱码文件和目录方法1方法2总结Linux下删除乱码文件和目录方法1使用ls -i命令找到文件或目录

SpringBoot+EasyExcel实现自定义复杂样式导入导出

《SpringBoot+EasyExcel实现自定义复杂样式导入导出》这篇文章主要为大家详细介绍了SpringBoot如何结果EasyExcel实现自定义复杂样式导入导出功能,文中的示例代码讲解详细,... 目录安装处理自定义导出复杂场景1、列不固定,动态列2、动态下拉3、自定义锁定行/列,添加密码4、合并

mybatis执行insert返回id实现详解

《mybatis执行insert返回id实现详解》MyBatis插入操作默认返回受影响行数,需通过useGeneratedKeys+keyProperty或selectKey获取主键ID,确保主键为自... 目录 两种方式获取自增 ID:1. ​​useGeneratedKeys+keyProperty(推

Spring Boot集成Druid实现数据源管理与监控的详细步骤

《SpringBoot集成Druid实现数据源管理与监控的详细步骤》本文介绍如何在SpringBoot项目中集成Druid数据库连接池,包括环境搭建、Maven依赖配置、SpringBoot配置文件... 目录1. 引言1.1 环境准备1.2 Druid介绍2. 配置Druid连接池3. 查看Druid监控

Linux在线解压jar包的实现方式

《Linux在线解压jar包的实现方式》:本文主要介绍Linux在线解压jar包的实现方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录linux在线解压jar包解压 jar包的步骤总结Linux在线解压jar包在 Centos 中解压 jar 包可以使用 u

c++ 类成员变量默认初始值的实现

《c++类成员变量默认初始值的实现》本文主要介绍了c++类成员变量默认初始值,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着小编来一起学习学习吧... 目录C++类成员变量初始化c++类的变量的初始化在C++中,如果使用类成员变量时未给定其初始值,那么它将被

Qt使用QSqlDatabase连接MySQL实现增删改查功能

《Qt使用QSqlDatabase连接MySQL实现增删改查功能》这篇文章主要为大家详细介绍了Qt如何使用QSqlDatabase连接MySQL实现增删改查功能,文中的示例代码讲解详细,感兴趣的小伙伴... 目录一、创建数据表二、连接mysql数据库三、封装成一个完整的轻量级 ORM 风格类3.1 表结构