扩展卡尔曼滤波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

相关文章

Vue3组件中getCurrentInstance()获取App实例,但是返回null的解决方案

《Vue3组件中getCurrentInstance()获取App实例,但是返回null的解决方案》:本文主要介绍Vue3组件中getCurrentInstance()获取App实例,但是返回nu... 目录vue3组件中getCurrentInstajavascriptnce()获取App实例,但是返回n

go 指针接收者和值接收者的区别小结

《go指针接收者和值接收者的区别小结》在Go语言中,值接收者和指针接收者是方法定义中的两种接收者类型,本文主要介绍了go指针接收者和值接收者的区别小结,文中通过示例代码介绍的非常详细,需要的朋友们下... 目录go 指针接收者和值接收者的区别易错点辨析go 指针接收者和值接收者的区别指针接收者和值接收者的

售价599元起! 华为路由器X1/Pro发布 配置与区别一览

《售价599元起!华为路由器X1/Pro发布配置与区别一览》华为路由器X1/Pro发布,有朋友留言问华为路由X1和X1Pro怎么选择,关于这个问题,本期图文将对这二款路由器做了期参数对比,大家看... 华为路由 X1 系列已经正式发布并开启预售,将在 4 月 25 日 10:08 正式开售,两款产品分别为华

SQL表间关联查询实例详解

《SQL表间关联查询实例详解》本文主要讲解SQL语句中常用的表间关联查询方式,包括:左连接(leftjoin)、右连接(rightjoin)、全连接(fulljoin)、内连接(innerjoin)、... 目录简介样例准备左外连接右外连接全外连接内连接交叉连接自然连接简介本文主要讲解SQL语句中常用的表

kotlin中const 和val的区别及使用场景分析

《kotlin中const和val的区别及使用场景分析》在Kotlin中,const和val都是用来声明常量的,但它们的使用场景和功能有所不同,下面给大家介绍kotlin中const和val的区别,... 目录kotlin中const 和val的区别1. val:2. const:二 代码示例1 Java

CSS Padding 和 Margin 区别全解析

《CSSPadding和Margin区别全解析》CSS中的padding和margin是两个非常基础且重要的属性,它们用于控制元素周围的空白区域,本文将详细介绍padding和... 目录css Padding 和 Margin 全解析1. Padding: 内边距2. Margin: 外边距3. Padd

Springboot @Autowired和@Resource的区别解析

《Springboot@Autowired和@Resource的区别解析》@Resource是JDK提供的注解,只是Spring在实现上提供了这个注解的功能支持,本文给大家介绍Springboot@... 目录【一】定义【1】@Autowired【2】@Resource【二】区别【1】包含的属性不同【2】@

Java中的String.valueOf()和toString()方法区别小结

《Java中的String.valueOf()和toString()方法区别小结》字符串操作是开发者日常编程任务中不可或缺的一部分,转换为字符串是一种常见需求,其中最常见的就是String.value... 目录String.valueOf()方法方法定义方法实现使用示例使用场景toString()方法方法

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

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

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

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