clear all
close all
clc
% Constants that we will need
% Speed of light
c = 299792458;
% Earth's rotation rate
omega_e = 7.2921151467e-5; %(rad/sec)
% load out data
data = load('rtcm_data.mat');
% Data is a cell array containing data about different RTCM messages. We
% are interested in 1002 and 1019
% msgs is a an array of message ids (1002, 1019 etc)
msgs = [data.data{1,:}];
% Get indicies of ephemeris info (msg 1019)
[idx_1019] = find(msgs == 1019);
% Get indicies of raw pseudorange info (msg 1002)
[idx_1002] = find(msgs == 1002);
% Satellite ephemeris data is all mixed up since different satellites are visible at
% different epocks.
eph = [data.data{2, idx_1019}];
% Lets group data for each satellite
% find unique satellite indicies
sv_arr = unique(eph(1,:));
% eph_data will contain ephemeris data for all epochs grouped by satellite
% number
eph_data = {};
for i = 1: length(sv_arr)
% find indicies of all entries corresponding to this satellite
sv = sv_arr(i);
idx = find(eph(1,:) == sv);
eph_data{sv} = eph(:,idx);
end
% Now let's deal with 1002 messages. 1002 messages have two entries - first
% one (nav1) is a 6*1 array containing: reference station id, receiver time
% of week, number of satellites etc. See decode_1002.m in goGPS for details
% The important pieces of info in nav1 are the receiver time of week and the
% number of satellites visible
% The second one (nav2) contains a block of 56*5 for every epoch.
% Num of rows (56) refers to the maximum number of satellites in the
% constellation. Num of cols (5) is the number of data elements for each satellite.
% We are interested in the second element, the raw pseudorange.
% For those satellites for which no info is available,
% the rows of nav2 contain 0s.
nav1 = [data.data{2, idx_1002}];
nav2 = [data.data{3, idx_1002}];
len = length(nav1);
% Arrays to store various outputs of the position estimation algorithm
user_position_arr = [];
user_position_arr_xyz=[];
HDOP_arr = [];
VDOP_arr = [];
user_clock_bias_arr = [];
% initial position of the user
xu = [0 0 0];
% initial clock bias
b = 0;
% 1002 messages are spaced apart 200ms. Let's use 1 out of every 5 samples.
% This means that we'll compute position every second, which is sufficient
for idx = 1: 5: floor(len/20)
% second element of nav1 contains receiver time of week
rcvr_tow = nav1(2,idx);
% data block corresponding to this satellite
nav_data = nav2(:, 5*(idx-1)+1: 5*idx);
% find indicies of rows containing non-zero data. Each row corresponds
% to a satellite
ind = find(sum(nav_data,2) ~= 0);
numSV = length(ind);
eph_formatted_ = [];
% The minimum number of satellites needed is 4, let's go for more than
% that to be more robust
if (numSV > 4)
pr_ = [];
% Correct for satellite clock bias and find the best ephemeris data
% for each satellite. Note that satellite ephemeris data (1019) is sent
% far less frequently than pseudorange info (1002). So for every
% epoch, we find the closest (in time) ephemeris data.
for i = 1: numSV,
sv_idx = ind(i);
sv_data = nav_data(sv_idx,:);
% find ephemeris data closest to this time of week
[c_ eph_idx] = min(abs(eph_data{sv_idx}(18,:)-rcvr_tow));
eph_ = eph_data{sv_idx}(:, eph_idx);
% Convert the ephemeris data into a standard format so it can
% be input to routines that process it to calculate satellite
% position and satellite clock bias
eph_formatted = format_ephemeris3(eph_);
eph_formatted_{end+1} = eph_formatted;
% To be correct, the satellite clock bias should be calculated
% at rcvr_tow - tau, however it doesn't make much difference to
% do it at rcvr_tow
dsv = estimate_satellite_clock_bias(rcvr_tow, eph_formatted);
% measured pseudoranges corrected for satellite clock bias.
% Also apply ionospheric and tropospheric corrections if
% available
pr_raw = sv_data(2);
pr_(end+1) = pr_raw + c*dsv;
end
% Now lets calculate the satellite positions and construct the G
% matrix. Then we'll run the least squares optimization to
% calculate corrected user position and clock bias. We'll iterate
% until change in user position and clock bias is less than a
% threhold. In practice, the optimization converges very quickly,
% usually in 2-3 iterations even when the starting point for the
% user position and clock bias is far away from the true values.
dx = 100*ones(1,3); db = 100;
% GPS Spoofing Attack on second satellite (HJ)
attack=1e07*[-1.2078 2.9080 0.8252];
while(norm(dx) > 0.1 && norm(db) > 1)
Xs = []; % concatenated satellite positions
pr = []; % pseudoranges corrected for user clock bias
for i = 1: numSV,
% correct for our estimate of user clock bias. Note that
% the clock bias is in units of distance
cpr = pr_(i) - b;
pr = [pr; cpr];
% Signal transmission time
tau = cpr/c;
% Get satellite position
[xs_ ys_ zs_] = get_satellite_position(eph_formatted_{i}, rcvr_tow-tau, 1);
% express satellite position in ECEF frame at time t
theta = omega_e*tau;
xs_vec = [cos(theta) sin(theta) 0; -sin(theta) cos(theta) 0; 0 0 1]*[xs_; ys_; zs_];
xs_vec = [xs_ ys_ zs_]';
Xs = [Xs; xs_vec'];
end
% Position of Satellite 2 is changed by attack
Xs(2,:)=Xs(2,:)+attack;
% Pseudorange is changed accordingly (this pseudorange is calculated
% assuming we already know the user position and user clock
% bias when GPS operates normally)
load('User_position_clock_bias.mat');
idx_adj=fix(idx/5)+rem(idx,5);
original_user_position=[user_position_arr_xyz_load(idx_adj,1) user_position_arr_xyz_load(idx_adj,2) user_position_arr_xyz_load(idx_adj,3)];
original_user_clock_bias=user_clock_bias_arr_load(idx_adj);
pr(2)=norm(original_user_position-Xs(2,:))+original_user_clock_bias;
% Run least squares to calculate new user position and bias
[x_, b_, norm_dp, G] = estimate_position(Xs, pr, numSV, xu, b, 3);
% Change in the position and bias to determine when to quite
% the iteration
dx = x_ - xu;
db = b_ - b;
xu = x_;
b = b_;
end % end of iteration
% original_x=xu(1)
% original_y=xu(2)
% original_z=xu(3)
% after_x=xu(1)
% after_y=xu(2)
% after_z=xu(3)
% Convert from ECEF to lat/lng
[lambda, phi, h] = WGStoEllipsoid(xu(1), xu(2), xu(3));
% Calculate Rotation Matrix to Convert ECEF to local ENU reference
% frame
lat = phi*180/pi
lon = lambda*180/pi
R1=rot(90+lon, 3);
R2=rot(90-lat, 1);
R=R2*R1;
G_ = [G(:,1:3)*R' G(:,4)];
H = inv(G_'*G_);
HDOP = sqrt(H(1,1) + H(2,2));
VDOP = sqrt(H(3,3));
% Record various quantities for saving and plotting
HDOP_arr(end+1,:) = HDOP;
VDOP_arr(end+1,:) = VDOP;
user_position_arr(end+1,:) = [lat lon h];
user_position_arr_xyz(end+1,:)=[xu(1) xu(2) xu(3)];
user_clock_bias_arr(end+1,:) = b;
end
end
HDOP_arr;
%Function R=rot(angle (degrees), axis) ret