ESKF IMU+GPS融合定位 MATLAB、c++实现

BUG-生产专家

已于 2023-03-01 17:43:04 修改

阅读量5.4k

点赞数 11

文章标签： matlab 线性代数矩阵 c++

于 2022-01-18 12:20:35 首次发布

本文链接：https://blog.csdn.net/weixin_44149262/article/details/122531012

版权

1. 在展示公式和代码之前，先理清一下旋转矩阵，四元素，轴角，旋转向量之间的关系

假设一个旋转向量为：

$\textbf{v} = \[a,b,c\]$

将旋转向量表示成轴角形式

$\mathbf{\phi} = \left \| v\right \| , \mathbf{u} = \frac{v}{\left \| v\right \|}$

所以

$\mathbf{v} = \phi \mathbf{u}$

轴角转换到旋转矩阵形式

轴角转换成四元数形式

反对称矩阵

2. nominal state process model

公式中q{x}, 指的是把旋转向量x，转换成对应的四元数，其他的都是普通的矩阵相乘和相加，具体可以看如下代码, 其中状态变量定义成如下图

% input
% dimension
% acc_data  ---- 3x1
% gyro_data ---- 3x1
% x_t       ---- cell which includes position(3x1), velocity(3x1), orientation(quaternion type), acceleration bias(3x1), gyro bias(3x1) and graviaty constant(3x1)
%% delta_t  ---- IMU sample rate
%% reference formular from 165a-165af 
function [x_t] = eskf_nominal_state_predict(x_t, acc_data, gyro_data, delta_t)

    p = x_t{1};
    v = x_t{2};
    q = x_t{3};
    ab = x_t{4};
    wb = x_t{5};
    g = x_t{6};
    R = quat2rotm(q);
    temp = (R * (acc_data - ab) + g);
    x_t{1} = p + v * delta_t + 0.5 * temp * delta_t * delta_t;
    x_t{2} = v + temp * delta_t;
    q = q * quaternion(((gyro_data - wb) * delta_t).', 'rotvec');
    x_t{3} = normalize(q);
    x_t{4} = ab;
    x_t{5} = wb;
    x_t{6} = g;
end

3. error state process model

$\mathbf{I}$ 是一个3x3的单位矩阵

$\mathbf{R{\[a_m-a_b\]}_x}$ 就是一个旋转矩阵乘上一个反对称矩阵

$\mathbf{R^T\{(\omega_m - \omega_b)}\bigtriangledown t\}$ 就是将花括号里面的三维变量（旋转向量）转换为旋转矩阵

$\mathbf{Q_i}$ 矩阵对角线上的内容如下

代码如下

function [delta_x, P] = eskf_err_state_predict(delta_x, x_t, P, acc_data, gyro_data, delta_t, am_noise, wm_noise, a_bias_noise, w_bias_noise)
    Fx = eye(18, 18);
    Fx(1:3, 4:6) = eye(3) * delta_t;
    skeu_matrix = @(x) [0, -x(3), x(2);
                    x(3), 0, -x(1);
                    -x(2), x(1), 0];
    ab = x_t{4};
    wb = x_t{5};
    q = x_t{3};
    R = quat2rotm(q);
    Fx(4:6, 7:9) = -R * skeu_matrix(acc_data - ab) * delta_t;
    Fx(4:6, 10:12) = -R * delta_t;
    Fx(4:6, 16:18) = eye(3) * delta_t;
    Fx(7:9, 7:9) = (rotationVectorToMatrix((gyro_data - wb) * delta_t)).';
    Fx(7:9, 13:15) = -eye(3) * delta_t;

    Fi = zeros(18, 12);
    Fi(4:6, 1:3) = eye(3);
    Fi(7:9, 4:6) = eye(3);
    Fi(10:12, 7:9) = eye(3);
    Fi(13:15, 10:12) = eye(3);

    Qi = zeros(12, 12);
    Qi(1:3, 1:3) = am_noise^2 * delta_t^2 * eye(3);
    Qi(4:6, 4:6) = wm_noise^2 * delta_t^2 * eye(3);
    Qi(7:9, 7:9) = a_bias_noise^2 * delta_t * eye(3);
    Qi(10:12, 10:12) = w_bias_noise^2 * delta_t * eye(3);

    P = Fx * P * Fx.' + Fi * Qi * Fi.';
    delta_x = Fx * delta_x;
end

4. measurement model

为了简化例子，我使用的measurement就只有GPS。

先贴一下测量更新的部分公式

在本例子上

$\mathbf{y} = h(\mathbf{x_t})) +v = [P_x,P_y,P_z, V_x, V_y, V_z]^T + v$

H矩阵求解如以下公式

where

$\mathbf{H_x} = \begin{bmatrix} 1 & 0& 0 &0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 &0 \\ 0& 1& 0& 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 &0 \\ 0& 0& 1& 0 &0 & 0 & 0 &0 & 0 &0 & 0 & 0& 0 & 0 & 0& 0 & 0 & 0 &0 \\ 0& 0& 0 & 1 & 0& 0 &0 &0 &0 &0 & 0&0 &0 &0 &0 &0 &0 &0 &0 \\ 0& 0& 0 & 0 & 1 & 0 &0 &0 &0 &0 &0 &0 &0 &0 &0 &0 & 0& 0& 0\\ 0& 0& 0 & 0 & 0 &1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix}$

具体测量更新的代码如下

function [delta_x, P] = eskf_measurement_update(x_t, measurement, P, delta_x, GPS_POS_NOISE, GPS_VEL_NOISE)
    Hx = zeros(6, 19);
    Hx(1:6, 1:6) = eye(6, 6);
    X_Deltax = zeros(19, 18);
    X_Deltax(1:6, 1:6) = eye(6);
    q = x_t{3}.compact;
    qx = q(2);
    qy = q(3);
    qz = q(4);
    qw = q(1);
    Q_DeltaTheta = 0.5 * [-qx, -qy, -qz;
                    qw, -qz, qy;
                    qz, qw, -qx;
                    -qy, qx, qw];
    X_Deltax(7:10, 7:9) = Q_DeltaTheta;
    X_Deltax(11:19, 10:18) = eye(9);
    GPS_X_ERR = GPS_POS_NOISE;
    GPS_Y_ERR = GPS_POS_NOISE;
    GPS_Z_ERR = GPS_POS_NOISE;
    GPS_VX_ERR = GPS_VEL_NOISE;
    GPS_VY_ERR = GPS_VEL_NOISE;
    GPS_VZ_ERR = GPS_VEL_NOISE;
    V = [GPS_X_ERR^2, 0, 0, 0, 0, 0;
        0, GPS_Y_ERR^2, 0, 0, 0, 0;
        0, 0, GPS_Z_ERR^2, 0, 0, 0;
        0, 0, 0, GPS_VX_ERR^2, 0, 0;
        0, 0, 0, 0, GPS_VY_ERR^2, 0;
        0, 0, 0, 0, 0, GPS_VZ_ERR^2];
    H = Hx * X_Deltax;

    K = P * H.' * (H * P * H.' + V);
    delta_x = delta_x + K * (measurement - cell2mat(x_t(1:2)));
    P = (eye(size(P, 1)) - K * H) * P;
end

5. state correction

6. reset

第五步修正完状态之后，要把误差项和协方差进行修正，修正公式如下图

为了简化，矩阵 $\mathbf{G}$ 可以设置成单位矩阵

7. simulation

测试的参数都是大概调，目的是学习怎么实现，有兴趣的话，可以自己在代码上对GPS，IMU的参数进行调整

imu + gps（left）， only imu(right)

8. REFERENCE

Sola, Joan. "Quaternion kinematics for the error-state Kalman filter." arXiv preprint arXiv:1711.02508 (2017).

9. code example

clear all
close all
%%
% created by HaochanYou
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%generate data start%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% generate IMU DATA, GPS DATA
waypoints = [0, 0, 0; ... % Initial position
            3.5, 1, 1; ...
            5, 9.5, 2; ...
             6.2, 12, 3; ...
            8.6, 22.5, 3; ...
            10.2, 25.6, 5; ...
            20.2, 30.5, 3.1; ...
            25.2, 31, 3.3; ...
            30.2, 32.6, 3; ...
            35, 33, 2]; % Final position

        
toa = 0:5:45; % time of arrival`

orientation = quaternion([0, 0, 0; ...
                        0, 1, 1; ...
                        0, 0, 0; ...
                        1, 1, 2; ...
                        1.3, 1, 2; ...
                        1, 1.5, 2; ...
                        1, 1, 2.6; ...
                        1, 1.4, 2; ...
                        1.6, 1, 2; ...
                        1, 2, 0], ...
    'euler', 'ZYX', 'frame');
deltat = 100;
trajectory = waypointTrajectory(waypoints, ...
    'TimeOfArrival', toa, ...
    'Orientation', orientation, ...
    'SampleRate', deltat, ...
    'ReferenceFrame', 'NED');


vel = zeros(toa(end) * trajectory.SampleRate, 3);
acc = zeros(toa(end) * trajectory.SampleRate, 3);
av = zeros(toa(end) * trajectory.SampleRate, 3);
currentPosition = zeros(toa(end) * trajectory.SampleRate, 3);
orientationLog = zeros(toa(end) * trajectory.SampleRate, 1, 'quaternion');
count = 1;

while ~isDone(trajectory)
    [currentPosition(count, :), orientationLog(count), vel(count, :), acc(count, :), av(count, :)] = trajectory();

    %    pause(trajectory.SamplesPerFrame/trajectory.SampleRate)
    count = count + 1;
end

rpy = euler(orientationLog, 'ZYX', 'frame');

% %% IMU simulation
deltaT = 1 / deltat;
imu = imuSensor('accel-gyro');
% % Accelerometer
% imu.Accelerometer.MeasurementRange = 19.6133;
% imu.Accelerometer.Resolution = 0.0023928;
imu.Accelerometer.NoiseDensity = 0.0012356;

% Gyroscope
% imu.Gyroscope.MeasurementRange = deg2rad(250);
% imu.Gyroscope.Resolution = deg2rad(0.00625);
% imu.Gyroscope.ConstantBias = deg2rad([3.125 3.125 7]);
% imu.Gyroscope.AxesMisalignment = [1.5 0 3];
imu.Gyroscope.NoiseDensity = deg2rad(0.0025);


[accel, gyro] = imu(acc, av, orientationLog);

accels = -accel;


%% GPS Simulation
gps = gpsSensor('SampleRate', 100);
% gps.ReferenceLocation = refloc;
gps.DecayFactor = 0.99;
gps.HorizontalPositionAccuracy = 0.05;
gps.VerticalPositionAccuracy = 0.05;
gps.VelocityAccuracy = 0.05;

% gps.DecayFactor = 0;

[llaGPSRate, gpsvelGPSRate] = gps(currentPosition, ...
vel);
lla0 = [0, 0, 0];
GPS_XYZ = lla2ned(llaGPSRate, lla0, 'flat');
%%%%%%%%%%%%%%%%%%%%%%%%%%generate data done%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% csvwrite("GPS_XYZ.txt", GPS_XYZ);
% csvwrite("gpsvelGPSRate.txt", gpsvelGPSRate);
% csvwrite("accels.txt", accels);
% csvwrite("gyro.txt", gyro);


% GPS_XYZ = csvread("GPS_XYZ.txt");
% gpsvelGPSRate = csvread("gpsvelGPSRate.txt");
% accels = csvread("accels.txt");
% gyro = csvread("gyro.txt");
%% start fuse the imu data and gps data to estimate the pose of robot

ab = [0; 0; 0];
wb = [0; 0; 0];
p = currentPosition(1, :).';
v = vel(1, :).';
q = orientationLog(1);
g = [0; 0; 9.81];
pos = [];
pos_predict = [];
velocitys = [];
qs = [];
x_t = cell(6, 1);

%% only use imu data to get the pose of robot
x_t(1) = {p};
x_t(2) = {v};
x_t(3) = {q};
x_t(4) = {ab};
x_t(5) = {wb};
x_t(6) = {g};
delta_x = zeros(18, 1);
P = zeros(18, 18);

for i = 1:size(accels, 1)
    x_t = eskf_nominal_state_predict(x_t, accels(i, :).', gyro(i, :).', deltaT);
    pos_predict = [pos_predict, x_t{1}];
end
%% only use imu data to get the pose of robot  done


%% use imu data to predict, gps to update

x_t = cell(6, 1);
x_t(1) = {p};
x_t(2) = {v};
x_t(3) = {q};
x_t(4) = {ab};
x_t(5) = {wb};
x_t(6) = {g};


% GPS SENSOR NOISE
GPS_POS_NOISE = 25;
GPS_VEL_NOISE = 10;
GPS_SAMPLE_RATE = 10; % 10 means 0.1s
% IMU SENSOR NOISE
am_noise = 0.1; wm_noise = 0.1; a_bias_noise = 0.01; w_bias_noise = 0.01;


for i = 1:size(accels, 1)
    %% nominal state process model 
    x_t = eskf_nominal_state_predict(x_t, accels(i, :).', gyro(i, :).', deltaT);
    %% error state process model
    [delta_x, P] = eskf_err_state_predict(delta_x, x_t, P, accels(i, :).', gyro(i, :).', deltaT, ...
        am_noise, wm_noise, a_bias_noise, w_bias_noise);
    %% measurement model
    if (mod(i, GPS_SAMPLE_RATE) == 0)
        measurement = [GPS_XYZ(i, :), gpsvelGPSRate(i, :)].';
        [delta_x, P] = eskf_measurement_update(x_t, measurement, P, delta_x, GPS_POS_NOISE, GPS_VEL_NOISE);
        [x_t] = state_correction(delta_x, x_t);
        [delta_x, P] = reset(delta_x, P);
    end

    pos = [pos, x_t{1}];
end

pos = pos.'
figure(1)
plot3(currentPosition(:, 1), currentPosition(:, 2), currentPosition(:, 3), 'r.');
xlabel('X')
ylabel('Y')
zlabel('Z')
grid on
hold on

% plot3(GPS_XYZ(1:10:end, 1), GPS_XYZ(1:10:end, 2), GPS_XYZ(1:10:end, 3), '*');
plot3(pos(:, 1), pos(:, 2), pos(:, 3), 'g-');
pos_predict = pos_predict.';
plot3(pos_predict(:, 1), pos_predict(:, 2), pos_predict(:, 3), 'b');
legend('ground truth', 'estimate','imu-predict')

%% evaluate the filter result
[MeanErr, MeanSquareErr] = evaluate(currentPosition, pos);
[MeanErr, MeanSquareErr] = evaluate(currentPosition, pos_predict);

function [MeanErr, MeanSquareErr] = evaluate(groundTruth, estimateData)
    [row, ~] = size(groundTruth);
    Nek = [];

    for i = 1:1:row
        Nek = [Nek, norm(estimateData(i, 1:3) - groundTruth(i, 1:3))];
    end

    MeanErr = sum(Nek) / row;
    MSE = [];

    for i = 1:length(Nek)
        MSE = [MSE, (Nek(i) - MeanErr)^2];
    end

    figure
    i = 1:row;
    plot(i, Nek);
    hold on
    plot(i, MSE);
    legend('MSE', 'RMSE');
    MeanSquareErr = MSE / row;

end

%%
% function [p, v, q, ab, wb, g] = eskf_nominal_state_predict(p, v, q, ab, wb, g, acc_data, gyro_data, delta_t)

%     R = quat2rotm(q);
%     temp = (R * (acc_data - ab) + g);
%     p = p + v * delta_t + 0.5 * temp * delta_t * delta_t;
%     v = v + temp * delta_t;
%     q = q * quaternion(((gyro_data - wb) * delta_t).', 'rotvec');
%     q = normalize(q);
%     ab = ab;
%     wb = wb;
%     g = g;
% end
% input
% dimension
% acc_data  ---- 3x1
% gyro_data ---- 3x1
% x_t       ---- cell which includes position(3x1), velocity(3x1), orientation(quaternion type), acceleration bias(3x1), gyro bias(3x1) and graviaty constant(3x1)
%% delta_t  ---- IMU sample rate
%% reference formular from 165a-165af 
function [x_t] = eskf_nominal_state_predict(x_t, acc_data, gyro_data, delta_t)

    p = x_t{1};
    v = x_t{2};
    q = x_t{3};
    ab = x_t{4};
    wb = x_t{5};
    g = x_t{6};
    R = quat2rotm(q);
    temp = (R * (acc_data - ab) + g);
    x_t{1} = p + v * delta_t + 0.5 * temp * delta_t * delta_t;
    x_t{2} = v + temp * delta_t;
    q = q * quaternion(((gyro_data - wb) * delta_t).', 'rotvec');
    x_t{3} = normalize(q);
    x_t{4} = ab;
    x_t{5} = wb;
    x_t{6} = g;
end

function [delta_x, P] = eskf_err_state_predict(delta_x, x_t, P, acc_data, gyro_data, delta_t, am_noise, wm_noise, a_bias_noise, w_bias_noise)
    Fx = eye(18, 18);
    Fx(1:3, 4:6) = eye(3) * delta_t;
    skeu_matrix = @(x) [0, -x(3), x(2);
                    x(3), 0, -x(1);
                    -x(2), x(1), 0];
    ab = x_t{4};
    wb = x_t{5};
    q = x_t{3};
    R = quat2rotm(q);
    Fx(4:6, 7:9) = -R * skeu_matrix(acc_data - ab) * delta_t;
    Fx(4:6, 10:12) = -R * delta_t;
    Fx(4:6, 16:18) = eye(3) * delta_t;
    Fx(7:9, 7:9) = (rotationVectorToMatrix((gyro_data - wb) * delta_t)).';
    Fx(7:9, 13:15) = -eye(3) * delta_t;

    Fi = zeros(18, 12);
    Fi(4:6, 1:3) = eye(3);
    Fi(7:9, 4:6) = eye(3);
    Fi(10:12, 7:9) = eye(3);
    Fi(13:15, 10:12) = eye(3);

    Qi = zeros(12, 12);
    Qi(1:3, 1:3) = am_noise^2 * delta_t^2 * eye(3);
    Qi(4:6, 4:6) = wm_noise^2 * delta_t^2 * eye(3);
    Qi(7:9, 7:9) = a_bias_noise^2 * delta_t * eye(3);
    Qi(10:12, 10:12) = w_bias_noise^2 * delta_t * eye(3);

    P = Fx * P * Fx.' + Fi * Qi * Fi.';
    delta_x = Fx * delta_x;
end

function [delta_x, P] = eskf_measurement_update(x_t, measurement, P, delta_x, GPS_POS_NOISE, GPS_VEL_NOISE)
    Hx = zeros(6, 19);
    Hx(1:6, 1:6) = eye(6, 6);
    X_Deltax = zeros(19, 18);
    X_Deltax(1:6, 1:6) = eye(6);
    q = x_t{3}.compact;
    qx = q(2);
    qy = q(3);
    qz = q(4);
    qw = q(1);
    Q_DeltaTheta = 0.5 * [-qx, -qy, -qz;
                    qw, -qz, qy;
                    qz, qw, -qx;
                    -qy, qx, qw];
    X_Deltax(7:10, 7:9) = Q_DeltaTheta;
    X_Deltax(11:19, 10:18) = eye(9);
    GPS_X_ERR = GPS_POS_NOISE;
    GPS_Y_ERR = GPS_POS_NOISE;
    GPS_Z_ERR = GPS_POS_NOISE;
    GPS_VX_ERR = GPS_VEL_NOISE;
    GPS_VY_ERR = GPS_VEL_NOISE;
    GPS_VZ_ERR = GPS_VEL_NOISE;
    V = [GPS_X_ERR^2, 0, 0, 0, 0, 0;
        0, GPS_Y_ERR^2, 0, 0, 0, 0;
        0, 0, GPS_Z_ERR^2, 0, 0, 0;
        0, 0, 0, GPS_VX_ERR^2, 0, 0;
        0, 0, 0, 0, GPS_VY_ERR^2, 0;
        0, 0, 0, 0, 0, GPS_VZ_ERR^2];
    H = Hx * X_Deltax;

    K = P * H.' * inv(H * P * H.' + V);
    delta_x = delta_x + K * (measurement - cell2mat(x_t(1:2)));
    P = (eye(size(P, 1)) - K * H) * P;
end

function [x_t] = state_correction(delta_x, x_t)

    pv = cell2mat(x_t(1:2)) + delta_x(1:6);
    x_t{1} = pv(1:3);
    x_t{2} = pv(4:6);

    awg = cell2mat(x_t(4:6)) + delta_x(10:18);
    x_t{4} = awg(1:3);
    x_t{5} = awg(4:6);
    x_t{6} = awg(7:9);

    q = x_t{3};
    delta_theta = [delta_x(7), delta_x(8), delta_x(9)];
    q = q * quaternion(delta_theta, 'rotvec');
    x_t{3} = normalize(q);
end

function [delta_x, P] = reset(delta_x, P)
    delta_x = 0 .* delta_x;
    G = eye(size(P, 1));
    P = G * P * G.';
end

下面的例程实现语言为c++, 主要是融合Imu+encoder，因为车载传感器只有imu+编码器，所以只实现了imu融合编码器的内容，但是原理上和融合gps是类似的，也可以融合激光里程计

下面是源文件的github地址，把代码放到ros工作空间中编译下就可以了，代码里面用了google的log，如果没有安装依赖的话要安装一下，其次main文件中配置文件的位置要修改下，代码质量一般般大佬们将就看。。。。

GitHub - Youhaochan/eskf