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
