本文主要是介绍扩展卡尔曼滤波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实例学习。的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!