1. 在展示公式和代码之前,先理清一下旋转矩阵,四元素,轴角, 旋转向量之间的关系
假设一个旋转向量为:
将旋转向量表示成轴角形式
所以
轴角转换到旋转矩阵形式
轴角转换成四元数形式
反对称矩阵
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
是一个3x3的单位矩阵
就是一个旋转矩阵乘上一个反对称矩阵
就是将花括号里面的三维变量(旋转向量)转换为旋转矩阵
矩阵对角线上的内容如下
代码如下
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。
先贴一下测量更新的部分公式
在本例子上
H矩阵求解如以下公式
where
具体测量更新的代码如下
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
第五步修正完状态之后, 要把误差项和协方差进行修正, 修正公式如下图
为了简化,矩阵可以设置成单位矩阵
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文件中配置文件的位置要修改下, 代码质量一般般大佬们将就看。。。。