How to find waypoints, with data from a gyro and accelrometer

So I have the task of making waypoints and waypoint trajectory with the information given to me by a gyro and accelometer

Using the output of the gyro I have the orientation in both Euler and Quat

Use the acclerometer, I take the current reading - the previous reading to get the instant acceleration. And using SUVAT (PreVAcc t+PrevVelo=Velo) I have the velocity. Using further SUVAT I can find a distance (Assuming the object begins at 0,0,0, I can then find the positional data) (s=Velo*t+0.5*a*t^2). I know when using the waypoint trajectory function it normally outputs this kind of data but I need it to work backwards and give me the way points.

Any pointers

Answer 1

[Riya]

Hi,

I understand that you want to create waypoints from gyro and accelerometer data which involves sensor fusion, noise filtering, and double integration of acceleration to get position. Here is a concise step-by-step process:

• Apply filters to gyro and accelerometer data to reduce noise.
• Use gyro data to track the orientation of the object.
• Subtract gravity from the accelerometer data.
• Convert acceleration from the body frame to the inertial frame using the orientation data.
• Integrate the acceleration to obtain velocity.
• Integrate the velocity to obtain position.
• Create waypoints based on significant position changes.

Here is a sample code snippet for the same:

% Initialize state variables 
prevVelocity = [0; 0; 0]; 
prevPosition = [0; 0; 0]; 
waypoints = []; 

% Loop through sensor data 
for i = 1:length(sensorData) 
    % Filter and transform sensor data 
    orientation = getOrientation(sensorData(i).gyro); 
    accelInertial = transformAcceleration(sensorData(i).accel, orientation); 
    
    % Integrate for velocity and position 
    deltaTime = sensorData(i).deltaTime; 
    velocity = prevVelocity + accelInertial * deltaTime; 
    position = prevPosition + velocity * deltaTime; 
    
    % Update waypoints 
    if isNewWaypoint(position, waypoints) 
        waypoints = [waypoints; position']; 
    end 
    
    % Update state for next iteration 
    prevVelocity = velocity; 
    prevPosition = position; 
end 

function isNew = isNewWaypoint(position, waypoints) 
isNew = isempty(waypoints) || norm(position - waypoints(end, :)) > threshold; 
end 

This is a simplified representation and assumes you have functions like `getOrientation` and `transformAcceleration` to process gyro and accelerometer data, respectively. In practice, you'd need to handle sensor calibration, noise, and drift, and possibly use additional data sources to correct for accumulated errors.

For more information you can refer following articles for more information:

• https://www.matlabsolutions.com/resources/analyse-accelerometer-and-gyroscope-data.php
• https://www.researchgate.net/post/How-to-convert-raw-data-from-accelerometer-and-gyroscope-to-displacement
• https://www.researchgate.net/post/How-to-interpret-and-analyse-data-from-accelerometer-and-gyroscope