I am trying to integrate IMU and GPS data to find roll of ground vehicle on the same road at different velocities. But, I am getting inconsistent results and an error at higher velocities (45mph) compared to lower velocities(25mph).

10 次查看(过去 30 天)
function [nav_e] = ins_gnss(imu, gnss, att_mode)
% ins_gnss: loosely-coupled integrated navigation system.
%
% ins_gnss integrates IMU and GNSS measurements by using an Extended Kalman filter.
%
% INPUT
% imu: IMU data structure.
% t: Ix1 time vector (seconds).
% fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
% wb: Ix3 turn rates vector in body frame XYZ (radians/s).
% arw: 1x3 angle random walks (rad/s/root-Hz).
% vrw: 1x3 angle velocity walks (m/s^2/root-Hz).
% gstd: 1x3 gyros standard deviations (radians/s).
% astd: 1x3 accrs standard deviations (m/s^2).
% gb_sta: 1x3 gyros static biases or turn-on biases (radians/s).
% ab_sta: 1x3 accrs static biases or turn-on biases (m/s^2).
% gb_dyn: 1x3 gyros dynamic biases or bias instabilities (radians/s).
% ab_dyn: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
% gb_corr: 1x3 gyros correlation times (seconds).
% ab_corr: 1x3 accrs correlation times (seconds).
% gb_psd: 1x3 gyros dynamic biases PSD (rad/s/root-Hz).
% ab_psd: 1x3 accrs dynamic biases PSD (m/s^2/root-Hz);
% freq: 1x1 sampling frequency (Hz).
% ini_align: 1x3 initial attitude at t(1).
% ini_align_err: 1x3 initial attitude errors at t(1).
%
% gnss: GNSS data structure.
% t: Mx1 time vector (seconds).
% lat: Mx1 latitude (radians).
% lon: Mx1 longitude (radians).
% h: Mx1 altitude (m).
% vel: Mx3 NED velocities (m/s).
% std: 1x3 position standard deviations (rad, rad, m).
% stdm: 1x3 position standard deviations (m, m, m).
% stdv: 1x3 velocity standard deviations (m/s).
% larm: 3x1 lever arm from IMU to GNSS antenna (x-fwd, y-right, z-down) (m).
% freq: 1x1 sampling frequency (Hz).
% zupt_th: 1x1 ZUPT threshold (m/s).
% zupt_win: 1x1 ZUPT time window (seconds).
% eps: 1x1 time interval to compare current IMU time to current GNSS time vector (s).
%
% att_mode: attitude mode string.
% 'quaternion': attitude updated in quaternion format. Default value.
% 'dcm': attitude updated in Direct Cosine Matrix format.
%
% OUTPUT
% nav_e: INS/GNSS navigation estimates data structure.
% t: Ix1 INS time vector (seconds).
% tg: Mx1 GNSS time vector, when Kalman filter was executed (seconds).
% roll: Ix1 roll (radians).
% pitch: Ix1 pitch (radians).
% yaw: Ix1 yaw (radians).
% vel: Ix3 NED velocities (m/s).
% lat: Ix1 latitude (radians).
% lon: Ix1 longitude (radians).
% h: Ix1 altitude (m).
% xi: Mx15 Kalman filter a priori states.
% xp: Mx15 Kalman filter a posteriori states.
% z: Mx6 INS/GNSS measurements
% v: Mx6 Kalman filter innovations.
% b: Mx6 Kalman filter biases compensations, [gb_dyn ab_dyn].
% A: Mx225 Kalman filter transition-state matrices, one matrix per
% row ordered by columns.
% Pp: Mx225 Kalman filter a posteriori covariance matrices, one
% matrix per row ordered by columns.
% Pi: Mx225 Kalman filter a priori covariance matrices, one matrix
% per row ordered by columns.
% ins_gps.m, ins_gnss function is based on that previous function.
%
if nargin < 3, att_mode = 'quaternion'; end
%% ZUPT detection algorithm
zupt = false;
%% PREALLOCATION
% Constant matrices
I = eye(3);
O = zeros(3);
% Length of INS time vector
LI = length(imu.t);
% Length of GNSS time vector
LG = length(gnss.t);
% Attitude
roll_e = zeros (LI, 1);
pitch_e = zeros (LI, 1);
yaw_e = zeros (LI, 1);
% yawm_e = zeros (Mi, 1);
% Initialize estimates at INS time = 1
roll_e(1) = imu.ini_align(1);
pitch_e(1) = imu.ini_align(2);
yaw_e(1) = imu.ini_align(3);
% yawm_e(1) = imu.ini_align(3);
DCMnb = euler2dcm([roll_e(1); pitch_e(1); yaw_e(1);]);
DCMbn = DCMnb';
qua = euler2qua([roll_e(1) pitch_e(1) yaw_e(1)]);
% Velocities
vel_e = zeros (LI, 3);
% Initialize estimates at INS time = 1
vel_e(1,:) = gnss.vel(1,:);
% Positions
lat_e = zeros (LI,1);
lon_e = zeros (LI,1);
h_e = zeros (LI, 1);
% Initialize estimates at INS time = 1
h_e(1) = gnss.h(1);
lat_e(1) = gnss.lat(1);
lon_e(1) = gnss.lon(1);
% Biases
gb_dyn = imu.gb_dyn';
ab_dyn = imu.ab_dyn';
% Initialize Kalman filter matrices
% Prior estimates
kf.xi = [ zeros(1,9), imu.gb_dyn, imu.ab_dyn ]'; % Error vector state
kf.Pi = diag([imu.ini_align_err, gnss.stdv, gnss.std, imu.gb_dyn, imu.ab_dyn].^2);
kf.R = diag([gnss.stdv, gnss.stdm].^2);
kf.Q = diag([imu.arw, imu.vrw, imu.gb_psd, imu.ab_psd].^2);
fb_corrected = (imu.fb(1,:)' + ab_dyn );
fn = (DCMbn * fb_corrected);
% Vector to update matrix F
upd = [gnss.vel(1,:) gnss.lat(1) gnss.h(1) fn'];
% Update matrices F and G
[kf.F, kf.G] = F_update(upd, DCMbn, imu);
[RM,RN] = radius(gnss.lat(1));
Tpr = diag([(RM + gnss.h(1)), (RN + gnss.h(1)) * cos(gnss.lat(1)), -1]); % radians-to-meters
% Update matrix H
kf.H = [ O I O O O ;
O O Tpr O O ; ];
kf.R = diag([gnss.stdv gnss.stdm]).^2;
kf.z = [ gnss.stdv, gnss.stdm ]';
% Propagate prior estimates to get xp(1) and Pp(1)
kf = kf_update( kf );
% Kalman filter matrices for later performance analysis
xi = zeros(LG, 15); % Evolution of Kalman filter a priori states, xi
xp = zeros(LG, 15); % Evolution of Kalman filter a posteriori states, xp
z = zeros(LG, 6); % INS/GNSS measurements
v = zeros(LG, 6); % Kalman filter innovations
b = zeros(LG, 6); % Biases compensantions after Kalman filter correction
A = zeros(LG, 225); % Transition-state matrices, A
Pi = zeros(LG, 225); % Priori covariance matrices, Pi
Pp = zeros(LG, 225); % Posteriori covariance matrices, Pp
K = zeros(LG, 90); % Kalman gain matrices, K
S = zeros(LG, 36); % Innovation matrices, S
% Initialize matrices for Kalman filter performance analysis
xp(1,:) = kf.xp';
Pp(1,:) = reshape(kf.Pp, 1, 225);
b(1,:) = [imu.gb_sta, imu.ab_sta];
% INS (IMU) time is the master clock
for i = 2:LI
%% INERTIAL NAVIGATION SYSTEM (INS)
% % Print a dot on console every 10,000 INS executions
% if (mod(i,10000) == 0), fprintf('. '); end
% % Print a return on console every 200,000 INS executions
% if (mod(i,200000) == 0), fprintf('\n'); end
% IMU sampling interval
dti = imu.t(i) - imu.t(i-1);
% Inertial sensors corrected with KF biases estimation
wb_corrected = (imu.wb(i,:)' + gb_dyn );
fb_corrected = (imu.fb(i,:)' + ab_dyn );
% Turn-rates update
omega_ie_n = earthrate(lat_e(i-1));
omega_en_n = transportrate(lat_e(i-1), vel_e(i-1,1), vel_e(i-1,2), h_e(i-1));
% Attitude update
[qua_n, DCMbn, euler] = att_update(wb_corrected, DCMbn, qua, ...
omega_ie_n, omega_en_n, dti, att_mode);
roll_e(i) = euler(1);
pitch_e(i)= euler(2);
yaw_e(i) = euler(3);
qua = qua_n;
% Gravity update
gn = gravity(lat_e(i-1), h_e(i-1));
% Velocity update
fn = (DCMbn * fb_corrected);
vel_n = vel_update(fn, vel_e(i-1,:), omega_ie_n, omega_en_n, gn', dti);
vel_e (i,:) = vel_n;
% Position update
pos = pos_update([lat_e(i-1) lon_e(i-1) h_e(i-1)], vel_e(i,:), dti);
lat_e(i) = pos(1);
lon_e(i) = pos(2);
h_e(i) = pos(3);
% ZUPT detection algorithm
idz = floor( gnss.zupt_win / dti ); % Index to set ZUPT window time
if ( i > idz )
vel_m = mean (vel_e(i-idz:i , :));
if (abs(vel_m) <= gnss.zupt_th)
roll_e(i) = mean (roll_e(i-idz:i , :));
pitch_e(i)= mean (pitch_e(i-idz:i , :));
yaw_e(i) = mean (yaw_e(i-idz:i , :));
lat_e(i) = mean (lat_e(i-idz:i , :));
lon_e(i) = mean (lon_e(i-idz:i , :));
h_e(i) = mean (h_e(i-idz:i , :));
zupt = true;
end
end
%% KALMAN FILTER UPDATE
% Check if there is new GNSS data to process at current INS time
gdx = find (gnss.t >= (imu.t(i) - gnss.eps) & gnss.t < (imu.t(i) + gnss.eps));
if ( ~isempty(gdx) && gdx > 1)
%% INNOVATIONS
[RM,RN] = radius(lat_e(i));
Tpr = diag([(RM + h_e(i)), (RN + h_e(i)) * cos(lat_e(i)), -1]); % radians-to-meters
% Innovations for position with lever arm correction
zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [gnss.lat(gdx); gnss.lon(gdx); gnss.h(gdx);]) ...
+ (DCMbn * gnss.larm);
% Innovations for velocity with lever arm correction
zv = (vel_e(i,:) - gnss.vel(gdx,:) - ((omega_ie_n + omega_en_n) .* (DCMbn * gnss.larm))' ...
+ (DCMbn * skewm(wb_corrected) * gnss.larm )' )';
%% KALMAN FILTER
% GNSS sampling interval
dtg = gnss.t(gdx) - gnss.t(gdx-1);
% Vector to update matrix F
upd = [vel_e(i,:) lat_e(i) h_e(i) fn'];
% Update matrices F and G
[kf.F, kf.G] = F_update(upd, DCMbn, imu);
% Update matrix H
if(zupt == false)
kf.H = [ O I O O O ;
O O Tpr O O ; ];
kf.R = diag([gnss.stdv gnss.stdm]).^2;
kf.z = [ zv' zp' ]';
else
kf.H = [ O I O O O ; ];
kf.R = diag([gnss.stdv]).^2;
kf.z = zv;
end
% Execute the extended Kalman filter
kf.xp(1:9) = 0; % states 1:9 are forced to be zero (error-state approach)
kf = kalman(kf, dtg);
%% INS/GNSS CORRECTIONS
% Quaternion corrections
% Crassidis. Eq. 7.34 and A.174a.
antm = [0 qua_n(3) -qua_n(2); -qua_n(3) 0 qua_n(1); qua_n(2) -qua_n(1) 0];
qua = qua_n + 0.5 .* [qua_n(4)*eye(3) + antm; -1.*[qua_n(1) qua_n(2) qua_n(3)]] * kf.xp(1:3);
qua = qua / norm(qua); % Brute-force normalization
% DCM correction
DCMbn = qua2dcm(qua);
% % Another possible attitude correction
% euler = qua2euler(qua);
% roll_e(i) = euler(1);
% pitch_e(i)= euler(2);
% yaw_e(i) = euler(3);
%
% E = skewm(kf.xp(1:3));
% DCMbn = (eye(3) + E) * DCMbn_n;
% Attitude corrections
roll_e(i) = roll_e(i) - kf.xp(1);
pitch_e(i) = pitch_e(i) - kf.xp(2);
yaw_e(i) = yaw_e(i) - kf.xp(3);
% Velocity corrections
vel_e (i,1) = vel_e (i,1) - kf.xp(4);
vel_e (i,2) = vel_e (i,2) - kf.xp(5);
vel_e (i,3) = vel_e (i,3) - kf.xp(6);
% Position corrections
lat_e(i) = lat_e(i) - (kf.xp(7));
lon_e(i) = lon_e(i) - (kf.xp(8));
h_e(i) = h_e(i) - kf.xp(9);
% Biases corrections
gb_dyn = kf.xp(10:12);
ab_dyn = kf.xp(13:15);
% Matrices for later Kalman filter performance analysis
xi(gdx,:) = kf.xi';
xp(gdx,:) = kf.xp';
b(gdx,:) = [gb_dyn', ab_dyn'];
A(gdx,:) = reshape(kf.A, 1, 225);
Pi(gdx,:) = reshape(kf.Pi, 1, 225);
Pp(gdx,:) = reshape(kf.Pp, 1, 225);
if(zupt == false)
v(gdx,:) = kf.v';
K(gdx,:) = reshape(kf.K, 1, 90);
S(gdx,:) = reshape(kf.S, 1, 36);
else
zupt = false;
v(gdx,:) = [ kf.v' 0 0 0 ]';
K(gdx,1:45) = reshape(kf.K, 1, 45);
S(gdx,1:9) = reshape(kf.S, 1, 9);
end
end
end
%% Summary from INS/GNSS integration
nav_e.t = imu.t(1:i, :); % INS time vector
nav_e.tg = gnss.t; % GNSS time vector, which is the time vector when the Kalman filter was executed
nav_e.roll = roll_e(1:i, :); % Roll
nav_e.pitch = pitch_e(1:i, :); % Pitch
nav_e.yaw = yaw_e(1:i, :); % Yaw
nav_e.vel = vel_e(1:i, :); % NED velocities
nav_e.lat = lat_e(1:i, :); % Latitude
nav_e.lon = lon_e(1:i, :); % Longitude
nav_e.h = h_e(1:i, :); % Altitude
nav_e.xi = xi; % A priori states
nav_e.xp = xp; % A posteriori states
nav_e.m = z; % IMU/GPS measurements
nav_e.v = v; % Kalman filter innovations
nav_e.b = b; % Biases compensations
nav_e.A = A; % Transition matrices
nav_e.Pi = Pi; % A priori covariance matrices
nav_e.Pp = Pp; % A posteriori covariance matrices
nav_e.K = K; % Kalman gain matrices
nav_e.S = S; % Innovation matrices
fprintf('\n');
end
function kf = kalman(kf, dt)
% kalman: Kalman filter algorithm.
%
% INPUT
% kf: data structure with at least the following fields:
% xp: 21x1 a posteriori state vector (old).
% z: 6x1 innovations vector.
% F: 21x21 state transition matrix.
% H: 6x21 observation matrix.
% Q: 12x12 process noise covariance.
% R: 6x6 observation noise covariance.
% Pp: 21x21 a posteriori error covariance.
% G: 21x12 control-input matrix.
% dt: sampling interval.
%
% OUTPUT
% kf: the following fields are updated:
% xi: 21x1 a priori state vector (updated).
% xp: 21x1 a posteriori state vector (updated).
% v: 6x1 innovation vector.
% A: 21x21 state transition matrix.
% K: 21x6 Kalman gain matrix.
% Qd: 21x6 discrete process noise covariance.
% Pi: 21x21 a priori error covariance.
% Pp: 21x21 a posteriori error covariance.
% S: 6x6 innovation (or residual) covariance.
%
% PREDICTION STEP
kf = kf_prediction(kf, dt);
% UPDATE STEP
kf = kf_update(kf);
end
function kf = kf_update( kf )
% kalman: Measurement update part of the Kalman filter algorithm.
%
% INPUT
% kf: data structure with at least the following fields:
% xi: 15x1 a priori state vector.
% Pi: 15x15 a priori error covariance.
% z: 6x1 innovations vector.
% H: 6x15 observation matrix.
% R: 6x6 observation noise covariance.
%
% OUTPUT
% kf: the following fields are updated:
% xp: 15x15 a posteriori state vector (updated).
% Pp: 15x15 a posteriori error covariance (updated).
% v: 6x1 innovation vector.
% K: 15x6 Kalman gain matrix.
% S: 6x6 innovation (or residual) covariance.
% I = eye(size(kf.F));
% Find the Kalman gain by finding v and S i.e Innovations
kf.S = (kf.R + kf.H * kf.Pi * kf.H'); % Innovations (or pre-fit residual) covariance matrix
kf.v = kf.z - kf.H * kf.xi; % Innovations or measurement pre-fit vector
kf.K = (kf.Pi * kf.H') * (kf.S)^(-1) ; % Kalman gain matrix
% Update the a posteriori state estimate(xp)
kf.xp = kf.xi + kf.K * kf.v;
% Update the a posteriori covariance estimate matrix(Pp)
kf.Pp = kf.Pi - kf.K * kf.S * kf.K';
kf.Pp = 0.5 .* (kf.Pp + kf.Pp'); % Force Pi to be symmetric matrix
end
  1 个评论
Vishal Rao Venkatesh
Any suggestion would be helpful. Also, I am getting this warning
Warning: Matrix is singular, close to singular or badly scaled. Results may be inaccurate. RCOND = NaN.
> In mpower>integerMpower (line 80)
In ^ (line 49)
In kf_update (line 27)
In kalman (line 34)
In ins_gnss (line 284)
Thank You!

请先登录,再进行评论。

回答(1 个)

John D'Errico
John D'Errico 2019-11-13
编辑:John D'Errico 2019-11-13
The singular matrix suggests why it is you are getting garbage, as once you have that, nothing will likely make any sense. Is that the cause of the problem? That is one way to interpret it. But ... probably not really so. That is likely more of a symptom, of an error you have made somewhere.
For exmple, I read a mystery novel recently, where the statement was made that someone died of heart failure, and that heart failure is a very common cause of death. Of course, the immediate cause of this specific heart failure was a bullet, passing through the heart that caused the event. You could even argue that the real cause was the person who pulled the trigger of the loaded gun.
Similarly, the singularity you have observed may have been only part of the story.
Anyway, answering a question like this is almost impossible, unless I would be willing to read through this massive code line by line, running the code on the specific problem you tested it on, stepping through it with the debugger. (Of course, it is you who needs to do exactly that, since you are the person who apparently wrote the code.) The point is, I would need to check every single line written there. Was it done correctly? Is it doing what it should be designed to do? At some point, you would see a problem. But for me (or virtually anyone) to do that would be the work of probably many hours of work.
In the end, I'd argue the cause of the problem was the person who wrote the code. This code was written in very much the wrong way. When you write a large code like this, and then run it, hoping it will work, you are making a grave mistake. It will never work perfectly. And then you need to debug it carefully, from beginning to end (as you will need to do.)
Instead, you need to write code in SMALL pieces. Test every piece. Verify that ti does EXACTLY what it should be doing. Write small, modular code. Test every module separately. Ensure that every module is corrrect as you write it. Only at the very end should you then put it all together.
Or, you can do as you have done here. Write it as a huge mess of code, together with a fervent prayer that it will work, and that no silly mistake was made along the way. Sadly, the gods of MATLAB do not seem to respond well to prayer.
  1 个评论
Vishal Rao Venkatesh
John,
Thank You for your suggestion, I have been thinking on similar lines i.e. issue with singular matrix could be why this is messing things up. I am going to work on it.
Also, just so you know, I have been using Matlab for the past 5 years and every time I code on Matlab, I make sure I do not mess it up and hence write it in SMALL PIECES. The code I have pasted up there is 4 separate modules with perfect comments for clear understanding. Also, I already have an idea on where things could be messed up, this question is to know if people on this forum think the same. The same code worked perfectly for other data and sadly without using inbuilt matlab functions coding an entire INS system is quite complex. So, kindly be a little more respectful maybe? People post questions on this forum not expecting you to solve their problem but expecting you to give a hint based on your experience.
Thank You again!

请先登录,再进行评论。

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by