How can I generate trajectory and INS data from the GPS data I have?

I want to generate the trajectory using the GPS data I have in LLA (deg deg m).

Then I want to generate the IMU data from that trajectory.

My end goal is to apply insmargfilter to pose the Estimation and Orientation for a UAV in MATLAB.

[Cris LaPierre]

There is a trajectory function.

[Aashir]

I am not able to do it. Can you please give an example how to do it?

[Elad Kivelevitch]

I am not sure that this will help much for a UAV object. ScenarioBuilder is part of driving, not UAV.

I would use geoTrajectory.

[Aashir]

I have the gps data, i want to get the corresponding IMU data for the trajectory. I need to have ax ay az, gx gy gz, mx my mz, position, gps velocity, quaternion so that i can estimate the pose and orientation of the uav. Please guide thanks.

[Aashir]

How to use the geo trajectory? What I am trying to do is doable?

If you can please just write down the functions which I need to use to achieve the said purpose, I shall be grateful.

[Cris LaPierre]

I find the documentation is a great place to start. You'll find syntax descriptions, examples, and links to related content:

https://www.mathworks.com/help/fusion/ref/geotrajectory-system-object.html

For a specific answer, consider sharing your data. You can attach it to your post using the paperclip icon.

[Aashir]

Here is the data, please guide.

[Cris LaPierre]

Please share the code you have written as well. That will give us a helpful starting point.

[Aashir]

I do not have any code.

If you can tell me which functions to use, I can try it. My end goal is to run the example pose estimate and orientation of uav.

If that is not possible can you please tell me how the data is generated in this example? How did you get trajData.mat?

I just want to know that. Thanks

[Cris LaPierre]

Which example are you referring to? Could you share a link?

[Aashir]

I have the following data:

Ax, Ay, Az (m/s^2) - update rate 10 Hz

Gx, Gy, Gz (rad/s) - update rate 10 Hz

GPS LLA (deg deg meters) - update rate 2 Hz

Roll, Pitch, Yaw (degrees) update rate 10 Hz

I want to develop a GNSS aided INS.

I am following the example given below:

https://www.mathworks.com/help/nav/ug/imu-and-gps-fusion-for-inertial-navigation.html

initstate = [1-4 % Orientation as a quaternion

5-7 % Position (NED) %but my frame is in NEU maybe

8-10 % Velocity (NED)

11-13 % Delta Angle Bias (XYZ)

14-16 % Delta Velocity Bias (XYZ)

17-19; % Geomagnetic Field Vector (NED)

20-22] % Magnetometer Bias (XYZ)

How can I proceed with this?

How can I run this example with my data?

If that is not possible, is there any way to create a similar dummy data as in the example trajData?

[Aashir]

Example says:

UAV Trajectory This example uses a saved trajectory recorded from a UAV as the ground truth. This trajectory is fed to several sensor simulators to compute simulated accelerometer, gyroscope, magnetometer, and GPS data streams.

How do I compute simulated accelerometer, gyroscope, magnetometer, and GPS data using my trajectory?

Answer 1

[Cris LaPierre]

Looking at your raw data, it looks like you should probably use waypointTrajectory with the following syntax:

trajectory = waypointTrajectory(Waypoints,Velocities=velocities)

Your excel file has 37802 rows of data, but only 7355 have Lat/Lon/Height data, so I assumed those with missing data could be ignored.

log = readtable('Data.xlsx','MissingRule','omitrow')

Warning: Column headers from the file were modified to make them valid MATLAB identifiers before creating variable names for the table. The original column headers are saved in the VariableDescriptions property.
Set 'VariableNamingRule' to 'preserve' to use the original column headers as table variable names.

log = 7355x12 table
    GPSLongitude    GPSLatitude    GPSHeight    dAvgAX     dAvgAY     dAvgAZ       dAvgGX         dAvgGY         dAvgGZ       fNavSpeedE_100    fNavSpeedN_100    fNavSpeedU_100
    ____________    ___________    _________    ______    ________    _______    ___________    ___________    ___________    ______________    ______________    ______________

       72.039         31.403        167.19      2.1775    -0.18661    -9.5648    -0.00048591    -0.00051175     0.00045591          -1                1                 0       
       72.039         31.403        167.19      2.1748    -0.18644    -9.5581    -0.00020717     -0.0006527    -0.00026477          -1                1                 0       
       72.039         31.403        167.18      2.1788    -0.18702    -9.5698     0.00036346    -0.00027404     0.00048793          -1                2                 0       
       72.039         31.403        167.18      2.1768    -0.19553    -9.5645    -0.00071975    -0.00083373    -0.00032118          -1                2                 0       
       72.039         31.403        167.18      2.1767    -0.18875    -9.5641     0.00021002    -0.00097967     2.7855e-06          -1                2                 0       
       72.039         31.403        167.18      2.1772    -0.18601    -9.5672     0.00010558    -0.00054131     0.00032214          -1                2                 0       
       72.039         31.403        167.18      2.1857    -0.19286    -9.5654    -0.00044786    -0.00059937     0.00045713          -1                2                 0       
       72.039         31.403        167.18      2.1872    -0.18315    -9.5645     7.4857e-06    -0.00052392     0.00063988          -1                3                 0       
       72.039         31.403        167.19      2.1803    -0.19007    -9.5659    -0.00025747     -0.0006786      0.0011932          -1                3                 0       
       72.039         31.403        167.19      2.1837    -0.17955    -9.5672    -0.00022593    -0.00098522    -0.00043526          -2                3                 0       
       72.039         31.403         167.2      2.1813     -0.1882    -9.5678     -0.0010648    -0.00062555     0.00027332          -1                2                 0       
       72.039         31.403         167.2      2.1705    -0.18567    -9.5626    -0.00063489     1.1322e-05    -0.00028094          -1                2                 0       
       72.039         31.403        167.21      2.1858    -0.19167    -9.5674    -0.00054358    -0.00082081    -4.8649e-05          -1                2                 0       
       72.039         31.403        167.21       2.178    -0.18685    -9.5624    -5.3561e-05    -0.00014373    -0.00029054          -1                2                 0       
       72.039         31.403         167.2      2.1758    -0.19076    -9.5611    -0.00082912    -0.00084195      0.0003127          -1                3                 0       
       72.039         31.403         167.2      2.1747    -0.19682     -9.559     -3.898e-05     -0.0011071     0.00025947          -2                3                 0       

Waypoints = log{:,["GPSLatitude","GPSLongitude","GPSHeight"]};
vel = log{:,["fNavSpeedE_100","fNavSpeedE_100","fNavSpeedE_100"]};
uavTraj = waypointTrajectory(Waypoints,Velocities=vel)

uavTraj = 
  waypointTrajectory with properties:

         SampleRate: 100
    SamplesPerFrame: 1
          Waypoints: [7355x3 double]
      TimeOfArrival: [7355x1 double]
         Velocities: [7355x3 double]
             Course: [7355x1 double]
        GroundSpeed: [7355x1 double]
          ClimbRate: [7355x1 double]
          JerkLimit: Inf
        InitialTime: 0
           WaitTime: [7355x1 double]
        Orientation: [7355x1 quaternion]
          AutoPitch: 0
           AutoBank: 0
     ReferenceFrame: 'NED'

I found this answer helpful for the next part: Generation of artificial IMU data from simple waypoint trajectory

count = 1;
while ~isDone(uavTraj)
   [position(count,:),orientation(count,:),velocity(count,:),acceleration(count,:),angularVelocity(count,:)] = uavTraj();
   count = count + 1;
end

accelParams = accelparams;
gyroParams = gyroparams;

% Generate IMU readings
imu = imuSensor('accel-gyro', 'Accelerometer', accelParams, 'Gyroscope', gyroParams)

imu = 
  imuSensor with properties:

          IMUType: 'accel-gyro'
       SampleRate: 100
      Temperature: 25
    Accelerometer: [1x1 accelparams]
        Gyroscope: [1x1 gyroparams]
     RandomStream: 'Global stream'

[accelReadings, gyroReadings] = imu(acceleration, angularVelocity, orientation);

The final list of functions, then, would be

• waypointTrajectory
• accelparams
• gyroparams
• imuSensor

As for visualizing the data, I found the Visualize UAV Flight Path on 2-D and 3-D Maps example helpful for that as well as the geoplot3 function documentation page.

There is another way to simulat IMU sensor data that is shown in the Simulate IMU Sensor Mounted on UAV example, but I did not look into that.

[Aashir]

Your excel file has 37802 rows of data, but only 7355 have Lat/Lon/Height data, so I assumed those with missing data could be ignored.

These are not missing because the update rate of IMU and GPS is different. That’s why.

[Cris LaPierre]

Then there is still some work for you to do. waypointTrajectory accepts either time of arrival, groundspeed, or velocity as a second input. Whichever syntax you choose, it expects there to be the same number of rows as waypoints.

You could try geoTrajectory, but that only accepts time of arrival, and has the some requirement.

[Aashir]

Well for this case, can I give the time?

I also have time in my data, which would be equivalent to time of arrival.

[Cris LaPierre]

You can certainly try it out.

[Aashir]

Hi,

When I run this code as you gave:

clc;
clear all;
close all;

%%
log = readtable('GPS.xlsx','MissingRule','omitrow');

%%
Waypoints = log{:,["GPSLatitude","GPSLongitude","GPSHeight"]};
vel = log{:,["fNavSpeedE","fNavSpeedN","fNavSpeedU"]};
uavTraj = waypointTrajectory(Waypoints,Velocities=vel,ReferenceFrame="ENU")

%%
count = 1;
while ~isDone(uavTraj)
   [position(count,:),orientation(count,:),velocity(count,:),acceleration(count,:),angularVelocity(count,:)] = uavTraj();
   count = count + 1;
end

The output of each is NaN.

What can I do about this?

[Cris LaPierre]

Run the code using the same data you gave us.

[Aashir]

I am using the same file, data is same but Ax Ay Az, Gx Gy Gz was not included in the file and that you are not using in the code. Why is it NaN then?

[Aashir]

Also, why is it 285x3?

Shouldnt it be 7355?

[Cris LaPierre]

It is very hard to provide an answer based solely on a screenshot.

You can run the code you put in a post, as you can see in my post above. Try to duplicate the issue you are seeing here by attaching GPS.xlsx using the paperclip icon, paste your code into a code block, and then click the Run icon in the toolbar. Once you can get the issue to reproduce here, click submit.

[Aashir]

I want to know these five things can be calculated for my GPS LLA.

orient, acc, vel, pos, angvel

[Cris LaPierre]

You mean these 5 things?

Yes, once you have your trajectories, you can obtain these 5 items using the syntax

[position,orientation,velocity,acceleration,angularVelocity] = trajectory()

You can see some examples using this here and here

[Aashir]

This doesnt gives good results.

EDIT: running your files here

untitled5

Warning: Column headers from the file were modified to make them valid MATLAB identifiers before creating variable names for the table. The original column headers are saved in the VariableDescriptions property.
Set 'VariableNamingRule' to 'preserve' to use the original column headers as table variable names.

untitled6

End-to-End Simulation Position RMS Error
	X: 1.46 , Y: 1.25, Z: 6.82   (meters)

End-to-End Quaternion Distance RMS Error (degrees) 
	116.84 (degrees)

[Aashir]

You can try running the untitled5.m to generate [position,orientation,velocity,acceleration,angularVelocity]

Then run untitled6.m to see the localization results.

[Cris LaPierre]

Now that you have time of arrival, try geoTrajectory instead of waypointTrajectory. Based on the units of your waypoints, it is likely the better choice (latitude in degrees, longitude in degrees, and altitude above the WGS84 reference ellipsoid in meters of the geodetic waypoint).

log = readtable('Data_Updated3.xlsx');

Warning: Column headers from the file were modified to make them valid MATLAB identifiers before creating variable names for the table. The original column headers are saved in the VariableDescriptions property.
Set 'VariableNamingRule' to 'preserve' to use the original column headers as table variable names.

Waypoints = log{:,["GPSLatitude","GPSLongitude","GPSHeight"]};
uavTraj = geoTrajectory(Waypoints,TimeOfArrival=log.Time,SampleRate=10);

count = 1;
while ~isDone(uavTraj)
   [position(count,:),orientation(count,:),velocity(count,:),acceleration(count,:),angularVelocity(count,:)] = uavTraj();
   count = count + 1;
end

untitled6

●

End-to-End Simulation Position RMS Error
	X: 1315.04 , Y: 2586.59, Z: 379.23   (meters)

End-to-End Quaternion Distance RMS Error (degrees) 
	118.32 (degrees)

I'm afraid otherwise I'm well

[Aashir]

See, the error is 2.5km and 1.3 km which is too big on the simulated data. How can this be resolved?

[Cris LaPierre]

That does seem high. Unfortunately, at this point, I'd just be googling the same as you to try to figure out the issue. Hopefully someone else with more expierience can jump in.