Error state kalman filter estimates wrong values - matlab

Question

I've implemented this error state kalman filter, which has IMU data as input (220Hz) and it corrects the prediction with UWB measurements (50 Hz).
I want to estimate the pose of a quadrotor.
The code is:
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
It estimates position perfectly, and also velocity is nice.
The problem is that it estimates roll, pitch and yaw (which I derive from the quaternion thanks to quat2eul function) very badly and some bias are totally wrong.
Can someone tell me where the code is wrong?
Thanks
This is the main simulink model
In the green block there is the function of the filter.
In order to simulate a trajectory and UWB measurements in the UWB block there is this script:
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;
So the drone simulates a circular trajectory with radius equals to 3.
The IMU block contains just 2 vector:
am = [0 -1/3 -9.81] and wm = [0 0 1/3].
The bias should be constant and 0. I obtain instead values like 3 or 2 and non constant.
Roll and pitch aren't 0 as they should.
The text I am using to implement the ESKF are Quaternion kinematics for the error-state KF from pag. 52.