Sensor fusion orientation and velocity problems

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];

end

else

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");

end

end

Answer 1

[Ryan Salvo]

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.

Thanks,

Ryan

[Vittorio]

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]

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.