扩展卡尔曼滤波EKF,与LKF区别。IMU and GPS Fusion for Inertial Navigation,MATLAB实例学习。

本文主要是介绍扩展卡尔曼滤波EKF,与LKF区别。IMU and GPS Fusion for Inertial Navigation,MATLAB实例学习。,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

MATLAB:Sensor Fusion and Tracking Toolbox

实例一:IMU and GPS Fusion for Inertial Navigation

 一、IMUandGPSFusionExample.m

1、IMU的加速度计、陀螺仪的采样频率很高。Conversely,磁力计与GPS的采样频率较低。

2、数据:

        IMU:

                Orientation ,磁力计 ,用四元数表示

                AngularVelocity,陀螺仪

                Acceleration,加速度计

        GPS:

                Position

                Velocity

3、代码目标

        总体目标:将来自IMU的数据、与来自GPS的数据,通过EKF算法融合新的Position、Orientation。

        具体步骤:

        (1)读入真实的数据。

       (2)利用imu、gps模型(噪声参数自己设置),仿真得到IMU、GPS传感器的测量数据。

[accel, gyro, mag] = imu(trajAcc(fcnt,:), trajAngVel(fcnt, :), trajOrient(fcnt));[lla, gpsvel] = gps( trajPos(fcnt,:), trajVel(fcnt,:) );

      (3)通过IMU的 acc 、gyro数据,用predict函数去预测 Position and Orientation。

predict(fusionfilt, accel, gyro);

      (4)通过GPS的position数据,用fusegps函数去做Position的更新。通过IMU的magn数据,用fusemag函数去做Orientation的更新。

fusegps(fusionfilt, lla, Rpos, gpsvel, Rvel);
fusemag(fusionfilt, mag, Rmag);

   

二、核心代码 MARGGPSFuserBase.m文件

        1、 AccelerometerNoise = [1e-4 1e-4 1e-4];加速度计噪声的协方差

        2、AccelerometerBiasNoise = [1e-4 1e-4 1e-4];是加速度计bias协方差。

                这里IMU的噪声模型参考这篇文章:IMU误差模型与校准 - 修禅 - 博客园

                ①、noise是高斯噪声

                ②、bias是随机游走噪声、bias指零偏。

        3、% Multiplicative Process Noises

                乘法噪声

        4、% Additive Process Noises

                加法噪声

        5、预测过程

% Extended Kalman Filter predict algorithmxk:k时刻的状态
dang:角度变化
dvel:速度变化
dt:delat TXnext:K+1时刻的状态
P:协方差xnext = obj.stateTransFcn(xk, dang, dvel, dt);  %相当于A
dfdx = obj.stateTransJacobianFcn(xk, dang, dvel, dt);   %计算一阶雅克比           
dwdx = obj.processNoiseJacobianFcn(xk, multNoise);  %计算乘法噪声的一阶雅克比
Pnext = dfdx * P * (dfdx.') + dwdx  + addProcNoise; % 代替 APA+Q
xk = xnext;
P = Pnext;

        6、更新过程

% Basic EKF correctxk = obj.State;
h = measFcn(obj, xk);  % measFcn:观测方程 ,h是xk的观测值,相当于H*xk
innov = z - h;  % z:观测值,innov是观测误差 xest = xk + K*innov
dhdx = measJacobianFcn(obj, xk);
P = obj.StateCovariance;
[xest, P] = correctEqn(obj, xk, P, h, dhdx, z, measNoise);
obj.StateCovariance = P;
obj.State = xest;function [x, P] = correctEqn(obj, x, P, h, H, z, R)S = H*P*(H.') + R;W = P*(H.') / S;x = x + W*(z-h);P = P - W*H*P;end 

        7、EKF扩展卡尔曼滤波与LKF线性卡尔曼滤波区别

                ①线性卡尔曼滤波流程

                    ②、EKF体现:

                        1)在预测过程中:

                                状态估计:用obj.stateTransFcn 代替 A.*

                                计算P:用 Pnext = dfdx * P * (dfdx.') + dwdx  + addProcNoise 代替 APA+Q

                        3)在更新过程:

                                用 dhdx 代替 H。

        8、补充

                卡尔曼滤波主要是如何确定P的初值,只要不为0,对于初值不敏感(没有试过)。

                                

                                    

                                

这篇关于扩展卡尔曼滤波EKF,与LKF区别。IMU and GPS Fusion for Inertial Navigation,MATLAB实例学习。的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



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

相关文章

C# WinForms存储过程操作数据库的实例讲解

《C#WinForms存储过程操作数据库的实例讲解》:本文主要介绍C#WinForms存储过程操作数据库的实例,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录一、存储过程基础二、C# 调用流程1. 数据库连接配置2. 执行存储过程(增删改)3. 查询数据三、事务处

分辨率三兄弟LPI、DPI 和 PPI有什么区别? 搞清分辨率的那些事儿

《分辨率三兄弟LPI、DPI和PPI有什么区别?搞清分辨率的那些事儿》分辨率这个东西,真的是让人又爱又恨,为了搞清楚它,我可是翻阅了不少资料,最后发现“小7的背包”的解释最让我茅塞顿开,于是,我... 在谈到分辨率时,我们经常会遇到三个相似的缩写:PPI、DPI 和 LPI。虽然它们看起来差不多,但实际应用

springboot security验证码的登录实例

《springbootsecurity验证码的登录实例》:本文主要介绍springbootsecurity验证码的登录实例,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,... 目录前言代码示例引入依赖定义验证码生成器定义获取验证码及认证接口测试获取验证码登录总结前言在spring

tomcat多实例部署的项目实践

《tomcat多实例部署的项目实践》Tomcat多实例是指在一台设备上运行多个Tomcat服务,这些Tomcat相互独立,本文主要介绍了tomcat多实例部署的项目实践,具有一定的参考价值,感兴趣的可... 目录1.创建项目目录,测试文China编程件2js.创建实例的安装目录3.准备实例的配置文件4.编辑实例的

python+opencv处理颜色之将目标颜色转换实例代码

《python+opencv处理颜色之将目标颜色转换实例代码》OpenCV是一个的跨平台计算机视觉库,可以运行在Linux、Windows和MacOS操作系统上,:本文主要介绍python+ope... 目录下面是代码+ 效果 + 解释转HSV: 关于颜色总是要转HSV的掩膜再标注总结 目标:将红色的部分滤

GORM中Model和Table的区别及使用

《GORM中Model和Table的区别及使用》Model和Table是两种与数据库表交互的核心方法,但它们的用途和行为存在著差异,本文主要介绍了GORM中Model和Table的区别及使用,具有一... 目录1. Model 的作用与特点1.1 核心用途1.2 行为特点1.3 示例China编程代码2. Tab

Nginx指令add_header和proxy_set_header的区别及说明

《Nginx指令add_header和proxy_set_header的区别及说明》:本文主要介绍Nginx指令add_header和proxy_set_header的区别及说明,具有很好的参考价... 目录Nginx指令add_header和proxy_set_header区别如何理解反向代理?proxy

Java常用注解扩展对比举例详解

《Java常用注解扩展对比举例详解》:本文主要介绍Java常用注解扩展对比的相关资料,提供了丰富的代码示例,并总结了最佳实践建议,帮助开发者更好地理解和应用这些注解,需要的朋友可以参考下... 目录一、@Controller 与 @RestController 对比二、使用 @Data 与 不使用 @Dat

Java中&和&&以及|和||的区别、应用场景和代码示例

《Java中&和&&以及|和||的区别、应用场景和代码示例》:本文主要介绍Java中的逻辑运算符&、&&、|和||的区别,包括它们在布尔和整数类型上的应用,文中通过代码介绍的非常详细,需要的朋友可... 目录前言1. & 和 &&代码示例2. | 和 ||代码示例3. 为什么要使用 & 和 | 而不是总是使

C++中函数模板与类模板的简单使用及区别介绍

《C++中函数模板与类模板的简单使用及区别介绍》这篇文章介绍了C++中的模板机制,包括函数模板和类模板的概念、语法和实际应用,函数模板通过类型参数实现泛型操作,而类模板允许创建可处理多种数据类型的类,... 目录一、函数模板定义语法真实示例二、类模板三、关键区别四、注意事项 ‌在C++中,模板是实现泛型编程