基于Matlab移动车辆异步传感器姿态估计（附源码）

目录

一、模拟设置

二、融合过滤器

三、初始化的状态向量

四、设置过程噪声值

五、定义用于融合传感器数据的测量噪声值

六、初始化作用域

 七、总结

八、程序

---

此示例说明如何以不同的速率融合传感器以估计姿势。加速度计、陀螺仪、磁力计和 GPS 用于确定沿圆形路径移动的车辆的方向和位置。可以使用图形窗口上的控件来改变传感器速率并试验传感器丢失，同时查看对估计姿势的影响。

一、模拟设置

加载预先记录的传感器数据。传感器数据基于使用 theclass 创建的圆形轨迹。传感器值是使用 andclasses 创建的。这里使用的文件可以用函数生成。

ld = load('CircularTrajectorySensorData.mat');

Fs = ld.Fs; % maximum MARG rate
gpsFs = ld.gpsFs; % maximum GPS rate
ratio = Fs./gpsFs;
refloc = ld.refloc;

trajOrient = ld.trajData.Orientation;
trajVel = ld.trajData.Velocity;
trajPos = ld.trajData.Position;
trajAcc = ld.trajData.Acceleration;
trajAngVel = ld.trajData.AngularVelocity;

accel = ld.accel;
gyro = ld.gyro;
mag = ld.mag;
lla = ld.lla;
gpsvel = ld.gpsvel;

二、融合过滤器

创建安托保险丝 IMU + GPS 测量。该融合滤波器使用连续离散扩展卡尔曼滤波器 （EKF） 来跟踪方向（作为四元数）、角速度、位置、速度、加速度、传感器偏差和地磁矢量。由于使用连续离散 EKF，该方法可以将滤波器向前步进任意时间量。

fusionfilt = insfilterAsync('ReferenceLocation', refloc);

三、初始化的状态向量

跟踪元素向量中的姿势状态。

 帮助初始化筛选器状态，以便筛选器快速收敛到良好的答案。

Nav = 100;
initstate = zeros(28,1);
initstate(1:4) = compact( meanrot(trajOrient(1:Nav)));
initstate(5:7) = mean( trajAngVel(10:Nav,:), 1);
initstate(8:10) = mean( trajPos(1:Nav,:), 1);
initstate(11:13) = mean( trajVel(1:Nav,:), 1);
initstate(14:16) = mean( trajAcc(1:Nav,:), 1);
initstate(23:25) = ld.magField;

% The gyroscope bias initial value estimate is low for the Z-axis. This is
% done to illustrate the effects of fusing the magnetometer in the
% simulation.
initstate(20:22) = deg2rad([3.125 3.125 3.125]);
fusionfilt.State = initstate;

四、设置过程噪声值

过程噪声方差描述了滤波器使用的运动模型的不确定性。

fusionfilt.QuaternionNoise = 1e-2;
fusionfilt.AngularVelocityNoise = 100;
fusionfilt.AccelerationNoise = 100;
fusionfilt.MagnetometerBiasNoise = 1e-7;
fusionfilt.AccelerometerBiasNoise = 1e-7;
fusionfilt.GyroscopeBiasNoise = 1e-7;

五、定义用于融合传感器数据的测量噪声值

每个传感器在测量中都有一些噪声。这些值通常可以在传感器的数据表上找到。

Rmag = 0.4;
Rvel = 0.01;
Racc = 610;
Rgyro = 0.76e-5;
Rpos = 3.4;

fusionfilt.StateCovariance = diag(1e-3*ones(28,1));

六、初始化作用域

示波器可以绘制变量随时间的变化。它在这里用于跟踪姿势错误。示波器允许滤波器估计和地面实况姿势的 3D 可视化。示波器可能会减慢模拟速度。要禁用作用域，请将相应的逻辑变量设置为 false。

for ii=1:size(accel,1)
    fusionfilt.predict(1./Fs);

    % Fuse Accelerometer
    if (f.UserData.Accelerometer) && ...
        mod(ii, fix(Fs/f.UserData.AccelerometerSampleRate)) == 0

        fusionfilt.fuseaccel(accel(ii,:), Racc);
    end

    % Fuse Gyroscope
    if (f.UserData.Gyroscope) && ...
        mod(ii, fix(Fs/f.UserData.GyroscopeSampleRate)) == 0

        fusionfilt.fusegyro(gyro(ii,:), Rgyro);
    end

    % Fuse Magnetometer
    if (f.UserData.Magnetometer) && ...
        mod(ii, fix(Fs/f.UserData.MagnetometerSampleRate)) == 0

        fusionfilt.fusemag(mag(ii,:), Rmag);
    end

    % Fuse GPS
    if (f.UserData.GPS) && mod(ii, fix(Fs/f.UserData.GPSSampleRate)) == 0
        fusionfilt.fusegps(lla(ii,:), Rpos, gpsvel(ii,:), Rvel);
    end

    % Plot the pose error
    [p,q] = pose(fusionfilt);
    posescope(p, q, trajPos(ii,:), trajOrient(ii));

    orientErr = rad2deg(dist(q, trajOrient(ii) ));
    posErr = p - trajPos(ii,:);
    errscope(orientErr, posErr(1), posErr(2), posErr(3));
end

 七、总结

允许各种和不同的采样率。估计输出的质量在很大程度上取决于各个传感器的融合速率。任何传感器压降都会对输出产生深远的影响。

八、程序

使用Matlab R2022b版本，点击打开。

 打开下面的“PoseEstimation...SensorsExample.m”文件，点击运行，就可以看到上述效果。 

关注下面公众号，后台回复关键词：异步传感器姿态估计，发送源码链接。