code Adaptive Gaussian Sum Filter

ComputeFPE_QP_UT_noPG.m

@@ -0,0 +1,51 @@
+function [MM, NN] = ComputeFPE_QP_UT_noPG(weig, mu, sig, model, time, tk)
+
+% compute matrix MM
+MM = zeros(length(weig));
+MMtmp = MM;
+for i = 1 : length(weig)
+    for j = i+1 : length(weig)
+        MMtmp(i,j) = getLikelihood(mu{i}-mu{j}, sig{i}+sig{j});
+    end
+    MM(i,i) = 1/sqrt(det(4.*pi.*sig{i}));
+end
+MM = MM + MMtmp + MMtmp';  
+MM = MM/time.dt^2;
+
+% compute matrix NN
+NN = zeros(length(weig));    
+
+for i = 1 : length(weig)
+    for j = 1 : length(weig)
+
+        SigC = inv(inv(sig{i}) + inv(sig{j}));
+        muC = SigC*(inv(sig{i})*mu{i} + inv(sig{j})*mu{j});
+        cC = getLikelihood(mu{i}-mu{j},sig{i}+sig{j});
+
+%         sigma = SRUKF_getSigmaPoints(muC, chol(SigC)', model.fn, 10^-4);
+        sigma = IUT_getSigmaPoints(muC, chol(SigC)', model.fn);
+
+         for k = 1 : length(sigma)
+            x = sigma(k).x;
+
+            [pg, dpgdx, ddpgdx, dpgdmu, dpgdsig] = GaussianPDF_noPG(x, mu{j}, sig{j});
+
+            myX = [mu{j}; reshape(sig{j}, numel(sig{j}), 1)];
+            myDX = EKF_propagation(time.tspan(tk), myX, model);
+            mudot = myDX(1:model.fn);
+            sigdot = reshape(myDX(model.fn+1:end), model.fn, model.fn);
+
+            dpgidt = dpgdmu'*mudot + trace(dpgdsig*sigdot);
+
+            fx = feval(model.fx, time.tspan(tk), x);
+            Jx = feval([model.fx '_jac'], time.tspan(tk), x);
+            dopgi_dot = - dpgdx' * fx - trace(Jx) + 1/2*trace(model.Q * ddpgdx);     % pg*trace(Jx)
+
+            Pi = 1/time.dt;                             %         Pk(j) = pg/time.dt;
+            Lj = dpgidt - dopgi_dot - 1/time.dt;       %         Lk(j) = dpgidt - dopgi_dot - pg/time.dt;
+
+            NN(i,j) = NN(i,j) + sigma(k).W*Pi*Lj;
+        end;
+        NN(i,j) = NN(i,j)*cC;
+    end;
+end;

EKF_measurement_update.m

@@ -0,0 +1,13 @@
+function [omu, osig, z_mu, z_sig] = EKF_measurement_update(myfilter, model, imu, isig, T, opt, ymeas)
+%
+% EKF Measurement Update
+%
+% Gabriel Terejanu ([email protected])
+
+z_mu = feval(model.hx, imu);
+h = feval([model.hx '_jac'], imu);
+z_sig = h*isig*h';
+
+gain = isig * h' * inv(z_sig + model.R);
+osig = isig - gain*h*isig;
+omu = imu + gain*(ymeas - z_mu);

EKF_propagation.m

@@ -0,0 +1,12 @@
+function dx = EKF_propagation(t, x, model)
+
+n = model.fn;
+
+mu = x(1:n);
+sig = reshape(x(n+1:end),n,n);
+
+F = feval([model.fx '_jac'], t, mu);
+sig = F*sig + sig*F' + model.Q;
+mu = feval(model.fx, t, mu);
+
+dx = [mu; reshape(sig, numel(sig), 1)];

EKF_time_update.m

@@ -0,0 +1,14 @@
+function [omu, osig] = EKF_time_update(myfilter, model, imu, isig, T, opt)
+%
+% EKF Time Update
+%
+% Gabriel Terejanu ([email protected])
+
+n = model.fn;
+
+myX = [imu; reshape(isig, numel(isig), 1)];
+[t,y] = ode45(@EKF_propagation, T, myX, opt, model);
+
+myY = y(end,:);
+omu = myY(1:n)';
+osig = reshape(myY(n+1:end),n,n);

GaussianPDF_noPG.m

@@ -0,0 +1,10 @@
+function [pg, dpgdx, ddpgdx, dpgdmu, dpgdsig] = GaussianPDF_noPG(x,mu,sig)
+
+r = x - mu;
+pg = getLikelihood(r, sig);
+isig = pinv(sig);
+
+dpgdmu = isig*r; % <-- pg has been removed
+dpgdsig = 1/2 * (isig*r*r'*isig - isig); % <-- pg has been removed
+dpgdx = -dpgdmu;
+ddpgdx = 2*dpgdsig;

IUT_getSigmaPoints.m

@@ -0,0 +1,26 @@
+function sigma = IUT_getSigmaPoints(x, S, n)
+
+alpha = normpdf(2)/normpdf(1);
+beta = normpdf(3)/normpdf(1);
+
+% # of sigma points to be generated = 6*n+1
+
+sigma(1).x = x;
+sigma(1).W = 1 - n*(1+alpha+beta)/(1+4*alpha+9*beta);
+
+for i = 1 : n
+    sigma(i+1).x = x + S(:,i);
+    sigma(i+1).W = 1/(2+8*alpha+18*beta);
+    sigma(i+n+1).x = x - S(:,i);
+    sigma(i+n+1).W = 1/(2+8*alpha+18*beta);
+
+    sigma(i+2*n+1).x = x + 2*S(:,i);
+    sigma(i+2*n+1).W = alpha/(2+8*alpha+18*beta);
+    sigma(i+3*n+1).x = x - 2*S(:,i);
+    sigma(i+3*n+1).W = alpha/(2+8*alpha+18*beta);    
+
+    sigma(i+4*n+1).x = x + 3*S(:,i);
+    sigma(i+4*n+1).W = beta/(2+8*alpha+18*beta);
+    sigma(i+5*n+1).x = x - 3*S(:,i);
+    sigma(i+5*n+1).W = beta/(2+8*alpha+18*beta);      
+end

PF_meas_update.m

@@ -0,0 +1,37 @@
+function [X_new, w_new] = PF_meas_update(X_old, w_old, model, pf, ymeas)
+%
+% Particle Filter Measurement Update
+%
+% X_new   - the new set of particles after reweighting and resampling if necessary
+%
+% Gabriel Terejanu ([email protected])
+
+%% ------------------------------------------------------------------------
+% weights update
+%--------------------------------------------------------------------------
+mX_pf = zeros(model.hn, pf.no_particles);
+w_new = zeros(size(w_old));
+for j = 1 : pf.no_particles
+    % get the measurement
+    mX_pf(:,j) = feval(model.hx, X_old(:,j));
+    w_new(j) = w_old(j) * getLikelihood(ymeas - mX_pf(:,j), model.R);
+end
+w_new = w_new./sum(w_new);
+X_new = X_old;
+
+%% ------------------------------------------------------------------------
+% resampling
+%--------------------------------------------------------------------------
+if (pf.resample) 
+	Neff = 1/sum(w_new.^2);
+    if (Neff < pf.neff * pf.no_particles)
+        I = myresampling(w_new);
+        I = round(I);
+        for j = 1 : pf.no_particles
+            X_new(:,j) = X_old(:,I(j));
+        end;    
+        % reset weights
+        w_new = ones(1,pf.no_particles)/pf.no_particles;
+    end;
+end;
+