Problem with yaw angle convergence

[na851998]

Hello,
Currently, I'm trying to use NaveGo with raw data. The problem is, when I initialize roll and pitch with random values, they can converge to the reference values, but it is not the case with yaw angle. So I was wondering, what may cause the problem.
(The reference yaw angle is ranged from 0 to 360 degrees so I subtract it by 180 degrees)

[rodralez]

A dead-reckoning systems, as an inertial navigation system, needs to known correct initial position, velocity and attitude (PVA). If you don't provide the correct PVA, PVA estimates will diverge.

[na851998]

Hi,
In real_ekinox example, I initilize roll, pitch and yaw as 2 rads, which are far different from true initial angles.



As you can see, all angles converge to their true values. But when I use different dataset, only pitch and roll angles converge.
I think there are two possibilities:

1. The dataset is wrong (but somehow pitch and roll still converge)
2. The conversion of yaw angle from range [0,2pi] to [-pi,pi] is wrong (yaw angle is subtracted by 2pi when value is greater than pi)

Thank you and look forward to your reply.
Kind regards.

[rodralez]

Hello,

You can try the following code to see how the yaw angle is corrected when is between 0 a 2pi.

clc
close all
clear
matlabrc

addpath ./ins/
addpath ./ins-gnss/
addpath ./ins-visual/
addpath ./conversions/

yaw_up = linspace(0, 2*pi, 300)';
yaw_down = linspace(2*pi, 0, 300)';
yaw = [yaw_up; yaw_down];

yaw_corr = correct_yaw(yaw);

figure
plot(yaw, '-', 'Color', 'green', 'LineWidth', 2)
hold on
plot(yaw_corr, '--', 'LineWidth', 2)
grid on
legend('yaw, from 0 to 2pi', 'yaw correct, from -pi to pi')