Clear Filters
Clear Filters

Sensor fusion orientation and velocity problems

3 views (last 30 days)
Hello guys, i'm trying to do a sensor fusion to get Position, Velocity and Orientation and i'm using an insfilterNonholonomic.
The position that i get is corrent, while the orientation and velocity are really different.
For example in the photo you can see on the left the real Velocity and on the right the estimated one.
What can i do?
The code looks like this:
function [estPosition, estOrientation, estVelocity] = fusion(accelData, gyroData, gps_pos, gps_vel,time)
persistent FUSE
if isempty(FUSE)
FUSE = insfilterNonholonomic("IMUSampleRate",5,"ReferenceFrame", "NED");
FUSE.State(1:4) = [0.707,0,0,0.707];
FUSE.State(5:7) = [0,0,0];
FUSE.State(8:10) = [0,0,0];
FUSE.State(11:13) = [0,0,0];
FUSE.State(14:16) = [0,0,0];
predict(FUSE, accelData, gyroData);
posCovariance = diag([0 0 0]);
velCovariance = diag([0.01 0.01 0.01]);
fusegps(FUSE, gps_pos, posCovariance, gps_vel, velCovariance);
[pos,quatOrient,estVelocity] = pose(FUSE);
estPosition = double(pos);
estOrientation = quat2eul(quatOrient, "XYZ");

Accepted Answer

Ryan Salvo
Ryan Salvo on 26 Mar 2024
Hi Victor,
Since you have an expected velocity, you can use the tune command to adjust the parameters on the insfilterNonholonomic object. The tune command attempts to reduce the estimation error of the filter by adjusting the filter parameters and measurement noises.
Vittorio on 26 Mar 2024
Ok that's correct but in terms of Simulation. I want to put this code in irl so i won't have the exact velocity, how could i do that for IRL?
Ryan Salvo
Ryan Salvo on 27 Mar 2024
You'll need to adjust the parameters based on the sensor noise characteristics and the type of motion you are estimating. For IMU, one method is to compute the Allan variance to extract noise parameters. Other filter parameters can be adjusted based on the motion, more detailed discussion can be found in this section of this example.

Sign in to comment.

More Answers (0)

Community Treasure Hunt

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

Start Hunting!