Ошибка состояния фильтра Калмана оценивает неправильные значения

Question

Я реализовал этот фильтр Кальмана состояния ошибки, который имеет данные IMU в качестве входных данных (220 Гц), и он корректирует прогноз с помощью измерений СШП (50 Гц).Я хочу оценить позу квадротора.

Код:

    function [x_hat,bound_p] = ESKF(d_meas,p_am,clockUWB,dt,u)

% u = IMU inputs
% d_meas = UWB measure
% p_am = anchor coordinate. There are 4 anchors that send the measurement one at a time cyclically       
    g = [0 0 9.81]';

    am = u(1:3);     % from acc
    wm = u(4:6);     % from gyro

persistent P Q R I dx_hat x_n p q v ba bw Fw 

if isempty(P)

    sig_an = 0.05;          % [m/s^2]
    sig_wn = 0.01;          % [rad/s]
    sig_wbn = 0.0001;        % [rad/s*sqrt(s)]
    sig_abn = 0.0001;        % [m/(s^2*sqrt(s)]
    sig_uwb = 0.0214;       % [m]

    Q_an = sig_an*sig_an*eye(3);      % [m^2/s^2]
    Q_wn = sig_wn*sig_wn*eye(3);      % [rad^2]
    Q_abn = sig_abn*sig_abn*eye(3);   % [m^2/s^4]
    Q_wbn = sig_wbn*sig_wbn*eye(3);   % [rad^2/s^2]

    clockUWB = 0;

    dx_hat = zeros(15,1);                      % error state
    x_n = [5 4 5 1 0 0 1 0 0 0 0 0 0 0 0 0]';     % initial state
    P = eye(15,15);
    Fw = [zeros(3,12);eye(12,12)];
    Q = blkdiag(Q_an,Q_wn,Q_wbn,Q_abn);
    R = sig_uwb*sig_uwb;        
    I = eye(length(dx_hat));

    p = x_n(1:3);
    v = x_n(4:6);
    q = x_n(7:10);
    bw = x_n(11:13);
    ba = x_n(14:16);
end

%% NOMINAL STATE

if ((wm - bw) == [0 0 0]')
    qw = [1 0 0 0]';
else
    nw = norm(wm - bw);
    qw = [cos(nw*dt/2); ((wm - bw)/nw)*sin(nw*dt/2)];      
end

R_ui = quat2rotm(q');                   

% PREDICTION
p = p + v*dt + 1/2*(R_ui*(am - ba) + g)*dt*dt;
v = v + (R_ui*(am - ba) + g)*dt;
q = quatmultiply(q',qw')';
q = q/norm(q);
% bw = bw;
% ba = ba;

%% ERROR STATE

% delta_p = delta_p + delta_v*dt;
% delta_v = delta_v + (-R_ui*skew(am - ba)*delta_th - R_ui*delta_ba)*dt + an); 
% delta_th = R_ui'*delta_th - delta_bw*dt + wn;
% delta_bw = delta_bw + wbn*ones(3,1);
% delta_ba = delta_ba + abn*ones(3,1);

F = [       eye(3),     eye(3)*dt,                     zeros(3,3),     zeros(3,3),     zeros(3,3);
        zeros(3,3),        eye(3),           -R_ui*skew(am-ba)*dt,     zeros(3,3),       -R_ui*dt;
        zeros(3,3),    zeros(3,3),                          R_ui',       -eye(3)*dt,     zeros(3,3);
        zeros(3,3),    zeros(3,3),                     zeros(3,3),         eye(3),     zeros(3,3);
        zeros(3,3),    zeros(3,3),                     zeros(3,3),     zeros(3,3),        eye(3)];

%PREDCTION

 dx_hat = F*dx_hat;                    

P = F * P * F' + Fw * Q * Fw';

%%  UPDATE
% Only when measures arrive

if (clockUWB == 1)

    h = p - p_am;  
    d_am = norm(h);                             

    H = [ (1/(2*d_am))*2*h'*eye(3), zeros(1,3), zeros(1,3), zeros(1,3), zeros(1,3)];

    K = P * H' * inv(H * P * H' + R);

    dx_hat = K * (d_meas - d_am);                                 

    delta_x = [dx_hat(1:6);[1,1/2*dx_hat(7:9)']';dx_hat(10:15)];   

    P = (I - K * H) * P * (I - K * H)' + K * R * K';

%% ERROR INJECTION

    p = p + delta_x(1:3);
    v = v + delta_x(4:6);
    q = quatmultiply(q',delta_x(7:10)')';
    q = q/norm(q);
    bw = bw + delta_x(11:13);
    ba = ba + delta_x(14:16);    

    sig_p = 3*[sqrt(P(1,1));sqrt(P(2,2));sqrt(P(2,2))];
    bound_p = [p(1) + sig_p(1),p(1) - sig_p(1),p(2) + sig_p(2),p(2) - sig_p(2),p(3) + sig_p(3),p(3) - sig_p(3)];

    x_hat = [p;v;q;bw;ba];
    yaw_err = abs(dx_hat(9));
    yaw_err_old = yaw_err;

%% RESET ERROR

    G = blkdiag(eye(6),eye(3) - skew(1/2*dx_hat(7:9)),eye(6));
    delta_x = zeros(16,1);
    dx_hat = zeros(15,1);
    P = G * P * G';

else

    x_hat = [p;v;q;bw;ba];
    delta_x = zeros(16,1);
    sig_p = 3*[sqrt(P(1,1));sqrt(P(2,2));sqrt(P(2,2))];
    bound_p = [p(1) + sig_p(1),p(1) - sig_p(1),p(2) + sig_p(2),p(2) - sig_p(2),p(3) + sig_p(3),p(3) - sig_p(3)];
end
end

Отлично оценивает положение, а также скорость хороша.Проблема в том, что он очень плохо оценивает крен, тангаж и рыскание (которые я получаю из кватерниона благодаря функции quat2eul), а некоторые смещения абсолютно неверны.Может кто-нибудь сказать мне, где код не так?Спасибо

Это основная модель Simulink

В зеленом блоке есть функция фильтра.

Для того, чтобы моделировать траекторию иСШП измерения в СШП блоке есть такой сценарий:

function [xt,yt,zt,d_meas,p_am] = fcn(t,clock)

sig_uwb = 0.0214;
dn = normrnd(0,sig_uwb);        

persistent k i
if isempty(k)
    k = 0;
    clock = 0;
    i = 0;
end

x = 3*cos(1/3*t) + 3;
y = 3*sin(1/3*t) + 3;
z = 5;

a1 = [-4.12,-3.67,2.72];   % anchors coordinates
a2 = [2.45,-2.70,0.063];
a3 = [-2.43,3.07,0.075];
a4 = [3.65,2.42,2.62];

d1 = norm(a1 - [x,y,z]);
d2 = norm(a2 - [x,y,z]);
d3 = norm(a3 - [x,y,z]);
d4 = norm(a4 - [x,y,z]);

A = [a1,d1;a2,d2;a3,d3;a4,d4];

if (clock == 1)
    k = k + 1;
    i = mod(k,4) + 1;
    d_meas = A(i,4) + dn;
    p_am = A(i,(1:3))';
else
    d_meas = 0;
    p_am = zeros(3,1);
end

xt = x;
yt = y;
zt = z;

Таким образом, дрон имитирует круговую траекторию с радиусом, равным 3. Блок IMU содержит всего 2 вектора: am = [0 -1/3 -9.81] и wm = [0 0 1/3].

Смещение должно быть постоянным и равным 0. Вместо этого я получаю значения, такие как 3 или 2 и не постоянные.Бросок и шаг не равны 0, как следует.

Текст, который я использую для реализации ESKF, - это Кватернионная кинематика для состояния ошибки KF из pag.52.