Related
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.
Consider the following code.
Wx = zeros(N, N);
for ii = 1 : 1 : N
x_ref = X(ii); y_ref = Y(ii);
nghlst_Local = nghlst(ii, find(nghlst(ii, :))); Nl = length(nghlst_Local);
x_Local = X(nghlst_Local, 1); y_Local = Y(nghlst_Local, 1);
PhiU = ones(Nl+1, Nl+1); PhiU(end, end) = 0;
Phi = ones(Nl+1, Nl+1); Phi(end, end) = 0;
Bx = zeros(Nl+1,1);
for jj = 1 : 1 : Nl
for kk = 1 : 1 : Nl
rx = x_Local(jj,1) - x_Local(kk,1);
ry = y_Local(jj,1) - y_Local(kk,1);
PhiU(jj, kk) = (1 - U(1,1))) / sqrt(rx^2 + ry^2 + c^2);
end
rx = x_ref - x_Local(jj);
ry = y_ref - y_Local(jj);
Bx(jj, 1) = ( (Beta * pi * U(1,1)/(2*r_0*norm(U))) * cos( (pi/2) * (-rx * U(1,1) - ry * U(2,1)) / (r_0 * norm(U)) ) ) / sqrt(rx^2 + ry^2 + c^2) - rx * (1 - Beta * sin( (pi/2) * (-rx * U(1,1) - ry * U(2,1)) / (r_0 * norm(U)) ))/ (rx^2 + ry^2 + c^2)^(3/2);
end
invPhiU = inv(PhiU);
CX = Bx' * invPhiU; CX = CX (1, 1:end-1); Wx (ii, nghlst_Local) = CX;
end
I want to convert the first for loop into parfor loop. The rest of the code works fine, but the following assignment statement does not work when I change for to parfor.
Wx (ii, nghlst_Local) = CX;
I want to know what is this is wrong and how to remove such errors. Thank you.
I am right now stuck on a problem in matlab. What I have done is that I have an equation that is passed on into another function which works by the bisection-method.
But I have a multiplier that I am trying to implement which somehow leads to the function crashing.
Before I introduced the multiplier it all worked, I tried breaking it down by entering the multiplier value manually and it didn't work
P_{1} = 0.6;
P_{2} = 0.2;
P_{3} = 0.2;
a_1 = 4/3;
a_2 = -7/3;
b_1 = -1/3;
b_2 = 4/3;
persistent multiplier
multiplier = exp(a_1 * 44 + a_2 * 14 + 0);
eqn = #(x) ((a_1 * x + b_1)^a_1) * ((a_2 * x + b_2)^a_2) * x ...
-(P_{1}^a_1) * (P_{2}^a_2) * P_{3} * multiplier;
Q_{3} = Bisectionmethod(a_1, a_2, b_1, b_2, eqn);
Here is the calculating part of the bisection method.
x_lower = max(0, -b_1 / a_1);
x_upper = -b_2 / a_2;
x_mid = (x_lower + x_upper)/2;
Conditional statement encompassing the method of bisection
while abs(eqn(x_mid)) > 10^(-10)
if (eqn(x_mid) * eqn(x_upper)) < 0
x_lower = x_mid;
else
x_upper = x_mid;
end
x_mid = (x_lower + x_upper)/2;
end
Based on the information you provided this is what I came up with
function Q = Stackoverflow
persistent multiplier
P{1} = 0.6;
P{2} = 0.2;
P{3} = 0.2;
a1 = 4/3;
a2 = -7/3;
b1 = -1/3;
b2 = 4/3;
multiplier = exp(a1 * 44 + a2 * 14 + 0);
eqn = #(x) ((a1 .* x + b1).^a1) .* ((a2 .* x + b2).^a2) .* x -(P{1}.^a1) .* (P{2}.^a2) .* P{3} .* multiplier;
Q{3} = Bisectionmethod(eqn, max([0, -b1/a1]), -b2/a2, 1E-10);
end
function XOut = Bisectionmethod(f, xL, xH, EPS)
if sign(f(xL)) == sign(f(xH))
XOut = [];
error('Cannot bisect interval because can''t ensure the function crosses 0.')
end
x = [xL, xH];
while abs(diff(x)) > EPS
x(sign(f(mean(x))) == sign(f(x))) = mean(x);
end
XOut = mean(x);
end
I'm trying to solve some ODEs in MatLab and seeing as the variables in the equations are populations they need to be constrained to being positive. So I tried using odeset() before calling the equation solver to make them non-negative but on plotting the values afterwards they are actually negative at times (in the code below it is the magenta line). What am I doing wrong?
Here's some code:
%Lots of variables
includeJ=1;
cullLIRate=1/2000;
cullDIRate=1/2000;
N = 16800;
beta = 2e-7;
delta = 0.5;
gamma = 1/50;
sigma = 1/400;
mu = 1/365;
maxTime = 30*365;
kappa = N;
gR = 0.05;
mJ = 1/3650;
initJPerAdult = 10;
numInitE = 1000;
TSpan = [0,maxTime];
initState = [N-numInitE,numInitE,0,0,0,initJPerAdult*N];
%IMPORTANT BIT HERE
options = odeset('NonNegative', 1:6)
scirSoln = ode45(#equation,TSpan,initState,[],beta,delta,gamma,sigma,mu,kappa,gR,mJ,cullLIRate,cullDIRate,includeJ);
scirVals = deval(scirSoln,timeToPlot);
plot(timeToPlot,scirVals(1,:));
hold on;
plot(timeToPlot,scirVals(3,:),'k');
plot(timeToPlot,scirVals(4,:),'g');
plot(timeToPlot,scirVals(6,:),'m');
timeToPlot = [0:max(TSpan)/1000:max(TSpan)];
The code for equation(...) is:
function retVal = equation(t,y,beta,delta,gamma,sigma,mu,kappa,gR,mJ,cullLIRate,cullDIRate,includeJ)
retVal = zeros(6,1);
S = y(1);
E = y(2);
LI = y(3);
DI = y(4);
R = y(5);
J = y(6);
retVal(1)= mJ * J - beta * S * (delta * LI + DI);
retVal(2) = beta * S * (delta * LI + DI) - gamma * E;
retVal(3) = gamma * E - (cullLIRate + sigma) * LI;
retVal(4) = sigma * LI - (mu + cullDIRate) * DI;
retVal(5) = mu * DI + cullLIRate* LI + cullDIRate * DI;
retVal(6) = gR * S * (1 - S / kappa) - mJ * J;
end
You are not passing your defined odeset (options variable) to the ODE45 - solver.
The syntax for the ODE45 is: [T,Y] = ODE45(ODEFUN,TSPAN,Y0,OPTIONS,P1,P2...)
Glad it worked! :)
I am stuck at vectorizing this tricky loop in MATLAB/Octave:
[nr, nc] = size(R);
P = rand(nr, K);
Q = rand(K, nc);
for i = 1:nr
for j = 1:nc
if R(i,j) > 0
eij = R(i,j) - P(i,:)*Q(:,j);
for k = 1:K
P(i,k) = P(i,k) + alpha * (2 * eij * Q(k,j) - beta * P(i,k));
Q(k,j) = Q(k,j) + alpha * (2 * eij * P(i,k) - beta * Q(k,j));
end
end
end
end
The code tries to factorize R into P and Q, and approaching the nearest P and Q with an update rule. For example, let R = [3 4 0 1 1; 0 1 0 4 4; 5 4 3 1 0; 0 0 5 4 3; 5 3 0 2 1], K=2, alpha=0.01 and beta=0.015. In my real case, I will use a huge sparse matrix R (that's why I need vectorization), and K remain small (less than 10). The goal of the whole script is producing a prediction value for every 0 elements in R, based on the non zero elements. I got this code from here, originally written in Python.
This looks like one of those cases that not all code can be vectorized. Still, you can make it a bit better than it is now.
[nr, nc] = size(R);
P = rand(nr, K);
Q = rand(K, nc);
for i = 1:nr
for j = 1:nc
if R(i,j) > 0
eij = R(i,j) - P(i,:)*Q(:,j);
P(i,:) = P(i,:) + alpha * (2 * eij * Q(:,j)' - beta * P(i,:));
Q(:,j) = Q(:,j) + alpha * (2 * eij * P(i,:)' - beta * Q(:,j));
end
end
end
Since the operations on P and Q are serial in nature (iterative updates) I do not think you can do much better. You can save the if in the loop:
[nr, nc] - size(R);
P = rand(nr, K);
Q = rand(K, nc);
[nzi nzj] = find( R > 0 );
for ii=1:numel(nzi)
i = nzi(ii);
j = nzj(ii);
eij = R(i,j) - P(i,:)*Q(:,j);
P(i,:) = P(i,:) + alpha * (2 * eij * Q(:,j)' - beta * P(i,:));
Q(:,j) = Q(:,j) + alpha * (2 * eij * P(i,:)' - beta * Q(:,j));
end