├── ukfmani ├── quaternions │ ├── test │ │ ├── .cvsignore │ │ ├── Contents.m │ │ ├── unit_test.m │ │ ├── test_qvqc.m │ │ ├── test_qnorm.m │ │ ├── test_dcm2q.m │ │ ├── test_qconj.m │ │ ├── test_q2dcm.m │ │ ├── test_isq.m │ │ ├── test_isnormq.m │ │ ├── test_qdecomp.m │ │ └── test_qmult.m │ ├── qvqc.m │ ├── qvrot.m │ ├── qvxform.m │ ├── qconj.m │ ├── qnorm.m │ ├── ChangeLog │ ├── qdecomp.m │ ├── isnormq.m │ ├── q2dcm.m │ ├── isq.m │ ├── Contents.m │ ├── dcm2q.m │ ├── qcvq.m │ └── qmult.m ├── helpers │ ├── cellget.m │ ├── sel1.m │ ├── sel2.m │ ├── meancovRot.m │ ├── meancovRn.m │ ├── se3dinv.m │ ├── se3dtrans.m │ ├── qmstep.m │ ├── skew.m │ ├── qmdelta.m │ ├── quatdiff.m │ ├── qomega2q.m │ ├── se3adj.m │ ├── sampleQuats.m │ ├── meannonan.m │ ├── permvector.m │ ├── svdsqrt.m │ ├── so3log.m │ ├── so3exp.m │ ├── ut_mweights2.m │ └── meancovSE3manopt.m ├── manifolds │ ├── manisetup.m │ ├── makeCom.m │ ├── makePower.m │ ├── makeQuat.m │ ├── makeRot.m │ ├── makeRn.m │ ├── tbd_makePositive.m │ ├── makeQuat1.m │ ├── makeSE3Quat.m │ ├── makeSPD.m │ ├── makeGenProduct.m │ ├── makeRot1.m │ ├── makeProject.m │ ├── tbd_makeSE3MatLocal.m │ ├── makeSE3Mat.m │ ├── makeProduct.m │ └── makeGenProduct.py ├── core │ ├── maniinfo.m │ ├── manipackalg.m │ ├── manifusion.m │ ├── maniunpackalg.m │ ├── manifolds.md │ ├── maniunpack.m │ ├── manipack.m │ ├── int_manisetup.m │ ├── manisigmas.m │ ├── manismoother.m │ ├── Contents.m │ ├── manifusionn.m │ ├── maniunsigma.m │ └── manimeancov.m └── coreclass │ ├── AManifold.m │ ├── next │ ├── AManifoldProject.m │ ├── AManifoldSO3.m │ ├── AManifoldK.m │ ├── AManifoldR.m │ ├── AManifoldSE3.m │ └── AManifoldProduct.m │ ├── AManifoldLie.m │ ├── AManifoldWrapLie.m │ ├── AManifoldWrap.m │ └── AGaussian.m ├── .gitignore ├── init.m ├── Contents.m ├── tests ├── testmaniunsigmaRn.m ├── testanycall.m ├── testpackunpack.m ├── testdeltastep.m └── testmeancovquat.m ├── examples ├── se3step_inertial_no_acc.m ├── testmakegen.m ├── exampleRot.m ├── exampleQuat.m ├── exampleSE3MatInertial.m ├── exampleSE3MatAug.m ├── exampleSE3MatAug2.m ├── exampleSE3Mat.m └── exampleSE3MatAugMissingPos.m ├── index.rst ├── Makefile ├── testSE3.m ├── task_headpose ├── headPose.py ├── headPose_estim.m ├── headPose_estim2.m └── headPose_estim3.m ├── TODO.md ├── manse3mat_e3_e3.m ├── conf.py └── README.md /ukfmani/quaternions/test/.cvsignore: -------------------------------------------------------------------------------- 1 | test.out 2 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.asv 2 | *.bak 3 | ~$*.docx 4 | *.pyc 5 | -------------------------------------------------------------------------------- /ukfmani/helpers/cellget.m: -------------------------------------------------------------------------------- 1 | function r = cellget(x,i) 2 | r=x{i}; -------------------------------------------------------------------------------- /ukfmani/helpers/sel1.m: -------------------------------------------------------------------------------- 1 | function r = sel1(x,i) 2 | 3 | r = x(i); -------------------------------------------------------------------------------- /ukfmani/manifolds/manisetup.m: -------------------------------------------------------------------------------- 1 | function r = manisetup(r) 2 | -------------------------------------------------------------------------------- /ukfmani/helpers/sel2.m: -------------------------------------------------------------------------------- 1 | function r = sel2(x,a,b) 2 | 3 | r = x(a,b); -------------------------------------------------------------------------------- /init.m: -------------------------------------------------------------------------------- 1 | mfilepath=fileparts(mfilename('fullpath')); 2 | addpath(genpath([mfilepath,filesep,'ukfmani'])); -------------------------------------------------------------------------------- /ukfmani/helpers/meancovRot.m: -------------------------------------------------------------------------------- 1 | function [m,S] = meancovRot(x) 2 | 3 | assert('to be implemented, check maniunsigma'); -------------------------------------------------------------------------------- /Contents.m: -------------------------------------------------------------------------------- 1 | % UKFMANI 2 | % 3 | % Files 4 | % init - 5 | % manpippo3 - 6 | % manse3mat_e3_e3 - 7 | -------------------------------------------------------------------------------- /ukfmani/helpers/meancovRn.m: -------------------------------------------------------------------------------- 1 | % Emanuele Ruffaldi 2017 @ SSSA 2 | function [m,S] = meancov(x) 3 | 4 | m = mean(x); 5 | S = cov(x); -------------------------------------------------------------------------------- /ukfmani/helpers/se3dinv.m: -------------------------------------------------------------------------------- 1 | function [mu,Sigma] = se3dinv(mu0,Sigma0) 2 | 3 | mu= inv(mu0); 4 | Ai = se3adj(mu); 5 | Sigma = Ai*Sigma0*Ai'; 6 | -------------------------------------------------------------------------------- /ukfmani/helpers/se3dtrans.m: -------------------------------------------------------------------------------- 1 | function [mu,Sigma] = se3dtrans(T,mu0,Sigma0) 2 | 3 | mu = T*mu0; 4 | AT = se3adj(T); 5 | Sigma = AT*Sigma0*AT'; -------------------------------------------------------------------------------- /tests/testmaniunsigmaRn.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | % build Rn 4 | 5 | % test maniunsigma using log/nolog 6 | [mz,Czz,Cxz] = maniunsigma(model,Chiz,sigmainfo,vChi,steps) -------------------------------------------------------------------------------- /ukfmani/helpers/qmstep.m: -------------------------------------------------------------------------------- 1 | function q = mstep(X,y) 2 | 3 | if norm(y) == 0 4 | q = X; 5 | else 6 | q = qnorm(qmult(qomega2q(y(:)'),X)); 7 | end 8 | -------------------------------------------------------------------------------- /ukfmani/helpers/skew.m: -------------------------------------------------------------------------------- 1 | % Emanuele Ruffaldi 2017 @ SSSA 2 | function S = skew(v) 3 | S = [ 0 -v(3) v(2) 4 | v(3) 0 -v(1) 5 | -v(2) v(1) 0]; -------------------------------------------------------------------------------- /ukfmani/manifolds/makeCom.m: -------------------------------------------------------------------------------- 1 | % combination of models 2 | % 3 | % Emanuele Ruffaldi 2017 @ SSSA 4 | function m = makeCom(varargin) 5 | 6 | m = makeProduct(varargin{:}); 7 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makePower.m: -------------------------------------------------------------------------------- 1 | % replicates the model m0 n times 2 | function m = makePower(m0,n) 3 | 4 | m = makeProduct(repmat({m0},1,n)); 5 | m.type = {'Power',n,m0.type}; -------------------------------------------------------------------------------- /ukfmani/core/maniinfo.m: -------------------------------------------------------------------------------- 1 | % Infos from Manifold 2 | % 3 | % Emanuele Ruffaldi 2017 @ SSSA 4 | function [count,group,alg] = maniinfo(m) 5 | 6 | [count,group,alg] = [m.count,m.group,m.alg]; 7 | -------------------------------------------------------------------------------- /examples/se3step_inertial_no_acc.m: -------------------------------------------------------------------------------- 1 | function [Tk,wk,vk] = se3step_inertial_no_acc(Tk0,wk0,vk0,II,dt) 2 | 3 | IIL = inv(Tk0)*II; 4 | vk = vk0; 5 | wk = wk0 - inv(IIL)*(cross(wk0,IIL*wk0)); 6 | Tk = se3step(Tk0,[wk,vk]); -------------------------------------------------------------------------------- /ukfmani/helpers/qmdelta.m: -------------------------------------------------------------------------------- 1 | function w = qmdelta(X,Y) 2 | 3 | w = getomega(qmult(X,qconj(Y))); % log(x*inv(y)) 4 | 5 | function w = getomega(q) 6 | 7 | [v,phi] = qdecomp(q); 8 | assert(isnan(phi) == 0) 9 | w = v*phi; 10 | w = w(:)'; -------------------------------------------------------------------------------- /ukfmani/helpers/quatdiff.m: -------------------------------------------------------------------------------- 1 | function w = quatdiff(X,Y) 2 | 3 | w = getomega(qmult(X,qconj(Y))); % log(x*inv(y)) 4 | 5 | function w = getomega(q) 6 | 7 | [v,phi] = qdecomp(q); 8 | assert(isnan(phi) == 0) 9 | w = v*phi; 10 | w = w(:)'; -------------------------------------------------------------------------------- /ukfmani/helpers/qomega2q.m: -------------------------------------------------------------------------------- 1 | % reverse of qdecomp 2 | % Emanuele Ruffaldi 2017 @ SSSA 3 | function q = qomega2q(omega) 4 | 5 | momega = norm(omega); 6 | if momega == 0 7 | q = [0,0,0,1]; 8 | else 9 | domega = omega/momega; 10 | 11 | q = [domega*sin(momega/2),cos(momega/2)]; 12 | end -------------------------------------------------------------------------------- /ukfmani/helpers/se3adj.m: -------------------------------------------------------------------------------- 1 | % Adjoint of group (R,t) or of R as 4x4 2 | % 3 | % Convention: omega linear 4 | function M = se3adj(R,t) 5 | 6 | if nargin == 1 7 | if length(R) == 4 8 | t = R(1:3,4); 9 | R = R(1:3,1:3); 10 | else 11 | t = [0,0,0]; 12 | end 13 | end 14 | M = blkdiag(R,R); 15 | M(4:6,1:3) = skew(t)*R; 16 | -------------------------------------------------------------------------------- /ukfmani/helpers/sampleQuats.m: -------------------------------------------------------------------------------- 1 | function y = sampleQuats(n,muquat,Sigma) 2 | 3 | C = cholcov(Sigma); 4 | y = zeros(4,n); 5 | Q = randn(3,n); 6 | for i=1:n 7 | y(:,i) = mstep(muquat,C*Q(:,i)); 8 | end 9 | y = y'; %n x 4 10 | 11 | 12 | function q = mstep(X,y) 13 | 14 | if norm(y) == 0 15 | q = X; 16 | else 17 | q = qnorm(qmult(qomega2q(y(:)'),X)); 18 | end 19 | -------------------------------------------------------------------------------- /ukfmani/core/manipackalg.m: -------------------------------------------------------------------------------- 1 | % takes a model m and cell array and builds a algebra 2 | % Emanuele Ruffaldi 2017 @ SSSA 3 | function y = manipackalg(m,c) 4 | 5 | assert(isfield(m,'s'),'missing setup, use manisetup(m)'); 6 | y = zeros(size(c,1),m.alg); 7 | s = m.s; 8 | 9 | for I=1:length(s) 10 | for J=1:size(c,1) 11 | y(J,s(I).alg(1):s(I).alg(2)) = c{J,I}; 12 | end 13 | end -------------------------------------------------------------------------------- /tests/testanycall.m: -------------------------------------------------------------------------------- 1 | % we use this: 2 | % x = {[1,2,3],[],1}; 3 | % y = cell(1,nargout(@max)); 4 | % [y{:}] = max(x{:}) 5 | % 6 | % 7 | % Also remember that anonymous can return multiple output% 8 | % @(x,y) deal(x,y) 9 | % 10 | % Emanuele Ruffaldi 2017 11 | function varargout = testanycall(varargin) 12 | 13 | disp(sprintf('Nin: %d Nout:%d',nargin,nargout)); 14 | 15 | varargout = varargin; -------------------------------------------------------------------------------- /ukfmani/core/manifusion.m: -------------------------------------------------------------------------------- 1 | % given two estimates this operation performs the Covariance weighted 2 | % fusion of the two estimates 3 | % 4 | % Emanuele Ruffaldi 2017 5 | % 6 | % TODO: implement the multiple fusion algorithms == weighted mean 7 | function [X,C] = manifusion(m,x0,x1,C0,C1) 8 | 9 | % inv(inv(C0)+inv(C1)) 10 | C = C0 - C0/(C0+C1)*C0; 11 | 12 | v = m.delta(x1,x0); % from x0 to x1 13 | 14 | X = m.step(x0,C/C1*v(:)); % idem but weighted -------------------------------------------------------------------------------- /ukfmani/core/maniunpackalg.m: -------------------------------------------------------------------------------- 1 | % takes a model(C,G,A) m and a packed group x (matrix NxG) and builds a cell array 2 | % (NxC) 3 | % Emanuele Ruffaldi 2017 @ SSSA 4 | function c = maniunpack(m,x) 5 | 6 | 7 | assert(isfield(m,'s'),'missing setup, use manisetup(m)'); 8 | c = cell(size(x,1),m.count); 9 | s = m.s; 10 | 11 | for I=1:length(s) 12 | for J=1:size(x,1) 13 | c{J,I} = x(J,s(I).alg(1):s(I).alg(2)); 14 | end 15 | end 16 | 17 | 18 | -------------------------------------------------------------------------------- /ukfmani/helpers/meannonan.m: -------------------------------------------------------------------------------- 1 | 2 | function Y = meannonan(X,dim) 3 | 4 | if nargin == 1 5 | dim = find(size(X) > 1,1,'first'); 6 | if isempty(dim) 7 | dim = 1; 8 | end 9 | end 10 | 11 | if dim == 1 12 | Y = zeros(size(X,2),1); 13 | for I=1:size(Y,1) 14 | Y(I) = mean(X(isnan(X(:,I)) == 0,I)); 15 | end 16 | else 17 | Y = zeros(size(X,1),1); 18 | for I=1:size(Y,1) 19 | Y(I) = mean(X(I,isnan(X(I,:)) == 0)); 20 | end 21 | end 22 | -------------------------------------------------------------------------------- /ukfmani/helpers/permvector.m: -------------------------------------------------------------------------------- 1 | function P = permvector(neworder,n) 2 | if nargin == 2 3 | assert(isscalar(n),'n is the number of elements'); 4 | left = setdiff(1:n,neworder); 5 | neworder = [neworder(:);left(:)]; 6 | end 7 | 8 | assert(length(unique(neworder)) == length(neworder),'all elements'); 9 | assert(all(neworder >= 1)&all(neworder <= length(neworder)),'all elments 1..n'); 10 | 11 | P = zeros(length(neworder)); 12 | for I=1:length(neworder) 13 | P(I,neworder(I))= 1; 14 | end 15 | -------------------------------------------------------------------------------- /ukfmani/helpers/svdsqrt.m: -------------------------------------------------------------------------------- 1 | % sqrt using SVD 2 | % 3 | % Originally introduced in the demonstration https://github.com/eruffaldi/compare-mvn-transform 4 | % 5 | % Returns also the ordered list of singular values 6 | % 7 | % Emanuele Ruffaldi 2016-2017 8 | function [Y,L] = svdsqrt(X) 9 | 10 | [~,S,V] = svd(X); 11 | R = V; 12 | S = sqrt(S); 13 | Y = R*S; 14 | L = diag(S); 15 | %disp('svd') 16 | %Y*Y'-X 17 | %disp('cholcov') 18 | %C=cholcov(X)'; 19 | %if isempty(C) == 0 20 | % C*C'-X 21 | %end 22 | 23 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeQuat.m: -------------------------------------------------------------------------------- 1 | % SO3 as quaternion with (xyz w) 2 | % 3 | % Emanuele Ruffaldi 2017 @ SSSA 4 | function m = makeQuat() 5 | 6 | 7 | m = []; 8 | m.type = {'Quat'}; 9 | m.inv = @(X) qconj(X); 10 | m.prod = @(X,Y) qmult(X,Y); 11 | m.delta = @qmdelta; 12 | m.step = @qmstep; 13 | m.meancov = @manimeancov; % default 14 | m.count = 1; 15 | m.group = 4; % as unitary quaternion 16 | m.alg = 3; 17 | m.pack = @(x) x; 18 | m.unpack = @(x) x; 19 | m.transport = @(X,t,Y) t; 20 | m.islie = 1; 21 | m.s = int_manisetup([],[],m); 22 | -------------------------------------------------------------------------------- /index.rst: -------------------------------------------------------------------------------- 1 | .. UKF Manifold documentation master file, created by 2 | sphinx-quickstart on Sun Jan 21 00:33:28 2018. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to UKF Manifold's documentation! 7 | ======================================== 8 | 9 | .. toctree:: 10 | :maxdepth: 2 11 | :caption: Contents: 12 | 13 | 14 | 15 | Indices and tables 16 | ================== 17 | 18 | * :ref:`genindex` 19 | * :ref:`modindex` 20 | * :ref:`search` 21 | -------------------------------------------------------------------------------- /tests/testpackunpack.m: -------------------------------------------------------------------------------- 1 | %% 2 | mo = manisetup(makeProduct(makeSE3Mat())); 3 | 4 | zi = zeros(2,mo.group); 5 | zi(:) = 1:numel(zi); 6 | zu = maniunpack(mo,zi); 7 | zp = manipack(mo,zu); 8 | size(zu) 9 | size(zp) 10 | 11 | 12 | %% 13 | mo = manisetup(makeproduct(makeRot(),makeRot())); 14 | 15 | zi = zeros(2,mo.group); 16 | zi(:) = 1:numel(zi); 17 | zu = maniunpack(mo,zi); 18 | zp = manipack(mo,zu); 19 | size(zu) 20 | size(zp) 21 | 22 | zai = zeros(2,mo.alg); 23 | zai(:) = 1:numel(zai); 24 | zau = maniunpackalg(mo,zai); 25 | zap = manipackalg(mo,zau); 26 | size(zau) 27 | size(zap) -------------------------------------------------------------------------------- /ukfmani/coreclass/AManifold.m: -------------------------------------------------------------------------------- 1 | classdef AManifold 2 | %AMANIFOLD Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | G 7 | a 8 | count 9 | end 10 | 11 | methods (Abstract) 12 | y = step(obj,x,v); 13 | v = delta(obj,x,y); 14 | % given [n,C] cell array emits [n,G] array 15 | xvals = pack(obj,xcells); 16 | % given [n,G] array emits [n,C] cell 17 | xcells = unpack(obj,xvals); 18 | 19 | b = islie(obj); 20 | end 21 | end 22 | 23 | -------------------------------------------------------------------------------- /ukfmani/coreclass/next/AManifoldProject.m: -------------------------------------------------------------------------------- 1 | classdef AManifoldProject < AManifold 2 | %AMANIFOLDPROJECT Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | n 7 | other 8 | end 9 | 10 | methods 11 | function obj = AManifoldProject(otherManifold,T,k) 12 | obj.other = otherManifold; 13 | obj.n = n 14 | obj.k = k; 15 | obj.T = T; 16 | end 17 | 18 | function outputArg = method1(obj,inputArg) 19 | end 20 | end 21 | end 22 | 23 | -------------------------------------------------------------------------------- /ukfmani/core/manifolds.md: -------------------------------------------------------------------------------- 1 | 2 | Basic manifolds: 3 | 4 | - makeRn: euclidean R^n 5 | - makeQuat: SO3 quaternion 6 | - makeRot: SO3 matrix 3x3 7 | 8 | Operations: 9 | - makeProduct(m1...mK): combines the manifolds 10 | - makePower(m,n): takes the manifold m and replicates n times: 11 | 12 | In Progress: 13 | - makeSE3Mat: SE3 as matrix 4x4 14 | - makeQuat1: SO3 constrained to 1 axis of rotation, quaternion 15 | 16 | TODO 17 | - makeRot1: as above but with 3x3 matrix 18 | - makePositive: positive scalar (very specific case of positive definite matrices) 19 | - makeSE3Quat: SE3 as quaternion and position 20 | 21 | -------------------------------------------------------------------------------- /ukfmani/core/maniunpack.m: -------------------------------------------------------------------------------- 1 | % takes a model(C,G,A) m and a packed group x (matrix NxG) and builds a cell array 2 | % (NxC) 3 | % Emanuele Ruffaldi 2017 @ SSSA 4 | function c = maniunpack(m,x) 5 | 6 | 7 | assert(isfield(m,'s'),'missing setup, use manisetup(m)'); 8 | if length(m.s) == 1 9 | c = m.unpack(x')'; % transposed because of convention 10 | else 11 | c = cell(size(x,1),m.count); 12 | s = m.s; 13 | 14 | for I=1:length(s) 15 | sm = s(I).model.unpack; 16 | for J=1:size(x,1) 17 | c{J,I} = sm(x(J,s(I).group(1):s(I).group(2))'); 18 | end 19 | end 20 | end 21 | -------------------------------------------------------------------------------- /ukfmani/quaternions/test/Contents.m: -------------------------------------------------------------------------------- 1 | % Unit Tests for the Quaternion Toolbox 2 | % 3 | % NOTE: You need the test_tools toolbox to run these tests. It should be 4 | % available from the same place this toolbox was obtained. 5 | % 6 | % This script runs all the unit tests and records the output in test.out: 7 | % 8 | % unit_test 9 | % 10 | % The individual unit tests are just the function name preceeded by 11 | % "test_" 12 | 13 | 14 | % $Source: /home/stpierre/cvsroot/matlab/tools/quaternions/test/Contents.m,v $ 15 | % $Revision: 1.4 $ 16 | % $Date: 2009-07-24 19:14:44 $ 17 | 18 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 19 | -------------------------------------------------------------------------------- /ukfmani/core/manipack.m: -------------------------------------------------------------------------------- 1 | % takes a model m and cell array and builds a paclged group 2 | % Emanuele Ruffaldi 2017 @ SSSA 3 | function y = manipack(m,c) 4 | 5 | assert(isfield(m,'s'),'missing setup, use manisetup(m)'); 6 | y = zeros(size(c,1),m.group); 7 | if length(m.s) == 1 8 | assert(iscell(c)); 9 | assert(size(c,2)==m.count); % [N,C] 10 | for J=1:size(c,1) 11 | y(J,:) = m.pack(c(J,:)); 12 | end 13 | assert(size(y,2)==m.group); % [N,G] 14 | else 15 | s = m.s; 16 | 17 | for I=1:length(s) 18 | for J=1:size(c,1) 19 | y(J,s(I).group(1):s(I).group(2)) = s(I).model.pack(c{J,I}); 20 | end 21 | end 22 | end 23 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | SPHINXPROJ = UKFManifold 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) -------------------------------------------------------------------------------- /ukfmani/coreclass/next/AManifoldSO3.m: -------------------------------------------------------------------------------- 1 | classdef AManifoldSO3 < AManifoldLie 2 | %AMANIFOLDSO3 Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | 7 | end 8 | 9 | methods 10 | function obj = AManifoldSO3() 11 | end 12 | 13 | function outputArg = product(obj,inputArg) 14 | end 15 | 16 | function outputArg = inv(obj,inputArg) 17 | end 18 | 19 | function outputArg = exo(obj,inputArg) 20 | end 21 | 22 | function outputArg = log(obj,inputArg) 23 | end 24 | end 25 | end 26 | 27 | -------------------------------------------------------------------------------- /ukfmani/coreclass/AManifoldLie.m: -------------------------------------------------------------------------------- 1 | classdef AManifoldLie < AManifold 2 | %AMANIFOLD Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | end 7 | 8 | methods 9 | function Y = step(obj,X,v) 10 | Y = X*obj.exp(v); 11 | end 12 | 13 | function Y = delta(obj,X1,X2) 14 | Y = obj.log(X1*obj.inv(X2)); 15 | end 16 | 17 | function b = islie(obj) 18 | b = true; 19 | end 20 | end 21 | 22 | methods(Abstract) 23 | v = log(obj,x); 24 | x = exp(obj,v); 25 | z = prod(obj,x,y); 26 | y = inv(obj,x); 27 | end 28 | 29 | end -------------------------------------------------------------------------------- /ukfmani/coreclass/AManifoldWrapLie.m: -------------------------------------------------------------------------------- 1 | classdef AManifoldWrapLie < AManifoldWrap 2 | %AMANIFOLDWRAP Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | end 7 | 8 | methods 9 | function obj = AManifoldWrapLie(fx) 10 | obj@AManifoldWrap(fx); 11 | end 12 | 13 | function b = islie(obj) 14 | b = true; 15 | end 16 | 17 | function v = log(obj,x) 18 | end 19 | 20 | function x = exp(obj,v) 21 | end 22 | 23 | function z = prod(obj,x,y) 24 | end 25 | 26 | function y = inv(obj,x) 27 | end 28 | 29 | end 30 | end 31 | 32 | -------------------------------------------------------------------------------- /ukfmani/coreclass/next/AManifoldK.m: -------------------------------------------------------------------------------- 1 | classdef AManifoldK 2 | %AMANIFOLDK Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | Property1 7 | end 8 | 9 | methods 10 | function obj = AManifoldK(inputArg1,inputArg2) 11 | %AMANIFOLDK Construct an instance of this class 12 | % Detailed explanation goes here 13 | obj.Property1 = inputArg1 + inputArg2; 14 | end 15 | 16 | function outputArg = method1(obj,inputArg) 17 | %METHOD1 Summary of this method goes here 18 | % Detailed explanation goes here 19 | outputArg = obj.Property1 + inputArg; 20 | end 21 | end 22 | end 23 | 24 | -------------------------------------------------------------------------------- /ukfmani/coreclass/next/AManifoldR.m: -------------------------------------------------------------------------------- 1 | classdef AManifoldR 2 | %AMANIFOLDR Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | Property1 7 | end 8 | 9 | methods 10 | function obj = AManifoldR(inputArg1,inputArg2) 11 | %AMANIFOLDR Construct an instance of this class 12 | % Detailed explanation goes here 13 | obj.Property1 = inputArg1 + inputArg2; 14 | end 15 | 16 | function outputArg = method1(obj,inputArg) 17 | %METHOD1 Summary of this method goes here 18 | % Detailed explanation goes here 19 | outputArg = obj.Property1 + inputArg; 20 | end 21 | end 22 | end 23 | 24 | -------------------------------------------------------------------------------- /ukfmani/coreclass/next/AManifoldSE3.m: -------------------------------------------------------------------------------- 1 | classdef AManifoldSE3 2 | %AMANIFOLDSE3 Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | Property1 7 | end 8 | 9 | methods 10 | function obj = AManifoldSE3(inputArg1,inputArg2) 11 | %AMANIFOLDSE3 Construct an instance of this class 12 | % Detailed explanation goes here 13 | obj.Property1 = inputArg1 + inputArg2; 14 | end 15 | 16 | function outputArg = method1(obj,inputArg) 17 | %METHOD1 Summary of this method goes here 18 | % Detailed explanation goes here 19 | outputArg = obj.Property1 + inputArg; 20 | end 21 | end 22 | end 23 | 24 | -------------------------------------------------------------------------------- /ukfmani/coreclass/next/AManifoldProduct.m: -------------------------------------------------------------------------------- 1 | classdef AManifoldProduct 2 | %AMANIFOLDPRODUCT Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | Property1 7 | end 8 | 9 | methods 10 | function obj = AManifoldProduct(inputArg1,inputArg2) 11 | %AMANIFOLDPRODUCT Construct an instance of this class 12 | % Detailed explanation goes here 13 | obj.Property1 = inputArg1 + inputArg2; 14 | end 15 | 16 | function outputArg = method1(obj,inputArg) 17 | %METHOD1 Summary of this method goes here 18 | % Detailed explanation goes here 19 | outputArg = obj.Property1 + inputArg; 20 | end 21 | end 22 | end 23 | 24 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeRot.m: -------------------------------------------------------------------------------- 1 | % SO3 as matrix 2 | % 3 | % Emanuele Ruffaldi 2017 @ SSSA 4 | function m = makeRot() 5 | 6 | 7 | m = []; 8 | m.type = {'Rot'}; 9 | m.inv = @(X) flatten(unflatten(X)'); 10 | m.prod = @(X,Y) flatten(unflatten(X)*unflatten(Y)); 11 | m.log = @(X) so3log(unflatten(X)); 12 | m.exp = @(x) flatten(so3exp(x)); 13 | m.delta = @(X,Y) so3log(unflatten(X)*unflatten(Y)'); 14 | m.step = @(X,y) flatten(so3exp(y)*unflatten(X)); 15 | m.meancov = @manimeancov; 16 | m.count = 1; 17 | m.transport = @(X,t,Y) t; 18 | m.group = 9; % as matrix 19 | m.alg = 3; 20 | m.pack = @(x) x(:)'; 21 | m.unpack = @(x) reshape(x,3,3); 22 | m.islie = 1; 23 | m.s = int_manisetup([],[],m); 24 | 25 | 26 | function y = flatten(x) 27 | y = x(:)'; 28 | 29 | function y = unflatten(x) 30 | y = reshape(x,3,3); 31 | -------------------------------------------------------------------------------- /testSE3.m: -------------------------------------------------------------------------------- 1 | 2 | moL = makeSE3MatLocal(); 3 | moG = makeSE3MatGlobal(); 4 | 5 | % note that moL.exp == moG.exp 6 | m1 = moL.exp([1,0,2,0,3,4]); 7 | m2 = moL.exp([0,0,1,5,0,0]); 8 | mx = cellget(moL.unpack(m1),1)*cellget(moL.unpack(m2),1); % W..L1 L1..L2 9 | 10 | aG = moG.delta(mx,m2); % so that: exp(aG)*m2 == mx ==> exp(aG) = mx*inv(m2) 11 | m1G = moG.step(m2,aG); 12 | 13 | 14 | aL = moL.delta(mx,m2); % so that: m2*exp(aL) == mx ==> exp(aL) = inv(m2)*mx 15 | m1L = moL.step(m2,aL); 16 | m2p = cellget(moL.unpack(m2),1); 17 | m1p = cellget(moL.unpack(m1),1); 18 | m1Lp = cellget(moL.unpack(m1L),1); 19 | m1Gp = cellget(moL.unpack(m1G),1); 20 | aLG = (se3adj(m2p)*aL')'; 21 | m1p 22 | m1Lp 23 | m1Gp 24 | 25 | aL 26 | aG 27 | aLG 28 | 29 | 30 | %% 31 | se3adj(cellget(moL.unpack(moL.exp([0,0,0, 0,0,0])),1))*[0,0,0,0,2,0]' 32 | -------------------------------------------------------------------------------- /ukfmani/quaternions/qvqc.m: -------------------------------------------------------------------------------- 1 | function v_out=qvqc(q,v) 2 | % QVQc(Q,V) performs the operation Q*V*qconj(Q) 3 | % where the vector is treated as a quaternion with a scalar element of 4 | % zero. 5 | % 6 | % Q and V can be vectors of quaternions and vectors, but they must 7 | % either be the same length or one of them must have a length of one. 8 | % The output will have the same shape as V. Q will be passed through 9 | % QNORM to ensure it is normalized. 10 | % 11 | % See also QcQV, QNORM 12 | 13 | % Release: $Name: quaternions-1_3 $ 14 | % $Revision: 1.1 $ 15 | % $Date: 2009-07-24 19:14:44 $ 16 | 17 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 18 | 19 | 20 | if nargin~=2 21 | error('Two input arguments required'); 22 | else 23 | q = qconj(q); 24 | v_out = qcvq(q, v); 25 | end 26 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeRn.m: -------------------------------------------------------------------------------- 1 | % euclidean 2 | % 3 | % Emanuele Ruffaldi 2017 @ SSSA 4 | function m = makeRn(n) 5 | 6 | 7 | m = []; 8 | m.type = {'Rn',n}; 9 | m.fastinv = 1; 10 | m.inv = @(x) -x; 11 | %m.prod = @(x,y) x+y; 12 | m.prod = @makecomp; 13 | m.log = @(x) x; 14 | m.exp = @(x) x(:)'; 15 | m.meancov = @meancov; 16 | m.delta = @(x,y) x-y; 17 | m.step = @(x,y) x+y; 18 | m.group = n; 19 | m.alg = n; 20 | m.transport = @(X,t,Y) t; 21 | m.count = 1; 22 | %m.pack = @(x) x; 23 | m.pack = @mpack; 24 | m.unpack = @(x) x; 25 | m.islie = 1; 26 | m.s = int_manisetup([],[],m); 27 | 28 | 29 | % added test functions mpack, makecomp 30 | function p = mpack(x) 31 | if iscell(x) 32 | p = reshape(x{1},[],1); 33 | else 34 | p = x; 35 | end 36 | 37 | function c = makecomp(x, y) 38 | c = x + y; 39 | 40 | function [m,C] = meancov(x) 41 | 42 | m = mean(x); 43 | C = cov(x); 44 | -------------------------------------------------------------------------------- /ukfmani/quaternions/qvrot.m: -------------------------------------------------------------------------------- 1 | function v_out=qvrot(q,v) 2 | % QVROT(Q,V) rotates the vector V using the quaternion Q. 3 | % Specifically performs the operation Q*V*qconj(Q), where the vector 4 | % is treated as a quaternion with a scalar element of zero. 5 | % 6 | % Q and V can be vectors of quaternions and vectors, but they must 7 | % either be the same length or one of them must have a length of one. 8 | % The output will have the same shape as V. Q will be passed through 9 | % QNORM to ensure it is normalized. 10 | % 11 | % See also QVQc, QVXFORM. 12 | 13 | % Release: $Name: quaternions-1_3 $ 14 | % $Revision: 1.8 $ 15 | % $Date: 2009-07-24 19:14:44 $ 16 | 17 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 18 | 19 | if nargin~=2 20 | error('Two input arguments required'); 21 | else 22 | v_out = qvqc(q, v); 23 | end 24 | -------------------------------------------------------------------------------- /ukfmani/quaternions/qvxform.m: -------------------------------------------------------------------------------- 1 | function v_out=qvxform(q,v) 2 | % QVXFORM(Q,V) transforms the vector V using the quaternion Q. 3 | % Specifically performs the operation qconj(Q)*V*Q, where the vector 4 | % is treated as a quaternion with a scalar element of zero. 5 | % 6 | % Q and V can be vectors of quaternions and vectors, but they must 7 | % either be the same length or one of them must have a length of one. 8 | % The output will have the same shape as V. Q will be passed through 9 | % QNORM to ensure it is normalized. 10 | % 11 | % See also QcQV, QVROT. 12 | 13 | % Release: $Name: quaternions-1_3 $ 14 | % $Revision: 1.14 $ 15 | % $Date: 2009-07-24 19:14:44 $ 16 | 17 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 18 | 19 | if nargin~=2 20 | error('Two input arguments required'); 21 | else 22 | v_out = qcvq(q, v); 23 | end -------------------------------------------------------------------------------- /ukfmani/core/int_manisetup.m: -------------------------------------------------------------------------------- 1 | % builds structures for fast pack and unpack, also for nested manifolds 2 | % 3 | % pack/unpack group (cell NxC) <-> (matrix NxG) 4 | % pack/unpack alg (cell NxC) <-> (matrix NxA) 5 | % 6 | % Emanuele Ruffaldi 2017 @ SSSA 7 | function s = int_manisetup(group,alg,m) 8 | if isempty(group) 9 | group = 1; 10 | alg = 1; 11 | end 12 | if isfield(m,'models') 13 | s = []; 14 | for J=1:length(m.models) 15 | q = int_manisetup(group,alg,m.models{J}); 16 | if isempty(s) 17 | s = q; 18 | else 19 | s = [s, q]; 20 | end 21 | group = group + m.models{J}.group; 22 | alg = alg + m.models{J}.alg; 23 | end 24 | else 25 | s = struct('model',m,'group',group,'alg',alg); 26 | s.group = [group,group+m.group-1]; 27 | s.alg = [alg,alg+m.alg-1]; 28 | end -------------------------------------------------------------------------------- /ukfmani/manifolds/tbd_makePositive.m: -------------------------------------------------------------------------------- 1 | % positive value manifold 2 | % 3 | % http://www.ams.stonybrook.edu/~jias/AAAI17_camera_ready.pdf 4 | % http://proceedings.mlr.press/v37/huanga15.pdf 5 | % 6 | % deltaS(A,B) = log det((A+B)/2) - 1/2 log det(AB) 7 | % log det((A+B)/2)/sqrt(det(AB)) = log ((A+B)/2/sqrt(AB)) 8 | % GIVEN A and delta how to step it? 9 | % exp(d) = (A+B)/2/sqrt(AB) 10 | % 11 | % From manopt 12 | % ---------------------------- 13 | % symm = @(X) .5*(X+X'); 14 | % exponential 15 | % Y = symm(X*real(expm(X\(t*eta)))); 16 | % iff scalar = X*exp(t/X) 17 | % 18 | % logarithm: 19 | % H = symm(X*real(logm(X\Y))); 20 | % iff scalar = X*log(Y/X) 21 | % 22 | % eta = egrad2rgrad(X, eta) 23 | % eta = X*symm(eta)*X; 24 | % tangent2ambient = @(X, eta) eta; 25 | % proj = @(X, eta) symm(eta); 26 | function m = makePositive(n) 27 | 28 | error('not yet implemented') -------------------------------------------------------------------------------- /tests/testdeltastep.m: -------------------------------------------------------------------------------- 1 | mysetup('quaternions') 2 | %% 3 | m = makeQuat(); 4 | 5 | q0 = qomega2q([0.5,0.2,0]); 6 | w1 = [0.2,0,0]; 7 | qs = m.step(q0,w1); 8 | w2 = m.delta(qs,q0) 9 | w1-w2 10 | 11 | 12 | %% 13 | m = makeSE3Mat(); 14 | 15 | x0 = m.exp([0.5,0.2,0, 0.1,0,1]); % 4x4 16 | iix0 = m.inv(m.inv(x0)); 17 | lx0 = m.log(x0) 18 | 19 | w1 = [0.2,0,0, 0.0,0.0,0]; 20 | x1 = m.step(x0,w1); 21 | disp('With Rotation'); 22 | w2 = m.delta(x1,x0) 23 | w1-w2 24 | 25 | w1 = [0.0,0,0, 0.0,0.0,2]; 26 | x1 = m.step(x0,w1); 27 | disp('With Velocity'); 28 | w2 = m.delta(x1,x0) 29 | w1-w2 30 | 31 | w1 = [0.0,0.2,0.2, 0.0,0.0,2]; 32 | x1 = m.step(x0,w1); 33 | disp('Both'); 34 | w2 = m.delta(x1,x0) 35 | w1-w2 36 | 37 | %% 38 | mr = makeRot(); 39 | 40 | R0 = mr.exp([0.5,0.2,0]); 41 | w0 = mr.log(R0); 42 | R0u = det(mr.unpack(R0)); 43 | 44 | w1 = [0.2,0,0]; 45 | Rs = mr.step(R0,w1); 46 | w2 = mr.delta(Rs,R0) 47 | w1-w2 -------------------------------------------------------------------------------- /ukfmani/helpers/so3log.m: -------------------------------------------------------------------------------- 1 | function omega = so3log(R) 2 | 3 | theta = acos((trace(R)-1)/2); %acos(max(-1,min((trace(R)-1)/2,1))); 4 | if isreal(theta) == 0 5 | R = R/abs(det(R)); 6 | theta = acos((trace(R)-1)/2); 7 | end 8 | 9 | if abs(theta) < 1e-10 10 | B = 0.5; 11 | SO = (1/(2))*(R-R'); % =skew(omega) 12 | iV = eye(3); % use directly skew of omega 13 | else 14 | A = sin(theta)/theta; 15 | B = (1-cos(theta))/(theta*theta); 16 | SO = (1/(2*A))*(R-R'); % =skew(omega) 17 | %?? 18 | % syms x real 19 | % A = sin(x)/x 20 | % B= (1-cos(x))/(x*x) 21 | % Q=1/(x*x)*(1 - A/2/B) 22 | % taylor(Q,x,0) 23 | % x^4/30240 + x^2/720 + 1/12 24 | Q= 1/(theta^2)*(1 - A/2/B); 25 | iV = eye(3) - 1/2*SO + Q*SO^2; % use directly skew of omega 26 | end 27 | 28 | omega = [SO(3,2) SO(1,3) SO(2,1)]; 29 | 30 | -------------------------------------------------------------------------------- /ukfmani/core/manisigmas.m: -------------------------------------------------------------------------------- 1 | % computes sigmapoints of the manidolf using 2 | % 3 | % Inputs: 4 | % model 5 | % mean:mu 6 | % covariance: S 7 | % sigma weights: sigmainfo 8 | % 9 | % Outputs: 10 | % sigmapoints Chi 11 | % deltsa vChi 12 | % 13 | % TODO: modify convention to generate [G,N] instead of [N,G] 14 | % 15 | % Emanuele Ruffaldi 2017 @ SSSA 16 | function [Chi,vChi] = manisigmas(model,mu,S,sigmainfo) 17 | 18 | % decompose the covariance 19 | %C = cholcov(S); % TODO use svd 20 | C = sigmainfo.sqrt(S); 21 | if size(C,1) ~= size(S,1) 22 | C = eye(size(S,1)); 23 | end 24 | k = size(C,1); 25 | 26 | Chi = zeros(2*k+1,model.group); % flattened 27 | Chi(1,:) = mu; % not weighted 28 | vChi = zeros(2*k+1,model.alg); % delta 29 | 30 | c = sigmainfo.c; 31 | for I=1:k 32 | psi = c*C(I,:)'; % COLUMN 33 | vChi(I+1,:) = psi; 34 | vChi(I+1+k,:) = -psi; 35 | Chi(I+1,:) = model.step(mu,psi); 36 | Chi(I+1+k,:) = model.step(mu,-psi); 37 | end 38 | 39 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeQuat1.m: -------------------------------------------------------------------------------- 1 | % quaternion constrained to single axis 2 | % 3 | function m = makeQuat1(axis) 4 | 5 | axis = axis/norm(axis); 6 | 7 | m = []; 8 | m.type = {'Quat1',axis}; 9 | m.axis = axis; 10 | m.inv = @(X) qconj(X); 11 | m.prod = @(X,Y) qmult(X,Y); 12 | m.count = 1; 13 | m.group = 4; % as unitary quaternion 14 | m.alg = 1; 15 | m.pack = @(x) x; 16 | m.unpack = @(x) x; 17 | m.transport = @(X,t,Y) t; 18 | m.meancov = @manimeancov; % default 19 | m.step = @(X,y)mstep(m,X,y); 20 | m.delta = @(X,Y) mdelta(m,X,Y); 21 | m.islie = 1; 22 | m.s = int_manisetup([],[],m); 23 | 24 | function q = mstep(m,X,y) 25 | 26 | if norm(y) == 0 27 | q = X; 28 | else 29 | q = qnorm(qmult(qomega2q(m.axis*y),X)); 30 | end 31 | 32 | function q = mdelta(m.X,Y) 33 | 34 | w = getomega(qmult(X,qconj(Y))); % log(x*inv(y)) 35 | q = dot(w,m.axis); % project 36 | 37 | function w = getomega(q) 38 | 39 | [v,phi] = qdecomp(q); 40 | assert(isnan(phi) == 0) 41 | w = v*phi; 42 | w = w(:)'; -------------------------------------------------------------------------------- /task_headpose/headPose.py: -------------------------------------------------------------------------------- 1 | import csv 2 | import livestat 3 | import sys 4 | 5 | a = csv.DictReader(open(sys.argv[1] if len(sys.argv) > 1 else "headPose.txt","rb")) 6 | o = csv.DictWriter(open(sys.argv[2] if len(sys.argv) > 2 else "headPoseout.csv","wb"),fieldnames=[x.strip() for x in a.fieldnames]) 7 | o.writeheader() 8 | w = None 9 | for x in a: 10 | allfields = a.fieldnames 11 | asarray = [x[y] for y in allfields] 12 | # find bad field 13 | idx = allfields.index(" pose_Rx") 14 | # rebuild the dictionary 15 | asarray[idx] = asarray[idx].strip().split(" ") 16 | asarray = asarray[0:idx] + asarray[idx] + asarray[idx+1:] 17 | d = dict([(allfields[i].strip(),float(asarray[i])) for i in range(0,len(allfields))]) 18 | # create the statistics verifyier 19 | if w is None: 20 | w = dict([(k.strip(),livestat.LiveStat(k.strip())) for k in d.keys()]) 21 | 22 | # update the statistics 23 | for k in d.keys(): 24 | w[k].append(float(d[k])) 25 | 26 | print d["pose_Tx"],d["pose_Ty"],d["pose_Tz"],d["pose_Rx"],d["pose_Ry"],d["pose_Rz"] 27 | 28 | o.writerow(d) 29 | 30 | for k in sorted(w.keys()): 31 | print w[k] -------------------------------------------------------------------------------- /ukfmani/core/manismoother.m: -------------------------------------------------------------------------------- 1 | % 2 | % Kalman Smoorther using the following convention 3 | % 4 | % x(t) is the state variable 5 | % x(t+) f(x(t),A1,..An) is the process function that accepts x(t) and other 6 | % inputs, eventually noise 7 | % where Ai are time invariant variables with their distribution 8 | % 9 | % manifold mxa = mx + ma 10 | % 11 | % mxa = joint manifold (product manifold mandatory indexable) 12 | % x_manis = indices of manifold (typically the first k elements) 13 | % Xest = estimated 14 | % mean T,G_X 15 | % cov T,a_X,a_X 16 | % Xpred = prediction (optional) or built using f 17 | % mean T,G_X 18 | % cov T,a_X,a_X 19 | % A = constant associated to the manifold spec 20 | % mean 1,G_A 21 | % cov 1,a_A,a_A 22 | % 23 | % Emanuele Ruffaldi 2018 24 | function [A_mean,A_cov,x0_mean,x0_cov] = manismoother(mxa, x_manis,f, Xpred_mean, Xpred_cov, Xest_mean, Xest_cov, A_mean, A_cov) 25 | 26 | % compute Xpred if needed and store it 27 | % loop backward 28 | % compute intial value: x0_mean,x0_cov that are based 29 | % accumulate contributions for A 30 | % use last value to update x0 31 | % average the contributions for A -------------------------------------------------------------------------------- /ukfmani/coreclass/AManifoldWrap.m: -------------------------------------------------------------------------------- 1 | classdef AManifoldWrap < AManifold 2 | %AMANIFOLDWRAP Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | fx 7 | end 8 | 9 | methods 10 | function obj = AManifoldWrap(fx) 11 | obj.fx = fx; 12 | obj.G = fx.group; 13 | obj.a = fx.alg; 14 | end 15 | 16 | function b = islie(obj) 17 | end 18 | 19 | function y = step(obj,x,v) 20 | end 21 | function v = delta(obj,x,y) 22 | end 23 | 24 | % given [n,C] cell array emits [n,G] array 25 | function xvals = pack(obj,xcells) 26 | end 27 | % given [n,G] array emits [n,C] cell 28 | function xcells = unpack(obj,xvals) 29 | end 30 | end 31 | 32 | methods(Static) 33 | function e = wrap(model) 34 | if hasattr(model,'log') 35 | e = AManifoldWrapLie(model); 36 | else 37 | e = AManifoldWrap(model); 38 | end 39 | end 40 | end 41 | 42 | end 43 | 44 | -------------------------------------------------------------------------------- /ukfmani/core/Contents.m: -------------------------------------------------------------------------------- 1 | % CORE 2 | % 3 | % Files 4 | % manievalh - Evaluation of a function 5 | % manievalhsigma - Evaluation of a function 6 | % manifusion - given two estimates this operation performs the Covariance weighted 7 | % manifusionn - fusion 8 | % maniinfo - Infos from Manifold 9 | % manimeancov - given a manifold definition and a set of manifold data (packed) computes 10 | % manipack - takes a model m and cell array and builds a paclged group 11 | % manipackalg - takes a model m and cell array and builds a algebra 12 | % manisetup - builds structures for fast pack and unpack, also for nested manifolds 13 | % manisigmas - computes sigmapoints of the manidolf using 14 | % manismoother - Kalman Smoorther using the following convention 15 | % manistatestep - Emanuele Ruffaldi 2017 @ SSSA 16 | % maniukfupdate - Emanuele Ruffaldi 2017 @ SSSA 17 | % maniunpack - takes a model(C,G,A) m and a packed group x (matrix NxG) and builds a cell array 18 | % maniunpackalg - takes a model(C,G,A) m and a packed group x (matrix NxG) and builds a cell array 19 | % maniunsigma - recomposes the sigma points 20 | 21 | -------------------------------------------------------------------------------- /ukfmani/helpers/so3exp.m: -------------------------------------------------------------------------------- 1 | % Emanuele Ruffaldi 2017 @ SSSA 2 | function R = so3exp(omega) 3 | 4 | theta = norm(omega); 5 | if theta < 1e-12 6 | % Original 7 | % A = 1; 8 | % B = 0.5; 9 | % C = 1/6; 10 | % S = zeros(3); 11 | % R = eye(3) + A*S + B*S^2; 12 | % V = eye(3) + B*S + C*S^2; 13 | 14 | N = 10; 15 | R = eye(3); 16 | xM = eye(3); 17 | cmPhi = skew(omega); 18 | for n = 1:N 19 | xM = xM * (cmPhi / n); 20 | R = R + xM; 21 | end 22 | 23 | % Project the resulting rotation matrix back onto SO(3) 24 | R = R / sqrtm( R'*R ) ; 25 | 26 | else 27 | %Original 28 | A = sin(theta)/theta; 29 | B = (1-cos(theta))/(theta^2); 30 | C = (theta-sin(theta))/(theta^3); 31 | S = skew(omega); 32 | R = eye(3) + A*S + B*S^2; 33 | %V = eye(3) + B*S + C*S^2; 34 | 35 | %Barfoot 36 | if 0==1 37 | axis = omega/theta; 38 | cp = cos(theta); 39 | sp = sin(theta); 40 | sa = skew(axis); 41 | 42 | R = cp*eye(3) + (1-cp)*axis*(axis') + sp*sa; 43 | end 44 | assert(abs(det(R)-1) < 1e-5,'unitary'); 45 | end 46 | -------------------------------------------------------------------------------- /ukfmani/quaternions/test/unit_test.m: -------------------------------------------------------------------------------- 1 | %UNIT_TEST runs unit tests for the Quaternions toolbox. 2 | 3 | % Release: $Name: quaternions-1_3 $ 4 | % $Revision: 1.8 $ 5 | % $Date: 2009-07-24 19:14:44 $ 6 | 7 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 8 | 9 | if ~exist('check_value','file') 10 | error(['You do not appear to have the test_tools toolbox installed,', 10,... 11 | 'which is necessary to run these unit tests. You should be', 10,... 12 | 'able to get the test_tools toolbox from the same place you', 10,... 13 | 'obtained this toolbox.']); 14 | end 15 | 16 | unix('rm -f test.out') 17 | diary test.out 18 | 19 | Failures=0; 20 | 21 | test_isq; Failures=Failures+failures; 22 | test_isnormq; Failures=Failures+failures; 23 | 24 | test_qconj; Failures=Failures+failures; 25 | test_qnorm; Failures=Failures+failures; 26 | test_qmult; Failures=Failures+failures; 27 | 28 | test_qdecomp; Failures=Failures+failures; 29 | 30 | test_qcvq; Failures=Failures+failures; 31 | test_qvqc; Failures=Failures+failures; 32 | 33 | test_q2dcm; Failures=Failures+failures; 34 | test_dcm2q; Failures=Failures+failures; 35 | 36 | disp_num_failures('All Unit Tests', Failures) 37 | 38 | diary off 39 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeSE3Quat.m: -------------------------------------------------------------------------------- 1 | % SE3 to be tested, alternatively use makeCom(makeRot(),makeRn(3)) 2 | % 3 | % Emanuele Ruffaldi 2017 @ SSSA 4 | function m = makeSE3Quat() 5 | 6 | m = []; 7 | m.type = {'SE3Quat'}; 8 | m.inv = @se3qinv; 9 | m.prod = @se3qmul; 10 | m.log = @se3qlog; 11 | m.exp = @se3qexp; 12 | m.delta = @se3qdelta; 13 | m.step = @seqstep; 14 | m.pack = @(x) x; 15 | m.unpack = @(x) x; 16 | m.transport = @(X,t,Y) t; 17 | m.group = 7; 18 | m.alg = 6; 19 | m.count = 1; 20 | m.islie = 1; 21 | m.s = int_manisetup([],[],m); 22 | 23 | function y = se3qdelta(x,y) 24 | error('not implemented'); 25 | 26 | function y = se3qstep(x,y) 27 | error('not implemented'); 28 | 29 | function y = se3qlog(x) 30 | error('not implemented'); 31 | 32 | function y = se3qexp(x) 33 | error('not implemented'); 34 | 35 | function y = se3qmul(x,y) 36 | 37 | error('not implemented'); 38 | 39 | function y = se3qinv(x) 40 | 41 | qc = qconj(x(1:4)); 42 | y = [qc, - qvqform(x(5:7))]; 43 | 44 | function u = ubuild2(quat,pos) 45 | 46 | u = [quat(:);pos(:)]'; 47 | 48 | function q = getquat(x) 49 | 50 | q = x(1:4); 51 | 52 | function p = getpos(x) 53 | 54 | p = x(5:7); 55 | 56 | function u = getomega(x) 57 | 58 | u = x(1:3); 59 | 60 | function u = getvel(x) 61 | 62 | u = x(4:6); -------------------------------------------------------------------------------- /examples/testmakegen.m: -------------------------------------------------------------------------------- 1 | clear all 2 | 3 | if exist('manpippo.m','file') 4 | delete('manpippo.m'); % remove the fil 5 | end 6 | q = {makeQuat(),makeRn(3),makeRot()}; 7 | mout = makeGenProduct('pippo',q{:}); 8 | %,makeSE3Mat(),makeSE3Mat()); 9 | 10 | allinputs = rand(mout.group,100,2); 11 | tic 12 | s = 0; 13 | for I=1:100 14 | z1 = allinputs(:,I,1); 15 | z1c = mout.unpack(z1); 16 | z1c{1}= [1,0,0,0]; 17 | z1 = mout.pack(z1c); 18 | z2 = allinputs(:,I,2); 19 | z2c = mout.unpack(z2); 20 | z2c{1} = z2c{1}/norm(z2c{1}); 21 | z2 = mout.pack(z2c); 22 | z12 = mout.prod(z1,z2); 23 | z12d = mout.delta(z12,z2); 24 | z12s = mout.step(z1,z12d); 25 | s = s+sum(z12s); 26 | end 27 | w =toc; 28 | disp(sprintf('gen %f time %f',s,w)) 29 | 30 | 31 | mout = makeProduct(q{:}); 32 | tic 33 | s = 0; 34 | for I=1:100 35 | z1 = allinputs(:,I,1); 36 | z1c = mout.unpack(z1); 37 | z1c{1}= [1,0,0,0]; 38 | z1 = mout.pack(z1c); 39 | z2 = allinputs(:,I,2); 40 | z2c = mout.unpack(z2); 41 | z2c{1} = z2c{1}/norm(z2c{1}); 42 | z2 = mout.pack(z2c); 43 | z12 = mout.prod(z1,z2); 44 | z12d = mout.delta(z12,z2); 45 | z12s = mout.step(z1,z12d); 46 | s = s+sum(z12s); 47 | end 48 | w2 =toc; 49 | disp(sprintf('nongen %f time %f',s,w2)) 50 | disp(sprintf('non generated is %f%% slower',(w2-w)/w*100)); 51 | 52 | %Z = mout.log(z); 53 | %Zz = mout.exp(Z); -------------------------------------------------------------------------------- /tests/testmeancovquat.m: -------------------------------------------------------------------------------- 1 | addpath quaternions 2 | 3 | %% 4 | isquat = 0; 5 | 6 | if isquat 7 | mz = manisetup(makeQuat()); 8 | else 9 | mz = manisetup(makeRot()); 10 | end 11 | 12 | mu0q = qomega2q([2,0,0]); 13 | S0 = 8*[0.5 0 0.1; 0 0.2 0; 0.1 0 0.3]; 14 | wq = sampleQuats(1000,mu0q,S0); % generate 100 samples 15 | if isquat == 0 16 | %w = arrayfun(@(x) reshape(q2dcm(w(x,:)),1,[]),1:size(wp,1),'UniformOutput','false'); 17 | w = zeros(size(wq,1),9); 18 | for I=1:size(wq,1) 19 | w(I,:) = reshape(q2dcm(wq(I,:)),1,[]); 20 | end 21 | mu0 = q2dcm(mu0q); 22 | else 23 | w = wq; 24 | mu0 = mu0q; 25 | end 26 | 27 | [muo,So,nv] = manimeancov(mz,w,20); % iterate 15 steps 28 | if isquat == 0 29 | muo = reshape(muo,3,3); 30 | muoq = dcm2q(muo); 31 | else 32 | muoq = muo; 33 | end 34 | deltamuo0 = quatdiff(muoq,mu0q) 35 | 36 | S0 37 | So 38 | 39 | mu0 40 | muo 41 | 42 | subplot(3,1,1); 43 | plot(nv(:,1)); 44 | xlabel('Ireration'); 45 | ylabel('Norm of step'); 46 | set(gca,'XTick',1:size(nv,1)); 47 | subplot(3,1,2); 48 | plot(nv(:,2)); 49 | xlabel('Ireration'); 50 | ylabel('Residual Max'); 51 | set(gca,'XTick',1:size(nv,1)); 52 | 53 | subplot(3,1,3); 54 | plot(nv(:,3)); 55 | xlabel('Ireration'); 56 | ylabel('Residual Mean'); 57 | set(gca,'XTick',1:size(nv,1)); 58 | 59 | -------------------------------------------------------------------------------- /ukfmani/quaternions/qconj.m: -------------------------------------------------------------------------------- 1 | function qout=qconj(qin) 2 | % QCONJ(Q) calculates the conjugate of the quaternion Q. 3 | % Works on "vectors" of quaterions as well. Will return the same shape 4 | % vector as input. If input is a vector of four quaternions, QCONJ will 5 | % determine whether the quaternions are row or column vectors according 6 | % to ISQ. 7 | % 8 | % See also ISQ. 9 | 10 | % Release: $Name: quaternions-1_3 $ 11 | % $Revision: 1.16 $ 12 | % $Date: 2009-07-26 20:05:12 $ 13 | 14 | % Copyright (c) 2001-2009, Jay A. St. Pierre. All rights reserved. 15 | 16 | 17 | if nargin~=1 18 | error('qconj() requires one input argument'); 19 | else 20 | qtype = isq(qin); 21 | if ( qtype==0 ) 22 | error(... 23 | 'Invalid input: must be a quaternion or a vector of quaternions') 24 | elseif ( qtype==3 ) 25 | warning(... 26 | 'qconj:indeterminateShape', ... 27 | 'Component quaternion shape indeterminate, assuming row vectors') 28 | end 29 | end 30 | 31 | % Make sure component quaternions are row vectors 32 | if( qtype == 1 ) 33 | qin=qin.'; 34 | end 35 | 36 | qout(:,1)=-qin(:,1); 37 | qout(:,2)=-qin(:,2); 38 | qout(:,3)=-qin(:,3); 39 | qout(:,4)= qin(:,4); 40 | 41 | % Make sure output is same shape as input 42 | if( qtype == 1 ) 43 | qout=qout.'; 44 | end 45 | -------------------------------------------------------------------------------- /ukfmani/quaternions/qnorm.m: -------------------------------------------------------------------------------- 1 | function qout=qnorm(qin) 2 | % QNORM(Q) normalizes quaternions. 3 | % Works on vectors of quaternions too. If input is a vector of four 4 | % quaternions, QNORM will determine whether the quaternions are row or 5 | % column vectors according to ISQ. 6 | % 7 | % See also ISQ. 8 | 9 | % Release: $Name: quaternions-1_3 $ 10 | % $Revision: 1.11 $ 11 | % $Date: 2009-07-26 20:05:12 $ 12 | 13 | % Copyright (c) 2001-2009, Jay A. St. Pierre. All rights reserved. 14 | 15 | 16 | if nargin~=1 17 | error('qnorm() requires one input argument'); 18 | else 19 | qtype = isq(qin); 20 | if ( qtype == 0 ) 21 | error(['Invalid input: must be a quaternion or a vector of' ... 22 | ' quaternions']) 23 | elseif ( qtype==3 ) 24 | warning(... 25 | 'qnorm:indeterminateShape', ... 26 | 'Component quaternion shape indeterminate, assuming row vectors') 27 | end 28 | end 29 | 30 | 31 | % Make sure qin is a column of quaternions 32 | if( qtype == 1 ) 33 | qin=qin.'; 34 | end 35 | 36 | % Find the magnitude of each quaternion 37 | qmag=sqrt(sum(qin.^2,2)); 38 | 39 | % Make qmag the same size a q 40 | qmag=[qmag qmag qmag qmag]; 41 | 42 | % Divide each element of q by appropriate qmag 43 | qout=qin./qmag; 44 | 45 | % Make sure output is same shape as input 46 | if( qtype == 1 ) 47 | qout=qout.'; 48 | end 49 | -------------------------------------------------------------------------------- /ukfmani/helpers/ut_mweights2.m: -------------------------------------------------------------------------------- 1 | % n - Dimensionality of the group (if needed9 2 | % k - Dimensionality of the tangent space 3 | % alpha - Transformation parameter (optional, default 0.5) 4 | % beta - Transformation parameter (optional, default 2) 5 | % kappa - Transformation parameter (optional, default 3-n) 6 | % Copyright (C) 2006 Simo Sarkka 7 | % 8 | % Modified Emanele Ruffaldi 2014 9 | function [wei] = ut_weights2(n,k,alpha,beta,kappa) 10 | if nargin < 3 11 | alpha = 0.5; 12 | end 13 | if nargin < 4 14 | beta = 2; 15 | end 16 | if nargin < 5 17 | kappa = 3-k; 18 | end 19 | 20 | % mean: lambda/(k+lambda) 21 | % others: 1/(2*(k+lambda)) 22 | % 23 | % 2*k / (2*(k+lambda)) + lambda /(k+lambda) 24 | % (k / (k+lambda) + lambda/ (k+lambda)) 25 | % (k+lambda) / (k+lambda) == 1 26 | 27 | c = alpha^2*(k+kappa); 28 | lambda = c-k; % automatic lambda 29 | 30 | WM = repmat(1/(2*(k+lambda)), 2*k+1,1); % except first 31 | WM(1) = lambda / (k+lambda); % first is different 32 | 33 | WC = WM; 34 | WC(1) = WC(1) + (1-alpha^2+beta); % 35 | 36 | W0 = eye(length(WC)) - repmat(WM,1,length(WM)); 37 | W = W0*diag(WC)*W0'; 38 | 39 | % Original: 40 | % W0(c) = l/(n+l) + (1-aa+b) 41 | % Wi(c) = Wi(c) 42 | 43 | wei = []; 44 | wei.c = sqrt(c); 45 | wei.WM = WM; 46 | wei.W = W; 47 | wei.WC = diag(WC); % only considering differential, and not the mean 48 | wei.sqrt = @cholcov; 49 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeSPD.m: -------------------------------------------------------------------------------- 1 | % Symmetric Positive Definite 2 | % 3 | % Could be used for modeling covariance of Gaussians, but NOTE that a 4 | % typical prior of MVN Gaussians is the Normal-Wishart Distribution defined 5 | % by: 6 | % - mu0 7 | % - lambda 8 | % - W 9 | % - n dofs 10 | % 11 | % 12 | % 13 | % Emanuele Ruffaldi 2018 @ SSSA 14 | function m = makeSPD(n) 15 | 16 | 17 | m = []; 18 | m.type = {'SPD',n}; 19 | m.delta = @spd_delta; 20 | m.step = @spd_step; 21 | m.transport = @spd_transport; 22 | m.count = 1; 23 | m.group = n*n; % as matrix 24 | m.alg = n*(n+1)/2; 25 | m.pack = @(x) x(:)'; 26 | m.unpack = @(x) reshape(x,n,n); 27 | m.vpack = @vpack; 28 | m.vunpack = @(v) vunpack(v,n); 29 | m.islie = 0; 30 | m.s = int_manisetup([],[],m); 31 | 32 | function Y = symm(X) 33 | Y = 0.5*(X+X'); 34 | end 35 | 36 | function tp = vpack(t) 37 | a = triu(t); 38 | tp = a(:)'; 39 | end 40 | 41 | function t = vunpack(tp,n) 42 | t = zeros(n,n); % constant 43 | u = triu(true(n,n)); % constant 44 | t(u(:)) = tp; 45 | end 46 | 47 | function tpy = spd_transport(X,tpx,Y) 48 | 49 | % from manopt 50 | tx = vunpack(tpx); 51 | E = sqrtm((Y/X)); 52 | tpy = vpack(E*tx*E'); 53 | 54 | end 55 | 56 | function t = spd_delta(X,Y) 57 | t = vpack(symm(X*real(logm(X\Y)))); 58 | end 59 | 60 | function Y = spd_step(X,tp) 61 | t = vunpack(tp); 62 | Y = symm(X*real(expm(X\(t*eta)))); 63 | 64 | end 65 | 66 | end -------------------------------------------------------------------------------- /ukfmani/core/manifusionn.m: -------------------------------------------------------------------------------- 1 | % fusion 2 | function [X,C] = manifusionn(model,steps,mus,Cs) 3 | 4 | if isempty(steps) 5 | steps = 10; 6 | end 7 | 8 | assert(length(mus) > 0); 9 | assert(iscell(mus)); 10 | assert(iscell(Cs)); 11 | assert(length(mus) == length(Cs)); 12 | 13 | if length(Cs) == 1 14 | X = mus{1}; 15 | C = Cs{1}; 16 | return; 17 | elseif length(Cs) == 2 18 | [X,C] = manifusion(model,mus{1},Cs{1}); 19 | return; 20 | else 21 | C = zeros(size(Cs{1})); 22 | for I=1:length(C) 23 | C = C + inv(Cs{I}); 24 | end 25 | C = inv(C); 26 | Csi = cell(size(Cs)); 27 | for I=1:length(Csi) 28 | Csi{I} = C/Cs{I}; 29 | end 30 | 31 | 32 | steps = 20; 33 | N=length(mus); 34 | 35 | v = zeros(N,model.alg); % preallocated 36 | mz = mus{1}; % first is picked as mean 37 | 38 | % iterative 39 | for k=1:steps 40 | for i=1:N 41 | % same as: se3_logdelta but with igk once 42 | v(i,:) = Csi{i}*model.delta(mus{i},mz); % Csi{i}? 43 | end 44 | % mz = model.step(mz,mean(v,1)); 45 | mz = model.step(mz,mean(v,1)'); % averaging the weighted directions 46 | end 47 | X = mz; 48 | 49 | % update v for computing covariance 50 | for i=1:N 51 | % v(i,:) = Csi{i}*model.delta(mus(i,:),X); 52 | v(i,:) = Csi{i}*model.delta(mus{i},X); 53 | end 54 | 55 | C = v'*v; % covariance ZZ 56 | end 57 | -------------------------------------------------------------------------------- /ukfmani/quaternions/ChangeLog: -------------------------------------------------------------------------------- 1 | 2009-07-26 Jay A. St. Pierre 2 | 3 | * Version 1.3 released 4 | 5 | * Scrubbed mlint warnings 6 | 7 | 2009-07-24 Jay A. St. Pierre 8 | 9 | * Changed license from GPL to BSD for MATLAB Central posting. 10 | 11 | * Renamed qvxform and qvrot to qcvq and qvqc and aliased qvxform 12 | and qvrot to these functions. Added explanatory text and 13 | reference to Wertz. 14 | 15 | * Improved robustness of the dcm2q algorithm. Thanks to 16 | Tatsuki Kashitani for pointing out the problem. 17 | 18 | 2002-01-20 Jay A. St. Pierre 19 | 20 | * Version 1.2.2 released. 21 | 22 | * Contents.m, dcm2q.m, q2dcm.m, qvrot.m, qvxform.m: Improved help 23 | information. 24 | 25 | 2001-12-18 Jay A. St. Pierre 26 | 27 | * Version 1.2.1 released. 28 | 29 | * Added check for test_tools toolbox in unit_test.m 30 | 31 | 2001-05-01 Jay A. St. Pierre 32 | 33 | * Version 1.2 released. 34 | 35 | * qdecomp.m: Properly fixed behavior for input quaternions contain 36 | both zero and non zero rotation descriptions. Updated unit test 37 | to verify this. 38 | 39 | 2001-03-29 Jay A. St. Pierre 40 | 41 | * qdecomp.m: Fixed behavior for an input vector of quaternions 42 | where several of the component quaternions are [0 0 0 1]. 43 | 44 | -------------------------------------------------------------------------------- /examples/exampleRot.m: -------------------------------------------------------------------------------- 1 | % TODO: explain better the fact that angular velocity of the state is 2 | % GLOBAL rather than LOCAL 3 | 4 | % build input and output manifolds 5 | mx = manisetup(makeCom(makeRot(),makeRn(3))); 6 | mxr = makeRot(); % helper for the integration 7 | mz = manisetup(makeRot()); 8 | 9 | % initial state and noise definition 10 | x0 = mx.step(mx.pack({eye(3),[0,0,0]}),[pi/2,0.2,0, 0,0,0]); 11 | P0 = 0.5*eye(mx.alg); 12 | Q = 0.1*eye(mx.alg); % process noise 13 | R = 1e-3*eye(mz.alg); % measure noise 14 | zobs = @(x) mz.exp([pi/2,sin(x/100),0]); 15 | 16 | wsigmax = ut_mweights2(mx.group,mx.alg,0.5); 17 | wsigmax.sqrt = @svdsqrt; 18 | 19 | % observation is identity 20 | % process is the integral 21 | dt = 0.1; 22 | 23 | % integrate 24 | f_fx = @(qk,wk) deal(mxr.step(qk,dt*wk),wk); % f(rot,omega) -> (rot,omega) 25 | h_fx = @(qk,ok) qk; 26 | 27 | tic 28 | % loop 29 | deltas = zeros(200,mz.alg); 30 | states = zeros(size(deltas,1),mx.group); 31 | for L=1:size(deltas,1) 32 | states(L,:) = x0; 33 | 34 | [xp,Pp] = manistatestep(mx,x0,P0,f_fx,Q,wsigmax); 35 | [zm,Czz,Cxz] = manievalh(mx,mz,xp,Pp,h_fx,wsigmax); 36 | 37 | % Kalman update with observation noise (additive) 38 | Pvv = Czz + R; 39 | K = Cxz/Pvv; 40 | P0 = (eye(size(P0)) - K * Pvv * K') * P0; 41 | delta = mz.delta(zobs(L),zm); 42 | x0 = mx.step(xp,(K*delta')'); 43 | deltas(L,:) = delta; 44 | end 45 | toc 46 | figure(1) 47 | plot(deltas(10:end,:)) 48 | figure(3) 49 | plot(sum(deltas(10:end,:).^2,2)) 50 | 51 | figure(2) 52 | plot(states(10:end,:)) 53 | -------------------------------------------------------------------------------- /examples/exampleQuat.m: -------------------------------------------------------------------------------- 1 | % TODO: explain better the fact that angular velocity of the state is 2 | % GLOBAL rather than LOCAL 3 | % 4 | %http://it.mathworks.com/matlabcentral/fileexchange/1176-quaternion-toolbox 5 | %mysetup('quaternions'); 6 | % build input and output manifolds 7 | mx = manisetup(makeCom(makeQuat(),makeRn(3))); % quat and vel 8 | mz = manisetup(makeQuat()); 9 | 10 | % initial state and noise definition 11 | x0 = mx.step([0,0,0,1, 0,0,0],[pi/2,0.2,0, 0,0,0]); 12 | P0 = 0.5*eye(mx.alg); 13 | Q = 0.01*eye(mx.alg); % process noise 14 | R = 1e-3*eye(mz.alg); % measure noise 15 | zobs = qomega2q([pi/2,0,0]); 16 | 17 | wsigmax = ut_mweights2(mx.group,mx.alg,0.5); 18 | wsigmax.sqrt = @svdsqrt; 19 | 20 | % observation is identity 21 | % process is the integral 22 | dt = 0.1; 23 | 24 | % Process is the integral of the omega by time 25 | % Note: in the paper Kraft: they state qk q_noise q_vel 26 | f_fx = @(qk,ok) deal(qmult(qomega2q(dt*ok),qk),ok); 27 | h_fx = @(qk,ok) qk; 28 | 29 | % loop 30 | deltas = zeros(200,mz.alg); 31 | states = zeros(size(deltas,1),mx.group); 32 | tic 33 | for L=1:size(deltas,1) 34 | states(L,:) = x0; 35 | 36 | [xp,Pp] = manistatestep(mx,x0,P0,f_fx,Q,wsigmax); 37 | [zm,Czz,Cxz] = manievalh(mx,mz,xp,Pp,h_fx,wsigmax); 38 | 39 | % Kalman update with observation noise (additive) 40 | Pvv = Czz + R; 41 | K = Cxz/Pvv; 42 | P0 = (eye(size(P0)) - K * Pvv * K') * P0; 43 | delta = mz.delta(zobs,zm); 44 | x0 = mx.step(xp,(K*delta')'); 45 | deltas(L,:) = delta; 46 | end 47 | toc 48 | figure(1) 49 | plot(deltas(10:end,:)) 50 | figure(2) 51 | plot(states(10:end,:)) 52 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeGenProduct.m: -------------------------------------------------------------------------------- 1 | function mout = makeGenProduct(name,varargin) 2 | % produce a new output file called: man##name.m 3 | % 4 | % that implements makeProduct of the above spec 5 | % 6 | % Supported: 7 | % - Rot Rot1 SE3Mat SE3Quat Quat Quat1 Rn 8 | % - Product ... 9 | % 10 | if nargin == 1 11 | mout = varargin{1}; 12 | return; 13 | end 14 | 15 | q = makeProduct(varargin{:}); % then continue by using m.models m.types 16 | ofile = ['man',name,'.m']; 17 | if exist(ofile,'file') 18 | mout = eval([ofile(1:end-2),'()']); 19 | return; 20 | end 21 | q.output = ofile; 22 | q.withlog = all(cellfun(@(x) isfield(x,'log'),varargin)); 23 | q = removefunction(q); 24 | jq = jsonencode(q); 25 | [pathstr,NAME,EXT] = fileparts(mfilename('fullpath')); 26 | tmpfile = tempname(); 27 | disp(tmpfile) 28 | fp = fopen(tmpfile,'w'); 29 | fprintf(fp,'%s',jq); 30 | fclose(fp); 31 | % store jq to tmpfile 32 | system(['python ',pathstr,filesep,'makeGenProduct.py',' ',tmpfile]); 33 | if exist(ofile,'file') == 0 34 | ofile = ''; 35 | mout = []; 36 | else 37 | mout = eval([ofile(1:end-2),'()']); 38 | end 39 | 40 | 41 | function x = removefunction(x) 42 | 43 | if isstruct(x) 44 | f = fieldnames(x); 45 | for I=1:length(f) 46 | v = x.(f{I}); 47 | if isstruct(v) 48 | for J=1:length(v) 49 | v(J) = removefunction(v(J)); 50 | end 51 | x.(f{I}) = v; 52 | else 53 | x.(f{I}) = removefunction(v); 54 | end 55 | end 56 | elseif iscell(x) 57 | for I=1:numel(x) 58 | x{I} = removefunction(x{I}); 59 | end 60 | elseif isa(x, 'function_handle') 61 | x = []; 62 | end -------------------------------------------------------------------------------- /ukfmani/quaternions/qdecomp.m: -------------------------------------------------------------------------------- 1 | function [v,phi]=qdecomp(q) 2 | % [V,PHI]=QDECOMP(Q) breaks out the unit vector and angle of rotation 3 | % components of the quaternion(s). Input will be run through QNORM to 4 | % insure that the component quaternion(s) are normalized. 5 | % 6 | % See also ISNORMQ, QNORM. 7 | 8 | % Release: $Name: quaternions-1_3 $ 9 | % $Revision: 1.12 $ 10 | % $Date: 2009-07-26 20:05:12 $ 11 | 12 | % Copyright (c) 2001-2009, Jay A. St. Pierre. All rights reserved. 13 | 14 | 15 | if nargin~=1 16 | error('qdecomp() requires one input argument'); 17 | else 18 | qtype = isq(q); 19 | if ( qtype == 0 ) 20 | error('Input Q must be a quaternion or a vector of quaternions') 21 | end 22 | end 23 | 24 | % Make sure q is a column of quaternions 25 | if( qtype == 1 ) 26 | q=q.'; 27 | end 28 | 29 | % Make sure quaternion is normalized to prevent warnings when using 30 | % sin(acos()) 31 | q=qnorm(q); 32 | 33 | half_phi=acos(q(:,4)); 34 | 35 | sin_half_phi=sin(half_phi); 36 | 37 | phi_zero_index = find(sin_half_phi==0); 38 | phi_notzero_index = find(sin_half_phi~=0); 39 | 40 | if (~isempty(phi_zero_index)) 41 | v1(phi_zero_index) = q(phi_zero_index, 1); 42 | v2(phi_zero_index) = q(phi_zero_index, 2); 43 | v3(phi_zero_index) = q(phi_zero_index, 3); 44 | end 45 | 46 | if (~isempty(phi_notzero_index)) 47 | v1(phi_notzero_index) = ... 48 | q(phi_notzero_index,1)./sin_half_phi(phi_notzero_index); 49 | v2(phi_notzero_index) = ... 50 | q(phi_notzero_index,2)./sin_half_phi(phi_notzero_index); 51 | v3(phi_notzero_index) = ... 52 | q(phi_notzero_index,3)./sin_half_phi(phi_notzero_index); 53 | end 54 | 55 | v=[v1(:) v2(:) v3(:)]; 56 | phi=2*half_phi; 57 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeRot1.m: -------------------------------------------------------------------------------- 1 | % SO3 as matrix 2 | % 3 | % Emanuele Ruffaldi 2017 @ SSSA 4 | function m = makeRot1(axis) 5 | 6 | 7 | m = []; 8 | m.type = {'Rot1',axis}; 9 | m.inv = @(X) flatten(unflatten(X)'); 10 | m.prod = @(X,Y) flatten(unflatten(X)*unflatten(Y)); 11 | m.log = @(X) so3log(unflatten(X)); % TODO 12 | m.exp = @(x) flatten(so3exp(x)); % TODO 13 | m.delta = @(X,Y) so3log(unflatten(X)*unflatten(Y)'); % TODO 14 | m.step = @(X,y) flatten(so3exp(y)*unflatten(X)); % TODO 15 | m.meancov = @manimeancov; 16 | m.count = 1; 17 | m.group = 9; % as matrix 18 | m.alg = 1; 19 | m.transport = @(X,t,Y) t; 20 | m.pack = @(x) x(:)'; 21 | m.unpack = @(x) reshape(x,3,3); 22 | m.islie = 1; 23 | m.s = int_manisetup([],[],m); 24 | 25 | function omega = so3log(R) 26 | 27 | theta = acos((trace(R)-1)/2); %acos(max(-1,min((trace(R)-1)/2,1))); 28 | if isreal(theta) == 0 29 | R = R/abs(det(R)); 30 | theta = acos((trace(R)-1)/2); 31 | end 32 | 33 | if abs(theta) < 1e-10 34 | B = 0.5; 35 | SO = (1/(2))*(R-R'); % =skew(omega) 36 | iV = eye(3); % use directly skew of omega 37 | else 38 | A = sin(theta)/theta; 39 | B = (1-cos(theta))/(theta*theta); 40 | SO = (1/(2*A))*(R-R'); % =skew(omega) 41 | %?? 42 | % syms x real 43 | % A = sin(x)/x 44 | % B= (1-cos(x))/(x*x) 45 | % Q=1/(x*x)*(1 - A/2/B) 46 | % taylor(Q,x,0) 47 | % x^4/30240 + x^2/720 + 1/12 48 | Q= 1/(theta^2)*(1 - A/2/B); 49 | iV = eye(3) - 1/2*SO + Q*SO^2; % use directly skew of omega 50 | end 51 | 52 | omega = [SO(3,2) SO(1,3) SO(2,1)]; 53 | 54 | 55 | function y = flatten(x) 56 | y = x(:)'; 57 | 58 | function y = unflatten(x) 59 | y = reshape(x,3,3); 60 | -------------------------------------------------------------------------------- /ukfmani/quaternions/test/test_qvqc.m: -------------------------------------------------------------------------------- 1 | %TEST_QVQc runs unit tests for the QVQc function. 2 | 3 | % Release: $Name: quaternions-1_3 $ 4 | % $Revision: 1.3 $ 5 | % $Date: 2009-07-26 20:05:13 $ 6 | 7 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 8 | 9 | test_title = 'qvqc'; 10 | disp_test_title(test_title); 11 | 12 | failures=0; 13 | 14 | q=[0 0 0 1], disp(' ') %#ok 15 | 16 | 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18 | disp_test_name('Insufficient Arguments: no arguments'); 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 20 | expected_err = 'Two input arguments required'; 21 | fct_call = 'qvqc'; 22 | failures=failures+check_err(fct_call, expected_err); 23 | 24 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 25 | disp_test_name('Insufficient Arguments: one argument'); 26 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 27 | expected_err = 'Two input arguments required'; 28 | fct_call = 'qvqc(1)'; 29 | failures=failures+check_err(fct_call, expected_err); 30 | 31 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 32 | disp_test_name('Algorithm check'); 33 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 34 | q=qnorm([-1 0.5 1 2; 5 6 7 8]), disp(' ') %#ok 35 | v=[1 2 3; 4 5 6].', disp(' ') %#ok 36 | truth_value = 'qvxform(qconj(q), v)'; 37 | test_value = 'qvqc(q, v)'; 38 | failures=failures+check_float(truth_value, test_value, 1e-15); 39 | 40 | 41 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 42 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 43 | disp_num_failures(test_title, failures) 44 | -------------------------------------------------------------------------------- /ukfmani/core/maniunsigma.m: -------------------------------------------------------------------------------- 1 | % recomposes the sigma points 2 | % 3 | % Input: 4 | % model 5 | % sigma: Chiz = [N,G] 6 | % sigmainfo: the weights 7 | % the delta of input sigma: vChi (see manisigma) 8 | % 9 | % Emanuele Ruffaldi 2017 @ SSSA 10 | function [mz,Czz,Cxz] = maniunsigma(model,Chiz,sigmainfo,vChi,steps) 11 | 12 | if nargin < 5 13 | steps = 20; 14 | end 15 | % estimates the mean in a weighted way 16 | N=size(Chiz,1); 17 | 18 | v = zeros(size(Chiz,1),model.alg); % preallocated 19 | mz = Chiz(1,:); % COLUMN vector 20 | 21 | % for lie group we make a little optimization using inv 22 | if isfield(model,'log') && isfield(model,'fastinv') 23 | % estimate mean but weighted of WM 24 | for k=1:steps 25 | imz = model.inv(mz); 26 | for i=1:N 27 | v(i,:) = model.log(model.prod(Chiz(i,:),imz)); 28 | end 29 | % update mz by weighted v 30 | mz = model.prod(model.exp(v'*sigmainfo.WM),mz); 31 | end 32 | 33 | % update v for computing covariance, skips the log 34 | imz = model.inv(mz); 35 | for i=1:N 36 | v(i,:) = model.log(model.prod(Chiz(i,:),imz)); 37 | end 38 | else 39 | % estimate mean but weighted of WM 40 | for k=1:steps 41 | for i=1:N 42 | % same as: se3_logdelta but with igk once 43 | v(i,:) = model.delta(Chiz(i,:),mz); 44 | end 45 | mz = model.step(mz,(sigmainfo.WM'*v)); % [A,S] [S,1] 46 | end 47 | 48 | % update v for computing covariance 49 | for i=1:N 50 | v(i,:) = model.delta(Chiz(i,:),mz); 51 | end 52 | end 53 | 54 | Czz = v'*sigmainfo.WC*v; % covariance ZZ - NOTE THAT WE USE DIFFERENTIAL 55 | 56 | if nargin >= 4 && nargout > 2 57 | Cxz = vChi'*sigmainfo.WC*v; % cross XZ - NOTE THAT WE USE DIFFERENTIAL 58 | end -------------------------------------------------------------------------------- /TODO.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | - test fusion 4 | - test projective 5 | - manifold of positive 6 | - manifold for estimating covariances 7 | - manifold SE3 right 8 | - HARD Epipolar constraint between projected points in two perspective views, see Roberto Tron's page 9 | - MAYVE Symmetric, positive definite matrices 10 | 11 | A point X on the manifold is represented as a symmetric positive definite 12 | matrix X (nxn). Tangent vectors are symmetric matrices of the same size 13 | (but not necessarily definite). 14 | 15 | The Riemannian metric is the bi-invariant metric, described notably in 16 | Chapter 6 of the 2007 book "Positive definite matrices" 17 | by Rajendra Bhatia, Princeton University Press. 18 | 19 | - example spd vs wishart 20 | - examples 21 | - smoothing 22 | http://becs.aalto.fi/en/research/bayes/ekfukf/documentation.pdf 23 | eq. 3.61 24 | 25 | input: all the states 26 | output: 27 | use X(k, kalman) and f to computer X(k+1,pred) 28 | D = C(k+1 pred, k) / P(k+1,pred) 29 | P(k,smoothed) = P(k,kalman) D (P(k+1,smoothed)-P(k+1,pred)) D' 30 | mu(k,smoothed) = mu(k,smoothed) boxplus D (mu(k+1,smoothed) boxminus mu(k+1,pred)) 31 | 32 | 1) what about other variables/parameters? e.g. a common set of parameters? See slides about total variance and uncoditioning 33 | we aim at: (Xt|t|Xt+1|t = xt+1) 34 | but we don't know xt+1 so we use law of total exèectatopm and variance: 35 | E(X) = EZ( E(X|Y = Z) ) 36 | Var(X) = EZ( Var(X|Y = Z) ) + VarZ( E(X|Y = Z) ) 37 | so we obtain 38 | X(t|T) 39 | noting that Xt|t | Xt+1|t=Xt+1|T 40 | 41 | 2) if we save both the X(k,kalman) and the X(k,pred) then we can skip the f 42 | 43 | - svdsqrt reduction if too small -------------------------------------------------------------------------------- /ukfmani/quaternions/isnormq.m: -------------------------------------------------------------------------------- 1 | function qtype=isnormq(q) 2 | % ISQ(Q) checks to see if Q is a normalized quaternion or set of quaternions. 3 | % ISNORMQ returns a value accordingly: 4 | % 5 | % 0 if Q is not a normalized quaternion or a vector of normalized 6 | % quaternions. 7 | % 8 | % 1 if Q is 4xN and only the columns are normalized. 9 | % 10 | % 2 if Q is Nx4 and only the rows are normalized. 11 | % 12 | % 3 if Q is 4x4 and both the columns and rows are normalized. 13 | % 14 | % The test for normalization uses 2*EPS as a tolerance. 15 | % 16 | % Some texts refer to a normalized quaternion as a "versor". 17 | % 18 | % See also ISQ, EPS. 19 | 20 | % Release: $Name: quaternions-1_3 $ 21 | % $Revision: 1.7 $ 22 | % $Date: 2009-07-26 20:05:12 $ 23 | 24 | % Copyright (c) 2001-2009, Jay A. St. Pierre. All rights reserved. 25 | 26 | if nargin~=1 27 | error('isnormq() requires one input argument'); 28 | else 29 | 30 | size_q=size(q); 31 | 32 | if ( length(size_q)~=2 || max(size_q==4)~=1 ) 33 | qtype=0; % Not a normalized quaternion or quaternion vector 34 | else 35 | 36 | tol=2*eps; 37 | 38 | cols_are_norm = size_q(1)==4 & ~sum((sum(q.^2,1)-ones(1,size_q(2)))>tol); 39 | rows_are_norm = size_q(2)==4 & ~sum((sum(q.^2,2)-ones(size_q(1),1))>tol); 40 | 41 | if ( ~cols_are_norm && ~rows_are_norm ) 42 | qtype=0; % Not a normalized quaternion or quaternion vector 43 | 44 | elseif ( cols_are_norm && ~rows_are_norm ) 45 | qtype=1; % Component normalized q's are column vectors 46 | 47 | elseif ( rows_are_norm && ~cols_are_norm) 48 | qtype=2; % Component normalized q's are row vectors 49 | 50 | else 51 | qtype=3; % Component normalized q's are either columns or rows 52 | 53 | end 54 | 55 | end 56 | 57 | end 58 | -------------------------------------------------------------------------------- /ukfmani/quaternions/test/test_qnorm.m: -------------------------------------------------------------------------------- 1 | %TEST_QNORM runs unit tests for the QNORM function. 2 | 3 | % Release: $Name: quaternions-1_3 $ 4 | % $Revision: 1.7 $ 5 | % $Date: 2009-07-26 20:05:13 $ 6 | 7 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 8 | 9 | test_title = 'qnorm'; 10 | disp_test_title(test_title); 11 | 12 | failures=0; 13 | 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | disp_test_name('Insufficient Arguments'); 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | expected_err = 'qnorm() requires one input argument'; 18 | fct_call = 'qnorm'; 19 | failures=failures+check_err(fct_call, expected_err); 20 | 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | disp_test_name('Invalid Input'); 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | expected_err = ... 25 | ['Invalid input: must be a quaternion or a vector of' ... 26 | ' quaternions']; 27 | fct_call = 'qnorm(1)'; 28 | failures=failures+check_err(fct_call, expected_err); 29 | 30 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 31 | disp_test_name('Column of two quaternions'); 32 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 33 | q1=1:4,q2=4:-1:1,disp(' ') %#ok 34 | truth_value = '[q1/sqrt(sum(q1.^2)); q2/sqrt(sum(q2.^2))]'; 35 | test_value = 'qnorm([q1; q2])'; 36 | failures=failures+check_value(truth_value, test_value); 37 | 38 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 39 | disp_test_name('Row of 6 quaternions'); 40 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 41 | truth_value = '.5*ones(4,6)'; 42 | test_value = 'qnorm(ones(4,6))'; 43 | failures=failures+check_value(truth_value, test_value); 44 | 45 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 46 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 47 | 48 | disp_num_failures(test_title, failures) 49 | 50 | -------------------------------------------------------------------------------- /ukfmani/quaternions/q2dcm.m: -------------------------------------------------------------------------------- 1 | function R=q2dcm(q) 2 | % Q2DCM(Q) converts quaternions into direction cosine matrices. 3 | % 4 | % The resultant DCM(s) will perform the same transformations as the 5 | % quaternion(s) in Q, i.e.: 6 | % 7 | % R*v = qvxform(q, v) 8 | % 9 | % where R is the DCM, V is a vector, and Q is the quaternion. Note that 10 | % for purposes of quaternion-vector multiplication, a vector is treated 11 | % as a quaterion with a scalar element of zero. 12 | % 13 | % If the input, Q, is a vector of quaternions, the output, R, will be 14 | % 3x3xN where input quaternion Q(k,:) corresponds to output DCM 15 | % R(:,:,k). 16 | % 17 | % Note that the input Q will be processed by QNORM to ensure normality. 18 | % 19 | % See also DCM2Q, QNORM. 20 | 21 | % Release: $Name: quaternions-1_3 $ 22 | % $Revision: 1.14 $ 23 | % $Date: 2009-07-24 19:14:44 $ 24 | 25 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 26 | 27 | 28 | if nargin~=1 29 | error('q2dcm() requires one input argument'); 30 | else 31 | qtype=isq(q); 32 | if ( qtype == 0 ) 33 | error(['Invalid input: must be a quaternion or a vector of' ... 34 | ' quaternions']) 35 | end 36 | end 37 | 38 | % Make sure input is a column of quaternions 39 | if( qtype==1 ) 40 | q=q.'; 41 | end 42 | 43 | % Make sure quaternion is normalized to prevent skewed DCM 44 | q=qnorm(q); 45 | 46 | % Build quaternion element products 47 | q1q1=q(:,1).*q(:,1); 48 | q1q2=q(:,1).*q(:,2); 49 | q1q3=q(:,1).*q(:,3); 50 | q1q4=q(:,1).*q(:,4); 51 | 52 | q2q2=q(:,2).*q(:,2); 53 | q2q3=q(:,2).*q(:,3); 54 | q2q4=q(:,2).*q(:,4); 55 | 56 | q3q3=q(:,3).*q(:,3); 57 | q3q4=q(:,3).*q(:,4); 58 | 59 | q4q4=q(:,4).*q(:,4); 60 | 61 | % Build DCM 62 | R(1,1,:) = q1q1 - q2q2 - q3q3 + q4q4; 63 | R(1,2,:) = 2*(q1q2 + q3q4); 64 | R(1,3,:) = 2*(q1q3 - q2q4); 65 | 66 | R(2,1,:) = 2*(q1q2 - q3q4); 67 | R(2,2,:) = -q1q1 + q2q2 - q3q3 + q4q4; 68 | R(2,3,:) = 2*(q2q3 + q1q4); 69 | 70 | R(3,1,:) = 2*(q1q3 + q2q4); 71 | R(3,2,:) = 2*(q2q3 - q1q4); 72 | R(3,3,:) = -q1q1 - q2q2 + q3q3 + q4q4; 73 | -------------------------------------------------------------------------------- /ukfmani/core/manimeancov.m: -------------------------------------------------------------------------------- 1 | % given a manifold definition and a set of manifold data (packed) computes 2 | % the mean and the variance 3 | % 4 | % Inputs 5 | % model is the model of the manifold 6 | % X is the input as a packed manifold (N x packedsize) 7 | % 8 | % Outputs 9 | % mz is the mean in the group 10 | % S is the covariance 11 | % 12 | % Note: for obtaining the sqrt of S use cholcov 13 | % 14 | % Emanuele Ruffaldi 2017 15 | function [mz,S,nv] = manimeancov(model,X,steps) 16 | 17 | if nargin < 3 18 | steps = 10; 19 | end 20 | 21 | 22 | % estimates the mean in a weighted way 23 | N=size(X,1); 24 | 25 | v = zeros(size(X,1),model.alg); % preallocated 26 | mz = X(1,:); % first is picked as mean 27 | 28 | % for lie group we make a little optimization using inv 29 | if nargout > 2 30 | nv = zeros(steps,3); 31 | end 32 | if isfield(model,'log') 33 | % estimate mean but weighted of WM 34 | for k=1:steps 35 | imz = model.inv(mz); 36 | for i=1:N 37 | v(i,:) = model.log(model.prod(X(i,:),imz)); 38 | end 39 | if nargout > 2 40 | p = sqrt(sum(v.^2,2)); 41 | nv(k,:) = [norm(mean(v,1)),max(p),mean(p)]; 42 | end 43 | % update mz by weighted v 44 | mz = model.prod(model.exp(mean(v,1)'),mz); 45 | end 46 | 47 | % update v for computing covariance, skips the log 48 | imz = model.inv(mz); 49 | for i=1:N 50 | v(i,:) = model.log(model.prod(X(i,:),imz)); 51 | end 52 | else 53 | % estimate mean but weighted of WM 54 | for k=1:steps 55 | for i=1:N 56 | % same as: se3_logdelta but with igk once 57 | v(i,:) = model.delta(X(i,:),mz); 58 | end 59 | if nargout > 2 60 | p = sqrt(sum(v.^2,2)); % norm of each vector, sized as Nx1 61 | nv(k,:) = [norm(mean(v,1)),max(p),mean(p)]; 62 | end 63 | mz = model.step(mz,mean(v,1)); % [A,S] [S,1] 64 | end 65 | 66 | % update v for computing covariance 67 | for i=1:N 68 | v(i,:) = model.delta(X(i,:),mz); 69 | end 70 | end 71 | 72 | S = 1/N*(v'*v); % covariance ZZ 73 | -------------------------------------------------------------------------------- /ukfmani/quaternions/isq.m: -------------------------------------------------------------------------------- 1 | function qtype=isq(q) 2 | % ISQ(Q) checks to see if Q is a quaternion or set of quaternions. 3 | % ISQ returns a value accordingly: 4 | % 5 | % 0 if Q is not a quaternion or vector of quaternions: 6 | % has more than 2 dimensions or neither dimension is of length 4 7 | % 8 | % 1 if the component quaternions of Q are column vectors: 9 | % Q is 4xN, where N~=4, or 10 | % Q is 4x4 and only the columns are normalized 11 | % 12 | % 2 if the component quaternions of Q are row vectors: 13 | % Q is Nx4, where N~=4, or 14 | % Q is 4x4 and only the rows are normalized 15 | % 16 | % 3 if the shape of the component quaternions is indeterminant: 17 | % Q is 4x4, and either both the columns and rows are normalized 18 | % or neither the columns nor rows are normalized. 19 | % 20 | % In other words, if Q is 4x4, ISQ attempts to discern the shape of 21 | % component quaternions by determining whether the rows or the columns 22 | % are normalized (i.e., it assumes that normalized quaternions are 23 | % the more typical use of quaternions). 24 | % 25 | % The test for normalization uses 2*EPS as a tolerance. 26 | % 27 | % See also ISNORMQ, EPS. 28 | 29 | % Release: $Name: quaternions-1_3 $ 30 | % $Revision: 1.7 $ 31 | % $Date: 2009-07-26 20:05:12 $ 32 | 33 | % Copyright (c) 2001-2009, Jay A. St. Pierre. All rights reserved. 34 | 35 | if nargin~=1 36 | 37 | error('isq() requires one input argument'); 38 | 39 | else 40 | 41 | tol=2*eps; 42 | 43 | size_q=size(q); 44 | 45 | if ( length(size_q)~=2 || max(size_q==4)~=1 ) 46 | qtype=0; % Not a quaternion or quaternion vector 47 | 48 | elseif ( size_q(1)==4 && ... 49 | ( size_q(2)~=4 || ( ~sum((sum(q.^2,1)-ones(1,4))>tol) && ... 50 | sum((sum(q.^2,2)-ones(4,1))>tol) ) ... 51 | ) ... 52 | ) 53 | qtype=1; % Component q's are column vectors 54 | 55 | elseif ( size_q(2)==4 && ... 56 | ( size_q(1)~=4 || ( ~sum((sum(q.^2,2)-ones(4,1))>tol) && ... 57 | sum((sum(q.^2,1)-ones(1,4))>tol) ) ... 58 | ) ... 59 | ) 60 | qtype=2; % Component q's are row vectors 61 | 62 | else 63 | qtype=3; % Component q's are either columns or rows (indeterminate) 64 | 65 | end 66 | 67 | end 68 | 69 | -------------------------------------------------------------------------------- /examples/exampleSE3MatInertial.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | mx = manisetup(makeProduct(makeSE3Mat(),makeRn(3),makeRn(3))); 4 | mz = manisetup(makeRn(3),makeRn(3)); 5 | 6 | mxt = makeSE3Mat(); % helper without the need of setup 7 | 8 | % initial state and noise definition 9 | x0 = mx.step(mx.exp([0,0,0, 0,1,0, 0,0,0, 0,0,0]),[pi/2,0.2,0, 0,0,0, 0,0,0, 0,0,0]); 10 | P0 = 0.5*eye(mx.alg); 11 | Q = 0.01*eye(mx.alg); % process noi!se 12 | R = 1e-3*eye(mz.alg); % measure noise 13 | 14 | % BUILD OBSERVATION 15 | z0 = mz.exp([pi/2,0,0, 0,0.1,1]); 16 | zobsval = zeros(200,16); 17 | v0 = zeros(6,1); 18 | v0(4) = 0.1; 19 | v0(1) = 0.2; 20 | zobsval(1,:) = z0; 21 | lzobsval = zeros(size(zobsval,1),mz.alg); 22 | lzobsval(1,:) = mz.log(z0); 23 | for I=2:size(zobsval,1) 24 | v0(2) = sin(I/100); 25 | zobsval(I,:) = mz.step(zobsval(I-1,:),v0); 26 | lzobsval(I,:) = mz.log(zobsval(I,:)); 27 | end 28 | zobs = @(t) zobsval(t,:); 29 | 30 | 31 | 32 | 33 | wsigmax = ut_mweights2(mx.group,mx.alg,0.5); 34 | wsigmax.sqrt = @svdsqrt; 35 | 36 | dt = 0.1; 37 | 38 | II = eye(3); 39 | % dot omega = - inv(I) ( omega cross I omega) 40 | % I that is local 41 | f_fx = @(Tk,wk,vk) se3step_inertial_no_acc(Tk,wk,vk,II,dt); 42 | h_fx = []; % @(Tk,wk,vk) deal(wk,vk); 43 | 44 | tic 45 | % loop 46 | deltas = zeros(200,mz.alg); 47 | states = zeros(size(deltas,1),mx.group); 48 | lstates = zeros(size(deltas,1),mx.alg); 49 | for L=1:size(deltas,1) 50 | states(L,:) = x0; 51 | lstates(L,:) = mx.log(x0); 52 | 53 | [xp,Pp] = manistatestep(mx,x0,P0,f_fx,Q,wsigmax); 54 | [zm,Czz,Cxz] = manievalh(mx,mz,xp,Pp,h_fx,wsigmax); 55 | 56 | % Kalman update with observation noise (additive) 57 | Pvv = Czz + R; 58 | K = Cxz/Pvv; 59 | P0 = (Pp - K * Pvv * K'); 60 | delta = mz.delta(zobs(L),zm); 61 | x0 = mx.step(xp,(K*delta')'); 62 | deltas(L,:) = delta; 63 | end 64 | %% 65 | toc 66 | figure(1) 67 | subplot(3,1,1); 68 | plot(deltas(:,1:3)) 69 | title('Deltas observation-prediction'); 70 | xlabel('sample'); 71 | subplot(3,1,2); 72 | plot(deltas(:,4:6)) 73 | title('Deltas observation-prediction'); 74 | xlabel('sample'); 75 | subplot(3,1,3); 76 | plot(sum(deltas.^2,2)); 77 | title('Norm of error'); 78 | xlabel('sample'); 79 | figure(2) 80 | plot(states(10:end,:)) 81 | title('All states as matrix'); 82 | 83 | %% Latency estimation 84 | dd = zeros(6,1); 85 | for J=1:6 86 | figure(2+J); 87 | dd(J) = finddelay(lstates(:,J),lzobsval(:,J)); 88 | 89 | plot([lstates(:,J),lzobsval(:,J)]); 90 | end 91 | disp('delay') 92 | dd 93 | -------------------------------------------------------------------------------- /ukfmani/quaternions/test/test_dcm2q.m: -------------------------------------------------------------------------------- 1 | %TEST_DCM2Q runs unit tests for the DCM2Q function. 2 | 3 | % Release: $Name: quaternions-1_3 $ 4 | % $Revision: 1.8 $ 5 | % $Date: 2009-07-26 20:05:12 $ 6 | 7 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 8 | 9 | test_title = 'dcm2q'; 10 | disp_test_title(test_title); 11 | 12 | failures=0; 13 | 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | disp_test_name('Insufficient Arguments'); 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | expected_err = 'One input argument required'; 18 | fct_call = 'dcm2q'; 19 | failures=failures+check_err(fct_call, expected_err); 20 | 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | disp_test_name('Invalid Input: scalar'); 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | expected_err = 'Invalid input: must be a 3x3xN array'; 25 | fct_call = 'dcm2q(1)'; 26 | failures=failures+check_err(fct_call, expected_err); 27 | 28 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 29 | disp_test_name('Algorithm'); 30 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 31 | t=0:2*pi/16:2*pi; 32 | a=sin(t)'; 33 | b=cos(t)'; 34 | z=0*a; 35 | 36 | Qin = qnorm(2*rand(10,4)-1); 37 | 38 | Qin = [Qin; [a b z z]]; 39 | Qin = [Qin; [a z b z]]; 40 | Qin = [Qin; [a z z b]]; 41 | 42 | Qin = [Qin; [b a z z]]; 43 | Qin = [Qin; [z a b z]]; 44 | Qin = [Qin; [z a z b]]; 45 | 46 | Qin = [Qin; [b z a z]]; 47 | Qin = [Qin; [z b a z]]; 48 | Qin = [Qin; [z z a b]]; 49 | 50 | Qin = [Qin; [b z z a]]; 51 | Qin = [Qin; [z b z a]]; 52 | Qin = [Qin; [z z b a]]; 53 | 54 | % make sure all quaternions have q4>=0 55 | for count=1:length(Qin) 56 | if Qin(count,4)<0 57 | Qin(count,:)=-Qin(count,:); 58 | end 59 | end 60 | Qin, disp(' ') %#ok 61 | 62 | A='q2dcm(Qin)', disp(' ') %#ok 63 | A=eval(A); 64 | 65 | T=(1:length(Qin))*0; 66 | for count=1:length(Qin) 67 | T(count) = trace(A(:,:,count)); 68 | end 69 | 70 | Qout='dcm2q(A)', disp(' ') %#ok 71 | Qout=eval(Qout); 72 | 73 | qdiff='qmult(qconj(Qout),Qin)', disp(' ') %#ok 74 | qdiff=eval(qdiff); 75 | 76 | truth_value = '[0.0 0.0 0.0]'; 77 | test_value = 'max(abs(qdiff(:,1:3)))'; 78 | 79 | failures=failures+check_value(truth_value, test_value); 80 | 81 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 82 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 83 | 84 | disp_num_failures(test_title, failures) 85 | 86 | -------------------------------------------------------------------------------- /ukfmani/helpers/meancovSE3manopt.m: -------------------------------------------------------------------------------- 1 | % given X in SE3 unpacked form we would like to compute mean and covariance 2 | % using manopt 3 | % 4 | % sympositivedefinitefactory(6,1) 5 | % specialeuclideanfactory(3, 1) 6 | function [m,S] = meancovSE3manopt(X) 7 | 8 | %bishop page 93 9 | %loglikelihood 10 | % cost + -N/2 log det(Sigma) - sum (xi-mu)' inv(Sigma) (xi-mu) 11 | % 12 | % derivative for mu 13 | % sum inv(sigma) (x-mu) 14 | % => mu = 1/N sum (x) 15 | % 16 | % derivative for Sigma 17 | % -N/2 inv(A)' - ... 18 | % => Sigma = 1/N sum (x-mu)' (x-mu) 19 | % 20 | % The maximization of (2.118) with respect to Σ is rather more involved. The simplest approach is to ignore the symmetry constraint and show that the resulting solution is symmetric as required. Alternative derivations of this result, which impose the symmetry and positive defi- niteness constraints explicitly, can be found in Magnus and Neudecker (1999). 21 | % es. 2.34 Using the results (C.21), (C.26), and (C.28) from Appendix C, show that the covariance matrix Σ that maximizes the log likelihood function (2.118) is given by the sample covariance (2.122). We note that the final result is necessarily symmetric and positive definite (provided the sample covariance is nonsingular). 22 | % 23 | % using: 24 | % d/dA ln(|A|) = (inv(A))' 25 | % d/dA tr(A) = I 26 | % d/dx inv(F) = - inv(F) dF/dx inv(F) 27 | % d/dA tr(A B A') = A (B + B') 28 | % 29 | % joint from Neucker 30 | % dLikelihood = 1/2 tr(dSigma) inv(Sigma) (Z-N Sigma) inv(Sigma) + N 31 | % (dmu)'inv(Sigma)(mean(X)-mu) 32 | % 33 | % where Z = cov(X,mu) aka the numeric covariance of X with mean mu 34 | % 35 | % there is also the second for the hessian 36 | % 37 | % Manifold loglikelihood 38 | % cost + -N/2 log det(Sigma) - sum delta(xi,mu)' inv(Sigma) delta(xi-mu) 39 | % 40 | % delta(m1,m2) = log(m1*inv(m2)) if the manifold is Lie Group 41 | % 42 | % http://www.janmagnus.nl/misc/mdc2007-3rdedition 43 | % https://docs.google.com/document/d/1QlzmQe1U1vEwN2ShqPTB3V2uG1tW0snGEosWZgbOb4Q/edit 44 | 45 | 46 | 47 | problem1.M = specialeuclideanfactory(3, 1); 48 | 49 | % zi = data 50 | % x = mu 51 | % sum_i (zi-x) 52 | problem1.cost = @(x) -x'*(A*x); 53 | problem1.egrad = @(x) -2*A*x; 54 | 55 | % Numerically check gradient consistency (optional). 56 | checkgradient(problem1); 57 | 58 | % Solve. 59 | [x, xcost, info, options] = trustregions(problem1); 60 | 61 | 62 | 63 | problem2.M = sympositivedefinitefactory(6, 1); 64 | 65 | % zi = data 66 | % x = mu 67 | 68 | % 1/N sum_i (z-x) 69 | problem2.cost = @(x) -x'*(A*x); 70 | problem2.egrad = @(x) -2*A*x; 71 | 72 | % Numerically check gradient consistency (optional). 73 | checkgradient(problem1); 74 | 75 | % Solve. 76 | [x, xcost, info, options] = trustregions(problem1); -------------------------------------------------------------------------------- /ukfmani/quaternions/test/test_qconj.m: -------------------------------------------------------------------------------- 1 | %TEST_QCONJ runs unit tests for the QCONJ function. 2 | 3 | % Release: $Name: quaternions-1_3 $ 4 | % $Revision: 1.8 $ 5 | % $Date: 2009-07-26 20:05:12 $ 6 | 7 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 8 | 9 | test_title = 'qconj'; 10 | disp_test_title(test_title); 11 | 12 | failures=0; 13 | 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | disp_test_name('Insufficient Arguments'); 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | expected_err = 'qconj() requires one input argument'; 18 | fct_call = 'qconj'; 19 | failures=failures+check_err(fct_call, expected_err); 20 | 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | disp_test_name('Invalid Input'); 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | expected_err = ... 25 | ['Invalid input: must be a quaternion or a vector of' ... 26 | ' quaternions']; 27 | fct_call = 'qconj(1)'; 28 | failures=failures+check_err(fct_call, expected_err); 29 | 30 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 31 | disp_test_name('Column of two quaternions'); 32 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 33 | truth_value = '[ -ones(2,3) ones(2,1) ]'; 34 | test_value = 'qconj(ones(2,4))'; 35 | failures=failures+check_value(truth_value, test_value); 36 | 37 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 38 | disp_test_name('Row of 6 quaternions'); 39 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 40 | truth_value = '[ -ones(3,6); ones(1,6) ]'; 41 | test_value = 'qconj(ones(4,6))'; 42 | failures=failures+check_value(truth_value, test_value); 43 | 44 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 45 | disp_test_name('Ambiguous Input: 4x4 normalized in both directions'); 46 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 47 | expected_warn = ['Component quaternion shape indeterminate, assuming' ... 48 | ' row vectors']; 49 | fct_call = 'qconj(0.5*ones(4,4));'; 50 | failures=failures+check_warn(fct_call, expected_warn); 51 | truth_value = '0.5*[-ones(4,3) ones(4,1)]'; 52 | warning_state = warning; warning('qconj:indeterminateShape', 'off'); 53 | test_value = 'qconj(0.5*ones(4,4))'; 54 | failures=failures+check_value(truth_value, test_value); 55 | warning(warning_state); 56 | 57 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 58 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 59 | 60 | disp_num_failures(test_title, failures) 61 | 62 | -------------------------------------------------------------------------------- /ukfmani/quaternions/test/test_q2dcm.m: -------------------------------------------------------------------------------- 1 | %TEST_Q2DCM runs unit tests for the Q2DCM function. 2 | 3 | % Release: $Name: quaternions-1_3 $ 4 | % $Revision: 1.6 $ 5 | % $Date: 2009-07-26 20:05:12 $ 6 | 7 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 8 | 9 | test_title = 'q2dcm'; 10 | disp_test_title(test_title); 11 | 12 | failures=0; 13 | 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | disp_test_name('Insufficient Arguments'); 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | expected_err = 'q2dcm() requires one input argument'; 18 | fct_call = 'q2dcm'; 19 | failures=failures+check_err(fct_call, expected_err); 20 | 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | disp_test_name('Invalid Input: scalar'); 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | expected_err = ... 25 | ['Invalid input: must be a quaternion or a vector of' ... 26 | ' quaternions']; 27 | fct_call = 'q2dcm(1)'; 28 | failures=failures+check_err(fct_call, expected_err); 29 | 30 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 31 | disp_test_name('Ambiguous Input: 4x4 non-normalized'); 32 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 33 | expected_warn = ... 34 | 'Component quaternion shape indeterminate, assuming row vectors'; 35 | fct_call = 'q2dcm(ones(4,4));'; 36 | failures=failures+check_warn(fct_call, expected_warn); 37 | 38 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 39 | disp_test_name('Column of two quaternions'); 40 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 41 | Q=qnorm(2*rand(2,4)-1), disp(' ') %#ok 42 | v=[1 2 3], disp(' ') %#ok 43 | truth_value = 'qvxform(Q, v)'; 44 | A='q2dcm(Q)', disp(' ') %#ok 45 | A=eval(A); %#ok 46 | test_value='[A(:,:,1)*v.'' A(:,:,2)*v.''].''', disp(' ') %#ok 47 | failures=failures+check_value(truth_value, test_value, 10*eps); 48 | 49 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 50 | disp_test_name('Row of two quaternions'); 51 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 52 | Q=qnorm(2*rand(4,2)-1), disp(' ') %#ok 53 | v=rand(3,2), disp(' ') %#ok 54 | truth_value = 'qvxform(Q, v)'; 55 | A='q2dcm(Q)', disp(' ') %#ok 56 | A=eval(A); 57 | test_value = '[A(:,:,1)*v(:,1) A(:,:,2)*v(:,2)]'; 58 | failures=failures+check_value(truth_value, test_value, 10*eps); 59 | 60 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 61 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 62 | 63 | disp_num_failures(test_title, failures) 64 | 65 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeProject.m: -------------------------------------------------------------------------------- 1 | % Takes a manifold and produces a new one whose tangential space is a 2 | % transformation of the original one, potentially projected 3 | % 4 | % This meta manifold works with a projection of the tangential space (e.g. 5 | % single axis rotation). The projection is expressed as follows: 6 | % R matrix that rotates the tangential space 7 | % k output degrees of freedom (the first k over the overall incoming n 8 | % degrees) 9 | % 10 | % Note that the state is still in the original frame 11 | % 12 | % m = makeProject(m0,R) 13 | % applis the linear transformation R 14 | % 15 | % m = makeProject(m0,R,k) 16 | % applies the transformation R and takes the first k tangent vectors 17 | % 18 | % The transformation can be buil using: 19 | % adjoint matrix (6x6) for SE(3) to constraint along direction 20 | % rotation matrix (3x3) for SO(3) 21 | % permutatio of axis (permVector) 22 | % 23 | % 24 | % Emanuele Ruffaldi 2017-2018 @ SSSA 25 | function m = makeProject(m0,R,k) 26 | 27 | n=m0.alg; 28 | if nargin == 2 29 | k=n; 30 | end 31 | assert(k <= n && k > 0,'new space should have correct number of dofs'); 32 | assert(all(size(R)==n),'R should be n by n'); 33 | %assert R not singular 34 | m = m0; 35 | m.alg = k; 36 | m.refmodels = {m0}; 37 | m.type = {'Project',m0.type,R}; 38 | m.alginc = [0,k]; 39 | ostep=m0.step; 40 | 41 | m.islie = m0.islie; 42 | m.unpack = @(x) projectunpack(m0,x); 43 | m.pack = @(x) m0.pack(x); 44 | 45 | 46 | if n == k 47 | m.step = @(X,v) ostep(X,(R\v(:))'); 48 | m.delta =@(X,Y) (R*m0.delta(X,Y)')'; 49 | else 50 | m.step = @(X,v) ostep(X,(R\zeroextendcol(v,n,k))'); 51 | m.delta =@(X,Y) firstk(R*m0.delta(X,Y)',k)'; 52 | end 53 | m.transport = @(X,t,Y) xtransport(X,t,Y,m0.transport,n,k); 54 | m.projectcov = @(C) projectcov(C,R,k); 55 | m.unprojectcov = @(C) unprojectcov(C,R,k,n); 56 | m.s = int_manisetup([],[],m); 57 | 58 | % for Lie Group delta is: log(X*inv(Y)) 59 | 60 | if isfield(m0,'log') 61 | olog=m0.log; 62 | oexp=m0.exp; 63 | if n == k 64 | m.log = @(X) (R*olog(X))'; 65 | m.exp = @(v) oexp((R\v(:))'); 66 | else 67 | m.log = @(X) firstk((R*reshape(olog(X),[],1)),k)'; 68 | m.exp = @(v) oexp((R\zeroextendrow(v,n,k))'); 69 | end 70 | end 71 | 72 | function v = xtransport(X,t,Y,tra,n,k) 73 | 74 | ve = tra(X,zeroextendcol(t,n,k,Y),Y); 75 | v = ve(1:k); 76 | 77 | function r = projectunpack(m0,x) 78 | r = m0.unpack(x); 79 | if iscell(r) 80 | r = r{1}; 81 | end 82 | function v = firstk(v0,k) 83 | v = v0(1:k); 84 | 85 | % extend as row 86 | function v = zeroextendrow(v0,n,k) 87 | v=zeros(1,n); 88 | v(1:k) = v0; 89 | 90 | % extend as col 91 | function v = zeroextendcol(v0,n,k) 92 | v=zeros(n,1); 93 | v(1:k) = v0; 94 | 95 | function CR = projectcov(C,R,k) 96 | 97 | tmp = R*C/R; 98 | CR = tmp(1:k,1:k); 99 | 100 | function C = unprojectcov(CR,R,k,n) 101 | 102 | C = zeros(n); 103 | C(1:k,1:k) = R\CR*R; 104 | -------------------------------------------------------------------------------- /ukfmani/quaternions/Contents.m: -------------------------------------------------------------------------------- 1 | % Quaternions Toolbox 2 | % Version 1.3 (JASP) 26-Jul-2009 3 | % ======================================================================== 4 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 5 | % This software is licensed under the terms of the BSD license. 6 | % See the file license.txt that came with the software for more details. 7 | % ======================================================================== 8 | % 9 | % For purposes of these tools, a quaternion, q, is just a four element 10 | % vector where q(1:3) is the "imaginary" or "vector" portion of the 11 | % hypercomplex number, and q(4) is the "real" or "scalar" portion. 12 | % Consequently, if q represents a rotation, then: 13 | % 14 | % q(1) = v1*sin(phi/2) 15 | % q(2) = v2*sin(phi/2) 16 | % q(3) = v3*sin(phi/2) 17 | % q(4) = cos(phi/2) 18 | % 19 | % where phi is the amount of rotation about the unit vector [v1 v2 v3]. 20 | % 21 | % All tools are vectorized, so "vectors" of quaternions (4xN or Nx4 22 | % matrices) can be handled as well. Since it is most common to work with 23 | % normalized quaternions (also referred to as "unit quaternions" and 24 | % "versors"), if a set of 4 quaternions, i.e., a 4x4 matrix, is input, the 25 | % tools will attempt to determine the shape of the component quaternions 26 | % (4x1 or 1x4) based on whether the rows or columns are normalized. 27 | % 28 | % Of course, some of the tools, like QDECOMP, only make sense for normalized 29 | % quaternions, and thus those tools enforce normality via QNORM. 30 | % 31 | % isq - determines whether or not input is a quaternion 32 | % isnormq - determines whether or not input is a normalized quaternion 33 | % 34 | % qconj - quaternion conjugate 35 | % qnorm - normalize quaternion 36 | % qmult - multiply quaternions 37 | % 38 | % qdecomp - decompose quaternion into unit vector and rotation angle 39 | % 40 | % qcvq - operation on vector: qconj(q) v q 41 | % qvqc - operation on vector: q v qconj(q) 42 | % 43 | % Because the author uses the convention described in "Spacecraft Attitude 44 | % Determination and Control" (Wertz, 1978), the following aliases exist: 45 | % 46 | % qvxform - quaternion/vector transform (alias for qcvq) 47 | % qvrot - quaternion/vector rotation (alias for qvqc) 48 | % 49 | % Likewise, the following operations assume the relationship between the 50 | % DCM and the quaternion is: R*v = qvxform(q, v) = qcvq(q, v). That is, 51 | % the q that performs the equivalent operation on v is the "right hand 52 | % quaterion". 53 | % 54 | % q2dcm - quaternion to direction cosine matrix 55 | % dcm2q - direction cosine matrix to quaternion 56 | % 57 | % Note that many more recent applications, particularly computer graphics 58 | % libraries, choose the opposite convention. That is, the equivalent "q" 59 | % is the "left hand quaternion" and consequently the qvxform/qvrot aliases 60 | % and q2dcm and dcm2q functions would be "backward". 61 | % 62 | % See also QLIB, the Quaternion block library for Simulink. 63 | 64 | % Package: $Name: quaternions-1_3 $ 65 | % File: $Revision: 1.17 $ 66 | % $Date: 2009-07-26 20:25:26 $ -------------------------------------------------------------------------------- /ukfmani/quaternions/dcm2q.m: -------------------------------------------------------------------------------- 1 | function q=dcm2q(R) 2 | % DCM2Q(R) converts direction cosine matrices into quaternions. 3 | % 4 | % The resultant quaternion(s) will perform the equivalent vector 5 | % transformation as the input DCM(s), i.e.: 6 | % 7 | % qconj(Q)*V*Q = R*V 8 | % 9 | % where R is the DCM, V is a vector, and Q is the quaternion. Note that 10 | % for purposes of quaternion-vector multiplication, a vector is treated 11 | % as a quaterion with a scalar element of zero. 12 | % 13 | % If the input is a 3x3xN array, the output will be a vector of 14 | % quaternions where input direction cosine matrix R(:,:,k) corresponds 15 | % to the output quaternion Q(k,:). 16 | % 17 | % Note that this function is meaningless for non-orthonormal matrices! 18 | % 19 | % See also Q2DCM. 20 | 21 | % Release: $Name: quaternions-1_3 $ 22 | % $Revision: 1.11 $ 23 | % $Date: 2009-07-25 04:28:18 $ 24 | 25 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 26 | 27 | % Thanks to Tatsuki Kashitani for pointing out the numerical instability in 28 | % the original implementation. His suggested fix also included a check for 29 | % the "sr" values below being zero. But I think I've convinced myself that 30 | % this isn't necessary if the input matrices are orthonormal (or at least 31 | % very close to orthonormal). 32 | 33 | if nargin~=1 34 | error('One input argument required'); 35 | else 36 | size_R=size(R); 37 | if ( size_R(1)~=3 || size_R(2)~=3 || length(size_R)>3 ) 38 | error('Invalid input: must be a 3x3xN array') 39 | end 40 | end 41 | 42 | q = zeros( 4, size( R, 3 ) ); 43 | 44 | for id_dcm = 1 : size( R, 3 ) 45 | dcm = R( :, :, id_dcm ); 46 | if trace( dcm ) > 0 47 | % Positve Trace Algorithm 48 | sr = sqrt( 1 + trace( dcm )); 49 | sr2 = 2*sr; 50 | q(1,id_dcm) = ( dcm(2,3) - dcm(3,2) ) / sr2; 51 | q(2,id_dcm) = ( dcm(3,1) - dcm(1,3) ) / sr2; 52 | q(3,id_dcm) = ( dcm(1,2) - dcm(2,1) ) / sr2; 53 | q(4,id_dcm) = 0.5 * sr; 54 | else 55 | % Negative Trace Algorithm 56 | if ( dcm(1,1) > dcm(2,2) ) && ( dcm(1,1) > dcm(3,3) ) 57 | % Maximum Value at DCM(1,1) 58 | sr = sqrt( 1 + (dcm(1,1) - ( dcm(2,2) + dcm(3,3) )) ); 59 | sr2 = 2*sr; 60 | q(1,id_dcm) = 0.5 * sr; 61 | q(2,id_dcm) = ( dcm(2,1) + dcm(1,2) ) / sr2; 62 | q(3,id_dcm) = ( dcm(3,1) + dcm(1,3) ) / sr2; 63 | q(4,id_dcm) = ( dcm(2,3) - dcm(3,2) ) / sr2; 64 | elseif dcm(2,2) > dcm(3,3) 65 | % Maximum Value at DCM(2,2) 66 | sr = sqrt( 1 + (dcm(2,2) - ( dcm(3,3) + dcm(1,1) )) ); 67 | sr2 = 2*sr; 68 | q(1,id_dcm) = ( dcm(2,1) + dcm(1,2) ) / sr2; 69 | q(2,id_dcm) = 0.5 * sr; 70 | q(3,id_dcm) = ( dcm(2,3) + dcm(3,2) ) / sr2; 71 | q(4,id_dcm) = ( dcm(3,1) - dcm(1,3) ) / sr2; 72 | else 73 | % Maximum Value at DCM(3,3) 74 | sr = sqrt( 1 + (dcm(3,3) - ( dcm(1,1) + dcm(2,2) )) ); 75 | sr2 = 2*sr; 76 | q(1,id_dcm) = ( dcm(3,1) + dcm(1,3) ) / sr2; 77 | q(2,id_dcm) = ( dcm(2,3) + dcm(3,2) ) / sr2; 78 | q(3,id_dcm) = 0.5 * sr; 79 | q(4,id_dcm) = ( dcm(1,2) - dcm(2,1) ) / sr2; 80 | end 81 | end 82 | end 83 | 84 | % Make quaternion vector a column of quaternions 85 | q=q.'; 86 | 87 | q=real(q); 88 | 89 | 90 | -------------------------------------------------------------------------------- /ukfmani/quaternions/qcvq.m: -------------------------------------------------------------------------------- 1 | function v_out=qcvq(q,v) 2 | % QcVQ(Q,V) performs the operation qconj(Q)*V*Q 3 | % where the vector is treated as a quaternion with a scalar element of 4 | % zero. 5 | % 6 | % Q and V can be vectors of quaternions and vectors, but they must 7 | % either be the same length or one of them must have a length of one. 8 | % The output will have the same shape as V. Q will be passed through 9 | % QNORM to ensure it is normalized. 10 | % 11 | % See also QVQc, QNORM, QMULT. 12 | 13 | % Note that QNORM is invoked by QMULT, therefore QcQV does not invoke 14 | % it directly. 15 | 16 | % Release: $Name: quaternions-1_3 $ 17 | % $Revision: 1.2 $ 18 | % $Date: 2009-07-26 20:05:12 $ 19 | 20 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 21 | 22 | 23 | if nargin~=2 24 | error('Two input arguments required.'); 25 | else 26 | 27 | qtype=isq(q); 28 | if ( qtype == 0 ) 29 | error('Input Q must be a quaternion or a vector of quaternions') 30 | end 31 | 32 | size_v=size(v); 33 | if ( length(size_v)~=2 || max(size_v==3)~=1 ) 34 | error(['Invalid input: second input must be a 3-element vector', 10, ... 35 | 'or a vector of 3-element vectors']) 36 | end 37 | 38 | end 39 | 40 | % Make sure q is a column of quaternions 41 | if ( qtype==1 ) 42 | q=q.'; 43 | end 44 | 45 | % Make sure v is a column of vectors 46 | row_of_vectors = (size_v(2) ~= 3); 47 | if ( row_of_vectors ) 48 | v=v.'; 49 | size_v=size(v); 50 | end 51 | 52 | size_q=size(q); 53 | 54 | if ( size_q(1)~=size_v(1) && size_q(1)~=1 && size_v(1)~=1 ) 55 | error(['Inputs do not have the same number of elements:', 10, ... 56 | ' number of quaternions in q = ', num2str(size_q(1)), 10,... 57 | ' number of vectors in v = ', num2str(size_v(1)), 10,... 58 | 'Inputs must have the same number of elements, or', 10, ... 59 | 'one of the inputs must have a single element.']) 60 | elseif ( size_q(1)==1 && size_v(1)==3 ) 61 | if ( qtype==1 ) 62 | warning(... 63 | 'qcvq:assumingVcols', ... 64 | 'Q is 4x1 and V is 3x3: assuming vectors are column vectors') 65 | row_of_vectors = 1; 66 | v=v.'; 67 | else 68 | warning(... 69 | 'qcvq:assumingVrows', ... 70 | 'Q is 1x4 and V is 3x3: assuming vectors are row vectors') 71 | end 72 | elseif ( qtype==3 && size_v(1)==1 ) 73 | if ( row_of_vectors ) 74 | warning(... 75 | 'qcvq:assumingQcols', ... 76 | 'Q is 4x4 and V is 3x1: assuming quaternions are column vectors') 77 | q=q.'; 78 | else 79 | warning(... 80 | 'qcvq:assumingQrows', ... 81 | 'Q is 4x4 and V is 1x3: assuming quaternions are row vectors') 82 | end 83 | end 84 | 85 | % Build up full vectors if one input is a singleton 86 | if ( size_q(1) ~= size_v(1) ) 87 | ones_length = ones(max(size_q(1),size_v(1)),1); 88 | if ( size_q(1) == 1 ) 89 | q = [q(1)*ones_length ... 90 | q(2)*ones_length ... 91 | q(3)*ones_length ... 92 | q(4)*ones_length ]; 93 | else % size_v(1) == 1 94 | v = [v(1)*ones_length ... 95 | v(2)*ones_length ... 96 | v(3)*ones_length ]; 97 | end 98 | end 99 | 100 | % Add an element to V 101 | v(:,4)=zeros(size_v(1),1); 102 | 103 | % Turn off warnings before calling qconj (it has simillar warnings as 104 | % qvxform, so all warnings would just be duplicated). Save current state of 105 | % warnings, though. 106 | warning_state = warning; warning('off', 'qconj:indeterminateShape'); 107 | local_warning = lastwarn; 108 | 109 | % Perform transform 110 | vt=qmult(qconj(q),qmult(v,q)); 111 | 112 | % Restore warning state to original state 113 | warning(warning_state); 114 | lastwarn(local_warning); 115 | 116 | % Eliminate last element of vt for output 117 | v_out = vt(:,1:3); 118 | 119 | % Make sure output vectors are the same shape as input vectors 120 | if ( row_of_vectors ) 121 | v_out = v_out.'; 122 | end 123 | -------------------------------------------------------------------------------- /ukfmani/manifolds/tbd_makeSE3MatLocal.m: -------------------------------------------------------------------------------- 1 | % SE3 in 4x4 matrix 2 | % the algebra is: omega linear 3 | % Emanuele Ruffaldi 2017 @ SSSA 4 | function m = makeSE3Mat() 5 | 6 | m1= makeRot(); 7 | m = []; 8 | m.type = {'SE3MatLocal'}; 9 | m.inv = @se3inv; 10 | m.prod = @(x,y) mflat(munflat(x)*munflat(y)); 11 | m.m1 = makeRot(); 12 | m.log = @se3log; 13 | m.exp = @se3exp; 14 | m.delta = @(x,y) se3log(munflat(se3inv(y))*munflat(x)); 15 | m.pack = @(x) reshape(x{1},[],1); 16 | m.unpack = @(x) {reshape(x,4,4)}; 17 | m.transport = @(X,t,Y) t; 18 | m.flat = @(x) reshape(x,1,[]); 19 | m.unflat = @(x) reshape(x,4,4); 20 | m.islie = 1; 21 | m.step = @(X,y) mflat(munflat(X)*munflat(m.exp(y))); 22 | m.group = 16; 23 | m.alg = 6; 24 | m.count = 1; 25 | m.s = int_manisetup([],[],m); 26 | 27 | function y = se3log(x) 28 | 29 | t = getpos(x); 30 | R = getrot(x); 31 | 32 | theta = acos((trace(R)-1)/2); %acos(max(-1,min((trace(R)-1)/2,1))); 33 | if isreal(theta) == 0 34 | R = R/abs(det(R)); 35 | theta = acos((trace(R)-1)/2); 36 | end 37 | 38 | if abs(theta) < 1e-10 39 | B = 0.5; 40 | SO = (1/(2))*(R-R'); % =skew(omega) 41 | iV = eye(3); % use directly skew of omega 42 | else 43 | A = sin(theta)/theta; 44 | B = (1-cos(theta))/(theta*theta); 45 | SO = (1/(2*A))*(R-R'); % =skew(omega) 46 | %?? 47 | % syms x real 48 | % A = sin(x)/x 49 | % B= (1-cos(x))/(x*x) 50 | % Q=1/(x*x)*(1 - A/2/B) 51 | % taylor(Q,x,0) 52 | % x^4/30240 + x^2/720 + 1/12 53 | Q= 1/(theta^2)*(1 - A/2/B); 54 | iV = eye(3) - 1/2*SO + Q*SO^2; % use directly skew of omega 55 | end 56 | omega = [SO(3,2) SO(1,3) SO(2,1)]; 57 | 58 | %y = [m1.log(getrot(x)), getpos(x)]; % not exact 59 | u = iV*t'; 60 | y = [omega(:);u(:)]'; 61 | 62 | function y = se3exp(x) 63 | 64 | omega = getomega(x); 65 | u = getvel(x); 66 | 67 | theta = norm(omega); 68 | if theta < 1e-12 69 | % Original 70 | % A = 1; 71 | % B = 0.5; 72 | % C = 1/6; 73 | % S = zeros(3); 74 | % R = eye(3) + A*S + B*S^2; 75 | % V = eye(3) + B*S + C*S^2; 76 | 77 | N = 10; 78 | R = eye(3); 79 | xM = eye(3); 80 | cmPhi = skew(omega); 81 | V = eye(3); 82 | pxn = eye(3); 83 | for n = 1:N 84 | xM = xM * (cmPhi / n); 85 | pxn = pxn*cmPhi/(n + 1); 86 | R = R + xM; 87 | V = V + pxn; 88 | end 89 | 90 | % Project the resulting rotation matrix back onto SO(3) 91 | R = R / sqrtm( R'*R ) ; 92 | 93 | else 94 | %Original 95 | if 1==1 96 | A = sin(theta)/theta; 97 | B = (1-cos(theta))/(theta^2); 98 | C = (theta-sin(theta))/(theta^3); 99 | S = skew(omega); 100 | R = eye(3) + A*S + B*S^2; 101 | V = eye(3) + B*S + C*S^2; 102 | else 103 | %Barfoot 104 | 105 | axis = omega/theta; 106 | cp = cos(theta); 107 | sp = sin(theta); 108 | cph = (1 - cos(theta))/theta; 109 | sph = sin(theta)/theta; 110 | sa = skew(axis); 111 | 112 | R = cp*eye(3) + (1-cp)*axis*(axis') + sp*sa; 113 | V = sph * eye(3) + (1 - sph) * axis * (axis') + cph * sa; 114 | end 115 | 116 | end 117 | 118 | y = eye(4); 119 | y(1:3,1:3) = R; 120 | y(1:3,4) = V*u(:); 121 | y = mflat(y); 122 | 123 | 124 | 125 | function u = mflat(x) 126 | 127 | u = x(:); 128 | 129 | function u = munflat(x) 130 | 131 | u = reshape(x,4,4); 132 | 133 | function y = se3inv(x) 134 | 135 | R = getrot(x); 136 | y = eye(4); 137 | y(1:3,1:3) = R'; 138 | y(1:3,4) = -y(1:3,1:3)*getpos(x)'; 139 | y = mflat(y); 140 | 141 | function u = ubuild2(rot,pos) 142 | 143 | u = eye(4); 144 | u(1:3,1:3) = reshape(rot,3,3); 145 | u(1:3,4) = pos; 146 | 147 | function R = getrot(x) 148 | 149 | M = munflat(x); 150 | R = M(1:3,1:3); 151 | 152 | function p = getpos(x) 153 | 154 | p = x(13:15); 155 | p = p(:)'; 156 | 157 | function u = getomega(x) 158 | 159 | u = x(1:3); 160 | 161 | function u = getvel(x) 162 | 163 | u = x(4:6); -------------------------------------------------------------------------------- /ukfmani/quaternions/qmult.m: -------------------------------------------------------------------------------- 1 | function q_out=qmult(q1,q2) 2 | % QMULT(Q1,Q2) calculates the product of two quaternions Q1 and Q2. 3 | % Inputs can be vectors of quaternions, but they must either have the 4 | % same number of component quaternions, or one input must be a single 5 | % quaternion. QMULT will determine whether the component quaternions of 6 | % the inputs are row or column vectors according to ISQ. 7 | % 8 | % The output will have the same shape as Q1. If the component 9 | % quaternions of either Q1 or Q2 (but not both) are of indeterminate 10 | % shape (see ISQ), then the shapes will be assumed to be the same for 11 | % both inputs. If both Q1 and Q2 are of indeterminate shape, then both 12 | % are assumed to be composed of row vector quaternions. 13 | % 14 | % See also ISQ. 15 | 16 | % Release: $Name: quaternions-1_3 $ 17 | % $Revision: 1.14 $ 18 | % $Date: 2009-07-26 20:05:12 $ 19 | 20 | % Copyright (c) 2001-2009, Jay A. St. Pierre. All rights reserved. 21 | 22 | 23 | if nargin~=2 24 | error('qmult() requires two input arguments'); 25 | else 26 | q1type = isq(q1); 27 | if ( q1type == 0 ) 28 | error(['Invalid input: q1 must be a quaternion or a vector of' ... 29 | ' quaternions']) 30 | end 31 | q2type = isq(q2); 32 | if ( q2type == 0 ) 33 | error(['Invalid input: q2 must be a quaternion or a vector of' ... 34 | ' quaternions']) 35 | end 36 | end 37 | 38 | % Make sure q1 is a column of quaternions (components are rows) 39 | if ( q1type==1 || (q1type==3 && q2type==1) ) 40 | q1=q1.'; 41 | end 42 | 43 | % Make sure q2 is a column of quaternions (components are rows) 44 | if ( q2type==1 || (q2type==3 && q1type==1) ) 45 | q2=q2.'; 46 | end 47 | 48 | num_q1=size(q1,1); 49 | num_q2=size(q2,1); 50 | 51 | if ( num_q1~=num_q2 && num_q1~=1 && num_q2~=1 ) 52 | error(['Inputs do not have the same number of elements:', 10, ... 53 | ' number of quaternions in q1 = ', num2str(num_q1), 10,... 54 | ' number of quaternions in q2 = ', num2str(num_q2), 10,... 55 | 'Inputs must have the same number of elements, or', 10, ... 56 | 'one of the inputs must be a single quaternion (not a', 10, ... 57 | 'vector of quaternions).']) 58 | end 59 | 60 | % Build up full quaternion vector if one input is a single quaternion 61 | if ( num_q1 ~= num_q2 ) 62 | ones_length = ones(max(num_q1,num_q2),1); 63 | if ( num_q1 == 1 ) 64 | q1 = [q1(1)*ones_length ... 65 | q1(2)*ones_length ... 66 | q1(3)*ones_length ... 67 | q1(4)*ones_length ]; 68 | else % num_q2 == 1 69 | q2 = [q2(1)*ones_length ... 70 | q2(2)*ones_length ... 71 | q2(3)*ones_length ... 72 | q2(4)*ones_length ]; 73 | end 74 | end 75 | 76 | % Products 77 | 78 | % If q1 and q2 are not vectors of quaternions, then: 79 | % 80 | % q1*q2 = q1*[ q2(4) -q2(3) q2(2) -q2(1) 81 | % q2(3) q2(4) -q2(1) -q2(2) 82 | % -q2(2) q2(1) q2(4) -q2(3) 83 | % q2(1) q2(2) q2(3) q2(4) ] 84 | % 85 | % But to deal with vectorized quaternions, we have to use the ugly 86 | % commands below. 87 | 88 | prod1 = ... 89 | [ q1(:,1).*q2(:,4) -q1(:,1).*q2(:,3) q1(:,1).*q2(:,2) -q1(:,1).*q2(:,1)]; 90 | prod2 = ... 91 | [ q1(:,2).*q2(:,3) q1(:,2).*q2(:,4) -q1(:,2).*q2(:,1) -q1(:,2).*q2(:,2)]; 92 | prod3 = ... 93 | [-q1(:,3).*q2(:,2) q1(:,3).*q2(:,1) q1(:,3).*q2(:,4) -q1(:,3).*q2(:,3)]; 94 | prod4 = ... 95 | [ q1(:,4).*q2(:,1) q1(:,4).*q2(:,2) q1(:,4).*q2(:,3) q1(:,4).*q2(:,4)]; 96 | 97 | q_out = prod1 + prod2 + prod3 + prod4; 98 | 99 | % Make sure output is same format as q1 100 | if ( q1type==1 || (q1type==3 && q2type==1) ) 101 | q_out=q_out.'; 102 | end 103 | 104 | % NOTE that the following algorithm proved to be slower than the one used 105 | % above: 106 | % 107 | % q_out = zeros(size(q1)); 108 | % 109 | % q_out(:,1:3) = ... 110 | % [q1(:,4) q1(:,4) q1(:,4)].*q2(:,1:3) + ... 111 | % [q2(:,4) q2(:,4) q2(:,4)].*q1(:,1:3) + ... 112 | % cross(q1(:,1:3), q2(:,1:3)); 113 | % 114 | % q_out(:,4) = q1(:,4).*q2(:,4) - dot(q1(:,1:3), q2(:,1:3), 2); 115 | 116 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeSE3Mat.m: -------------------------------------------------------------------------------- 1 | % SE3 in 4x4 matrix 2 | % 3 | % the algebra is: omega linear 4 | % 5 | % Emanuele Ruffaldi 2017 @ SSSA 6 | function m = makeSE3Mat() 7 | 8 | m1= makeRot(); 9 | m = []; 10 | m.type = {'SE3MatGlobal'}; 11 | m.inv = @se3inv; 12 | m.prod = @(x,y) mflat(munflat(x)*munflat(y)); 13 | m.m1 = makeRot(); 14 | m.log = @se3log; 15 | m.exp = @se3exp; 16 | 17 | m.delta = @(x,y) se3log(munflat(x)*munflat(se3inv(y))); 18 | m.pack = @(x) reshape(x,[],1); 19 | m.unpack = @(x) reshape(x,4,4); 20 | m.transport = @(X,t,Y) t; 21 | m.flat = @(x) reshape(x,1,[]); 22 | m.unflat = @(x) reshape(x,4,4); 23 | m.islie = 1; 24 | 25 | % Jacobian(y) * X 26 | m.step = @(X,y) mflat(munflat(m.exp(y))*munflat(X)); 27 | m.group = 16; 28 | m.alg = 6; 29 | m.count = 1; 30 | m.s = int_manisetup([],[],m); 31 | 32 | function y = se3log(x) 33 | 34 | t = getpos(x); 35 | R = getrot(x); 36 | 37 | theta = acos((trace(R)-1)/2); %acos(max(-1,min((trace(R)-1)/2,1))); 38 | if isreal(theta) == 0 39 | R = R/abs(det(R)); 40 | theta = acos((trace(R)-1)/2); 41 | end 42 | 43 | if abs(theta) < 1e-10 44 | B = 0.5; 45 | SO = (1/(2))*(R-R'); % =skew(omega) 46 | iV = eye(3); % use directly skew of omega 47 | else 48 | A = sin(theta)/theta; 49 | B = (1-cos(theta))/(theta*theta); 50 | SO = (1/(2*A))*(R-R'); % =skew(omega) 51 | %?? 52 | % syms x real 53 | % A = sin(x)/x 54 | % B= (1-cos(x))/(x*x) 55 | % Q=1/(x*x)*(1 - A/2/B) 56 | % taylor(Q,x,0) 57 | % x^4/30240 + x^2/720 + 1/12 58 | Q= 1/(theta^2)*(1 - A/2/B); 59 | iV = eye(3) - 1/2*SO + Q*SO^2; % use directly skew of omega 60 | end 61 | omega = [SO(3,2) SO(1,3) SO(2,1)]; 62 | 63 | %y = [m1.log(getrot(x)), getpos(x)]; % not exact 64 | u = iV*t'; 65 | y = [omega(:);u(:)]'; 66 | 67 | function y = se3exp(x) 68 | 69 | omega = getomega(x); 70 | u = getvel(x); 71 | 72 | theta = norm(omega); 73 | if theta < 1e-12 74 | % Original 75 | % A = 1; 76 | % B = 0.5; 77 | % C = 1/6; 78 | % S = zeros(3); 79 | % R = eye(3) + A*S + B*S^2; 80 | % V = eye(3) + B*S + C*S^2; 81 | 82 | N = 10; 83 | R = eye(3); 84 | xM = eye(3); 85 | cmPhi = skew(omega); 86 | V = eye(3); 87 | pxn = eye(3); 88 | for n = 1:N 89 | xM = xM * (cmPhi / n); 90 | pxn = pxn*cmPhi/(n + 1); 91 | R = R + xM; 92 | V = V + pxn; 93 | end 94 | 95 | % Project the resulting rotation matrix back onto SO(3) 96 | R = R / sqrtm( R'*R ) ; 97 | 98 | else 99 | %Original 100 | if 1==1 101 | A = sin(theta)/theta; 102 | B = (1-cos(theta))/(theta^2); 103 | C = (theta-sin(theta))/(theta^3); 104 | S = skew(omega); 105 | R = eye(3) + A*S + B*S^2; 106 | V = eye(3) + B*S + C*S^2; 107 | else 108 | %Barfoot 109 | 110 | axis = omega/theta; 111 | cp = cos(theta); 112 | sp = sin(theta); 113 | cph = (1 - cos(theta))/theta; 114 | sph = sin(theta)/theta; 115 | sa = skew(axis); 116 | 117 | R = cp*eye(3) + (1-cp)*axis*(axis') + sp*sa; 118 | V = sph * eye(3) + (1 - sph) * axis * (axis') + cph * sa; 119 | end 120 | 121 | end 122 | 123 | y = eye(4); 124 | y(1:3,1:3) = R; 125 | y(1:3,4) = V*u(:); 126 | y = mflat(y); 127 | 128 | 129 | 130 | function u = mflat(x) 131 | 132 | u = x(:)'; 133 | 134 | function u = munflat(x) 135 | 136 | u = reshape(x,4,4); 137 | 138 | function y = se3inv(x) 139 | 140 | R = getrot(x); 141 | y = eye(4); 142 | y(1:3,1:3) = R'; 143 | y(1:3,4) = -y(1:3,1:3)*getpos(x)'; 144 | y = mflat(y); 145 | 146 | function u = ubuild2(rot,pos) 147 | 148 | u = eye(4); 149 | u(1:3,1:3) = reshape(rot,3,3); 150 | u(1:3,4) = pos; 151 | 152 | function R = getrot(x) 153 | 154 | M = munflat(x); 155 | R = M(1:3,1:3); 156 | 157 | function p = getpos(x) 158 | 159 | p = x(13:15); 160 | p = p(:)'; 161 | 162 | function u = getomega(x) 163 | 164 | u = x(1:3); 165 | 166 | function u = getvel(x) 167 | 168 | u = x(4:6); -------------------------------------------------------------------------------- /examples/exampleSE3MatAug.m: -------------------------------------------------------------------------------- 1 | % We build first the manifolds explicitly stating the state manifold x (Mx) 2 | % and the augmented Mxa 3 | Mx = {makeSE3Mat(),makeRn(3),makeRn(3)}; 4 | Mxa = [Mx,makeRn(6)]; 5 | Mz = makeSE3Mat(); 6 | 7 | % Then we proceed to the setup of the manifold in usable way, also using 8 | % the code generation for speedup of the processing 9 | mx = manisetup(makeGenProduct('se3mat_e3_e3',Mx{:})); 10 | mxa = manisetup(makeGenProduct('se3mat_e3_e3_aug',Mxa{:})); % as makeProduct(....,+1) 11 | mz = manisetup(Mz); 12 | mzr = manisetup(makeRot()); % only rotation, loss of position 13 | mxt = makeSE3Mat(); % helper without the need of setup 14 | 15 | % We build these structures for supporting the Unscented transformation. We 16 | % need them for the manifold x in the process, and the augmented xa in the 17 | % observation 18 | mx.wsigma = ut_mweights2(mx.group,mx.alg,0.5); 19 | mx.wsigma.sqrt = @svdsqrt; 20 | mxa.wsigma = ut_mweights2(mxa.group,mxa.alg,0.5); 21 | mxa.wsigma.sqrt = @svdsqrt; 22 | 23 | 24 | % initial state and noise definition 25 | x0 = mx.step(mx.exp([0,0,0, 0,1,0, 0,0,0, 0,0,0]'),[pi/2,0.2,0, 0,0,0, 0,0,0, 0,0,0]'); 26 | P0 = 0.5*eye(mx.alg); 27 | Q = 0.01*eye(mx.alg); % process noi!se 28 | R = 1e-3*eye(mz.alg); % measure noise 29 | Rr = R(1:3,1:3); % only rotations 30 | 31 | 32 | %% Generate the Observation by simulation 33 | z0 = mz.exp([pi/2,0,0, 0,0.1,1]'); 34 | zobsval = zeros(16,200); 35 | v0 = zeros(6,1); 36 | v0(4) = 0.1; 37 | v0(1) = 0.2; 38 | zobsval(:,1) = z0; 39 | lzobsval = zeros(mz.alg,size(zobsval,2)); 40 | lzobsval(:,1) = mz.log(z0); 41 | for I=2:size(zobsval,2) 42 | v0(2) = sin(I/100); 43 | zobsval(:,I) = mz.step(zobsval(:,I-1),v0); 44 | lzobsval(:,I) = mz.log(zobsval(:,I)); 45 | end 46 | zobs = @(t) zobsval(:,t); 47 | 48 | 49 | % observation is identity 50 | % process is the integral 51 | dt = 0.1; 52 | 53 | %% Kalman Setup 54 | % functions work expanding each primitive manifold with their type (e.g. matrix 4x4) 55 | % if the output is made of multiple e manifolds use deal for returning 56 | % everything 57 | f_fx = @(Tk,wk,vk) deal(mxt.step(Tk,[wk;vk]),wk,vk); % Xk = (Tk,wk,vk) 58 | h_fx = @(Tk,wk,vk,ek) mxt.step(Tk,ek); 59 | hr_fx = @(Tk,wk,vk,ek) mxt.step(Tk(1:3,1:3),ek(1:3)); 60 | tic 61 | % loop 62 | deltas = zeros(200,mz.alg); 63 | states = zeros(size(deltas,1),mx.group); 64 | lstates = zeros(size(deltas,1),mx.alg); 65 | usereduxspace = 0; 66 | Rnonadditive = eye(mxa.alg-mx.alg); % generic formulation 67 | Rnonadditive = R; 68 | R = zeros(size(R)); % for testing augmentation 69 | 70 | for L=1:size(deltas,1) 71 | states(L,:) = x0; 72 | lstates(L,:) = mx.log(x0); % [ rodriguez(R) traslazione velocitàangolar velocitàlinear ] 73 | 74 | [xp,Pp] = manistatestep(mx,x0,P0,f_fx,Q); 75 | assert(size(xp,2)==1); 76 | 77 | 78 | % Build the augmented state by expanding the state with noise with 79 | % zero mean and provided covariance 80 | Ppa = blkdiag(Pp,Rnonadditive); 81 | xpa = [xp; zeros(mxa.alg-mx.alg,1)]; % mean is zero 82 | [zm,Czz,Cxaz] = manievalh(mxa,mz,xpa,Ppa,h_fx); 83 | 84 | 85 | Pvv = Czz + R; 86 | z = zobs(L); 87 | 88 | K = Cxaz/Pvv; 89 | Ppa = (Ppa - K * Pvv * K'); 90 | delta = mz.delta(z,zm); 91 | 92 | xpanew = mx.step(xpa,(K*delta')); 93 | % extract x0 from xpanew because we discard the augmentation 94 | % extract P0 from Ppa because we discard augmentation 95 | x0 = xpanew(1:mx.group); 96 | P0 = Ppa(1:mx.alg,1:mx.alg); 97 | 98 | deltas(L,:) = delta; 99 | end 100 | %% 101 | toc 102 | figure(1) 103 | subplot(3,1,1); 104 | plot(deltas(:,1:3)) 105 | title('Deltas observation-prediction'); 106 | xlabel('sample'); 107 | subplot(3,1,2); 108 | plot(deltas(:,4:6)) 109 | title('Deltas observation-prediction'); 110 | xlabel('sample'); 111 | subplot(3,1,3); 112 | plot(sum(deltas.^2,2)); 113 | title('Norm of error'); 114 | xlabel('sample'); 115 | figure(2) 116 | plot(states(10:end,:)) 117 | title('All states as matrix'); 118 | 119 | %% Latency estimation 120 | dd = zeros(1,6); 121 | for J=1:6 122 | figure(2+J); 123 | dd(J) = finddelay(lstates(J,:),lzobsval(:,J)); 124 | 125 | plot([lstates(J,:)';lzobsval(:,J)]); 126 | end 127 | disp('delay') 128 | dd 129 | disp('error') 130 | sqrt(meannonan(deltas.^2,1))' 131 | -------------------------------------------------------------------------------- /ukfmani/quaternions/test/test_isq.m: -------------------------------------------------------------------------------- 1 | %TEST_ISQ runs unit tests for the ISQ function. 2 | 3 | % Release: $Name: quaternions-1_3 $ 4 | % $Revision: 1.6 $ 5 | % $Date: 2009-07-24 19:14:44 $ 6 | 7 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 8 | 9 | test_title = 'isq'; 10 | disp_test_title(test_title); 11 | 12 | failures=0; 13 | 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | disp_test_name('Insufficient Arguments'); 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | expected_err = 'isq() requires one input argument'; 18 | fct_call = 'isq'; 19 | failures=failures+check_err(fct_call, expected_err); 20 | 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | disp_test_name('Input is a scalar'); 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | truth_value = '0'; 25 | test_value = 'isq(1)'; 26 | failures=failures+check_value(truth_value, test_value); 27 | 28 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 29 | disp_test_name('Input is 2D, but neither dim is size 4'); 30 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 31 | truth_value = '0'; 32 | test_value = 'isq(ones(3,5))'; 33 | failures=failures+check_value(truth_value, test_value); 34 | 35 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 36 | disp_test_name('Input is 3D'); 37 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 38 | truth_value = '0'; 39 | test_value = 'isq(ones(3,5,4))'; 40 | failures=failures+check_value(truth_value, test_value); 41 | 42 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 43 | disp_test_name('Input is 4xN, N~=4'); 44 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 45 | truth_value = '1'; 46 | test_value = 'isq(rand(4,5)-0.5)'; 47 | failures=failures+check_value(truth_value, test_value); 48 | 49 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 50 | disp_test_name('Input is 4x4, only columns are normalized'); 51 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 52 | set_val('M', '1+rand(4)') 53 | set_val('qmag', 'sqrt(sum(M.^2,1))') 54 | set_val('qmag', '[qmag; qmag; qmag; qmag]') 55 | set_val('q', 'M./qmag') 56 | truth_value = '1'; 57 | test_value = 'isq(q)'; 58 | failed=check_value(truth_value, test_value); 59 | if failed; error('FAILED'); end 60 | failures=failures+failed; 61 | 62 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 63 | disp_test_name('Input is Nx4, N~=4'); 64 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 65 | truth_value = '2'; 66 | test_value = 'isq(rand(3,4))'; 67 | failures=failures+check_value(truth_value, test_value); 68 | 69 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 70 | disp_test_name('Input is 4x4, only rows are normalized'); 71 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 72 | set_val('M', '1+rand(4)') 73 | set_val('qmag', 'sqrt(sum(M.^2,2))') 74 | set_val('qmag', '[qmag qmag qmag qmag]') 75 | set_val('q', 'M./qmag') 76 | truth_value = '2'; 77 | test_value = 'isq(q)'; 78 | failed=check_value(truth_value, test_value); 79 | if failed; error('FAILED'); end 80 | failures=failures+failed; 81 | 82 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 83 | disp_test_name('Input is 4x4, both rows and columns are normalized'); 84 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 85 | set_val('q', '0.5*ones(4)') 86 | truth_value = '3'; 87 | test_value = 'isq(q)'; 88 | failures=failures+check_value(truth_value, test_value); 89 | 90 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 91 | disp_test_name('Input is 4x4, neither rows nor columns are normalized'); 92 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 93 | set_val('q', 'ones(4)') 94 | truth_value = '3'; 95 | test_value = 'isq(q)'; 96 | failures=failures+check_value(truth_value, test_value); 97 | 98 | 99 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 100 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 101 | disp_num_failures(test_title, failures) 102 | 103 | if failures; error('FAILED'); end 104 | -------------------------------------------------------------------------------- /examples/exampleSE3MatAug2.m: -------------------------------------------------------------------------------- 1 | % We build first the manifolds explicitly stating the state manifold x (Mx) 2 | % and the augmented Mxa 3 | Mx = {makeSE3Mat(),makeRn(3),makeRn(3)}; 4 | Mxa = [Mx,makeRn(6)]; 5 | Mz = makeSE3Mat(); 6 | 7 | % Then we proceed to the setup of the manifold in usable way, also using 8 | % the code generation for speedup of the processing 9 | mx = manisetup(makeGenProduct('se3mat_e3_e3',Mx{:})); 10 | mxa = manisetup(makeGenProduct('se3mat_e3_e3_aug',Mxa{:})); % as makeProduct(....,+1) 11 | mz = manisetup(Mz); 12 | mzr = manisetup(makeRot()); % only rotation, loss of position 13 | mxt = makeSE3Mat(); % helper without the need of setup 14 | 15 | % We build these structures for supporting the Unscented transformation. We 16 | % need them for the manifold x in the process, and the augmented xa in the 17 | % observation 18 | mxa.wsigma = ut_mweights2(mxa.group,mxa.alg,0.5); 19 | mxa.wsigma.sqrt = @svdsqrt; 20 | %mxa.wsigma.WC = mxa.wsigma.W; % Use this for testing the assumption 21 | 22 | 23 | % initial state and noise definition 24 | x0 = mx.step(mx.exp([0,0,0, 0,1,0, 0,0,0, 0,0,0]'),[pi/2,0.2,0, 0,0,0, 0,0,0, 0,0,0]'); 25 | P0 = 0.5*eye(mx.alg); 26 | %Q = 0.01*eye(mx.alg); % process noi!se 27 | Qnonadditive = 0.01*eye(mxa.alg-mx.alg); 28 | R = 1e-3*eye(mz.alg); % measure noise 29 | Rr = R(1:3,1:3); % only rotations 30 | 31 | 32 | %% Generate the Observation by simulation 33 | z0 = mz.exp([pi/2,0,0, 0,0.1,1]'); 34 | zobsval = zeros(16,200); 35 | v0 = zeros(6,1); 36 | v0(4) = 0.1; 37 | v0(1) = 0.2; 38 | zobsval(:,1) = z0; 39 | lzobsval = zeros(mz.alg,size(zobsval,2)); 40 | lzobsval(:,1) = mz.log(z0); 41 | for I=2:size(zobsval,2) 42 | v0(2) = sin(I/100); 43 | zobsval(:,I) = mz.step(zobsval(:,I-1),v0); 44 | lzobsval(:,I) = mz.log(zobsval(:,I)); 45 | end 46 | zobs = @(t) zobsval(:,t); 47 | 48 | 49 | % observation is identity 50 | % process is the integral 51 | dt = 0.1; 52 | 53 | %% Kalman Setup 54 | % functions work expanding each primitive manifold with their type (e.g. matrix 4x4) 55 | % if the output is made of multiple e manifolds use deal for returning 56 | % everything 57 | %f_fx = @(Tk,wk,vk,ek) deal(mxt.step(mxt.step(Tk,[wk;vk]),ek),wk,vk,ek); % Xk = (Tk,wk,vk) 58 | f_fx = @(Tk,wk,vk,ek) deal(mxt.step(Tk,[wk;vk]+ek),wk,vk,ek); 59 | h_fx = @(Tk,wk,vk,ek) mxt.step(Tk,ek); 60 | hr_fx = @(Tk,wk,vk,ek) mxt.step(Tk(1:3,1:3),ek(1:3)); 61 | tic 62 | % loop 63 | deltas = zeros(200,mz.alg); 64 | states = zeros(size(deltas,1),mx.group); 65 | lstates = zeros(size(deltas,1),mx.alg); 66 | usereduxspace = 0; 67 | Rnonadditive = eye(mxa.alg-mx.alg); % generic formulation 68 | Rnonadditive = R; 69 | Q = []; 70 | R = zeros(size(R)); % for testing augmentation 71 | 72 | for L=1:size(deltas,1) 73 | states(L,:) = x0; 74 | lstates(L,:) = mx.log(x0); % [ rodriguez(R) traslazione velocitàangolar velocitàlinear ] 75 | 76 | Pa = blkdiag(P0,Qnonadditive); 77 | xa = [x0; zeros(mxa.alg-mx.alg,1)]; % mean is zero 78 | [xpa,Ppa,XSpa,XSpa_chi] = manistatestep(mxa,xa,Pa,f_fx); % predicted state, and sigma point and their differential 79 | 80 | 81 | % Build the augmented state by expanding the state with noise with 82 | % zero mean and provided covariance 83 | [zm,Czz,Cxaz] = manievalhsigma(mxa,mz,XSpa,XSpa_chi,h_fx); % pass directly the sigma points and the differential 84 | 85 | 86 | Pvv = Czz + R; 87 | z = zobs(L); 88 | 89 | K = Cxaz/Pvv; 90 | Ppa = (Ppa - K * Pvv * K'); 91 | delta = mz.delta(z,zm); 92 | xpanew = mx.step(xpa,(K*delta')); 93 | % extract x0 from xpanew because we discard the augmentation 94 | % extract P0 from Ppa because we discard augmentation 95 | x0 = xpanew(1:mx.group); 96 | P0 = Ppa(1:mx.alg,1:mx.alg); 97 | 98 | deltas(L,:) = delta; 99 | end 100 | %% 101 | toc 102 | figure(1) 103 | subplot(3,1,1); 104 | plot(deltas(:,1:3)) 105 | title('Deltas observation-prediction'); 106 | xlabel('sample'); 107 | subplot(3,1,2); 108 | plot(deltas(:,4:6)) 109 | title('Deltas observation-prediction'); 110 | xlabel('sample'); 111 | subplot(3,1,3); 112 | plot(sum(deltas.^2,2)); 113 | title('Norm of error'); 114 | xlabel('sample'); 115 | figure(2) 116 | plot(states(10:end,:)) 117 | title('All states as matrix'); 118 | 119 | %% Latency estimation 120 | dd = zeros(1,6); 121 | for J=1:6 122 | figure(2+J); 123 | dd(J) = finddelay(lstates(J,:),lzobsval(:,J)); 124 | 125 | plot([lstates(J,:)';lzobsval(:,J)]); 126 | end 127 | disp('delay') 128 | dd 129 | disp('error') 130 | sqrt(meannonan(deltas.^2,1))' 131 | -------------------------------------------------------------------------------- /examples/exampleSE3Mat.m: -------------------------------------------------------------------------------- 1 | % TODO: explain better the fact that angular velocity of the state is 2 | % GLOBAL rather than LOCAL 3 | 4 | % build input and output manifolds 5 | mx = manisetup(makeGenProduct('se3mat_e3_e3',makeSE3Mat(),makeRn(3),makeRn(3))); 6 | %mx = manisetup(makeProduct(makeSE3Mat(),makeRn(3),makeRn(3))); 7 | 8 | mz = manisetup(makeSE3Mat()); 9 | mzr = manisetup(makeRot()); % only rotation, loss of position 10 | 11 | mxt = makeSE3Mat(); % helper without the need of setup 12 | 13 | % initial state and noise definition 14 | x0 = mx.step(mx.exp([0,0,0, 0,1,0, 0,0,0, 0,0,0]'),[pi/2,0.2,0, 0,0,0, 0,0,0, 0,0,0]'); 15 | P0 = 0.5*eye(mx.alg); 16 | Q = 0.01*eye(mx.alg); % process noi!se 17 | R = 1e-3*eye(mz.alg); % measure noise 18 | Rr = R(1:3,1:3); % only rotations 19 | 20 | z0 = mz.exp([pi/2,0,0, 0,0.1,1]'); 21 | zobsval = zeros(16,200); 22 | 23 | % costant se3 velocity 24 | v0 = zeros(6,1); 25 | v0(4) = 0.1; 26 | v0(1) = 0.2; 27 | zobsval(:,1) = z0; 28 | lzobsval = zeros(mz.alg,size(zobsval,2)); 29 | lzobsval(:,1) = mz.log(z0); 30 | for I=2:size(zobsval,2) 31 | v0(2) = sin(I/100); 32 | zobsval(:,I) = mz.step(zobsval(:,I-1),v0); 33 | lzobsval(:,I) = mz.log(zobsval(:,I)); 34 | end 35 | zobs = @(t) zobsval(:,t); 36 | 37 | wsigmax = ut_mweights2(mx.group,mx.alg,0.5); 38 | wsigmax.sqrt = @svdsqrt; 39 | 40 | % observation is identity 41 | % process is the integral 42 | dt = 0.1; 43 | 44 | % functions work in the real group space 45 | f_fx = @(Tk,wk,vk) deal(mxt.step(Tk,[wk;vk]),wk,vk); % Xk = (Tk,wk,vk) 46 | h_fx = @(Tk,wk,vk) Tk; 47 | hr_fx = @(Tk,wk,vk) Tk(1:3,1:3); 48 | tic 49 | % loop 50 | deltas = zeros(200,mz.alg); 51 | states = zeros(size(deltas,1),mx.group); 52 | lstates = zeros(size(deltas,1),mx.alg); 53 | haspos = ones(200,1); 54 | haspos(4:4:end) = 0; 55 | haspos(5:4:end) = 0; 56 | haspos(6:4:end) = 0; 57 | usereduxspace = 0; 58 | 59 | for L=1:size(deltas,1) 60 | states(L,:) = x0; 61 | lstates(L,:) = mx.log(x0); % [ rodriguez(R) traslazione velocitàangolar velocitàlinear ] 62 | 63 | % check if x0s is available and reuse it below 64 | [xp,Pp] = manistatestep(mx,x0,P0,f_fx,Q,wsigmax); 65 | assert(size(xp,2)==1); 66 | if haspos(L) == 0 && usereduxspace 67 | [zm,Czz,Cxz] = manievalh(mx,mzr,xp,Pp,hr_fx,wsigmax); 68 | assert(size(zm,2)==1); 69 | 70 | % Kalman update with observation noise (additive) 71 | Pvv = Czz + Rr; 72 | K = Cxz/Pvv; 73 | P0 = (Pp - K * Pvv * K'); 74 | 75 | fullobs_mat = mz.unpack( zobs(L)); % from SE3 16x1 to SE3 4x4 76 | 77 | deltar = mzr.delta(mzr.pack(fullobs_mat(1:3,1:3)),zm); 78 | x0 = mx.step(xp,(K*deltar')'); 79 | delta = [deltar, NaN,NaN,NaN]; % ONLY for viz 80 | else 81 | [zm,Czz,Cxz] = manievalh(mx,mz,xp,Pp,h_fx,wsigmax); 82 | assert(size(zm,2)==1); 83 | 84 | % Kalman update with observation noise (additive) 85 | if haspos(L) == 0 86 | % maximal noise except for the rotation part 87 | Rr = 1e10*ones(size(R)); 88 | Rr(1:3,1:3) = R(1:3,1:3); 89 | Rr(1:3,4:6) = 0; 90 | Czz(1:3,4:6) = 0; 91 | Czz(4:6,1:3) = 0; 92 | Pvv = Czz + Rr; 93 | % zero out the observation (cannot use NaN) 94 | zc = mz.unpack(zobs(L)); 95 | zc{1}(1:3,4) = 0; 96 | z = mz.pack(zc); 97 | else 98 | Pvv = Czz + R; 99 | z = zobs(L); 100 | end 101 | assert(size(z,1) == mz.group); 102 | assert(size(z,2) == 1); 103 | K = Cxz/Pvv; 104 | %P0 = (eye(size(P0)) - K * Pvv * K') * P0; 105 | P0t = Pp - K*Pvv*K'; 106 | delta = K*mz.delta(z,zm)'; 107 | 108 | if 1==0 109 | % in some manifolds we cannot sum directly the delta 110 | % check Christoph Hertzberg Information Fusion 2013 111 | % NOT IMPLEMENTED YET [x0,P0,x0s] = manirelsum(xp,delta,P0t); 112 | else 113 | x0 = mx.step(xp,delta); 114 | P0 = P0t; 115 | end 116 | 117 | end 118 | deltas(L,:) = delta; 119 | end 120 | %% 121 | toc 122 | figure(1) 123 | subplot(3,1,1); 124 | plot(deltas(:,1:3)) 125 | title('Deltas observation-prediction'); 126 | xlabel('sample'); 127 | subplot(3,1,2); 128 | plot(deltas(:,4:6)) 129 | title('Deltas observation-prediction'); 130 | xlabel('sample'); 131 | subplot(3,1,3); 132 | plot(sum(deltas.^2,2)); 133 | title('Norm of error'); 134 | xlabel('sample'); 135 | figure(2) 136 | plot(states(10:end,:)) 137 | title('All states as matrix'); 138 | 139 | %% Latency estimation 140 | dd = zeros(1,6); 141 | for J=1:6 142 | figure(2+J); 143 | dd(J) = finddelay(lstates(J,:),lzobsval(:,J)); 144 | 145 | plot([lstates(J,:)';lzobsval(:,J)]); 146 | end 147 | disp('delay') 148 | dd 149 | disp('error') 150 | sqrt(meannonan(deltas.^2,1))' 151 | -------------------------------------------------------------------------------- /task_headpose/headPose_estim.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | clc; 3 | close all; 4 | %% load and divide 5 | load 'head_pose'; 6 | 7 | %addpath 'C:\Users\bigDaddy\Dropbox\project_ale\pose_estimator\ukfmanifold-master' 8 | %addpath '/home/cattaneo/Dropbox/project_ale/pose_estimator/ukfmanifold-master' 9 | addpath C:\Users\ing_Cattaneo\Dropbox\project_ale\pose_estimator\ukfmanifold-master; 10 | addpath C:\Users\ing_Cattaneo\Documents\MATLAB\lib\quaternions-1.3\quaternions; 11 | Tx=headpose(:,10); 12 | Ty=headpose(:,11); 13 | Tz=headpose(:,12); 14 | Px=headpose(:,13); 15 | Py=headpose(:,14); 16 | Pz=headpose(:,15); 17 | 18 | % build input and output manifolds 19 | 20 | 21 | 22 | mx = manisetup(makeProduct(makeSE3Mat(),makeRn(3),makeRn(3))); 23 | mz = manisetup(makeProduct(makeSE3Mat())); 24 | %% data 25 | start=20; 26 | stop=1540; 27 | 28 | Rr= headpose(start:stop,10:12); 29 | Tr= headpose(start:stop,13:15); 30 | zobs=nan(size(Tr,1),(4+3)); 31 | missingdata = sum(isnan(headpose(start:stop,10:15)),2) > 0; 32 | 33 | 34 | 35 | % plot data before ukf 36 | figure, 37 | subplot(3,1,1); plot(Tr(:,1)); grid on; legend('Tx'); title('x_{traslation}'); xlabel('sec'); ylabel('pixel'); 38 | subplot(3,1,2); plot(Tr(:,2)); grid on; legend('Ty'); title('y_{traslation}'); xlabel('sec'); ylabel('pixel'); 39 | subplot(3,1,3); plot(Tr(:,3)); grid on; legend('Tz'); title('z_{traslation}'); xlabel('sec'); ylabel('pixel'); 40 | 41 | figure, 42 | subplot(3,1,1); plot(Rr(:,1)); grid on; legend('Rx'); title('x_{rot}'); xlabel('sec'); ylabel('pixel'); 43 | subplot(3,1,2); plot(Rr(:,2)); grid on; legend('Ry'); title('y_{rot}'); xlabel('sec'); ylabel('pixel'); 44 | subplot(3,1,3); plot(Rr(:,3)); grid on; legend('Rz'); title('z_{rot}'); xlabel('sec'); ylabel('pixel'); 45 | close all; 46 | %% 47 | for i=1:stop-start 48 | zobs(i,:)=[eul2quat(Rr(i,:)),Tr(i,:)]; 49 | end 50 | 51 | %% UKF pose 52 | 53 | N=stop; 54 | 55 | % initial state and noise definition rotation and lineaqr traslation 56 | initialguess=eul2rotm(Rr(start,:)); 57 | x0 = mx.step(mx.exp([initialguess(1,:), initialguess(2,:),initialguess(3,:), 0,0,0]),[Rr(start,:), Tr(start,:), 0,0,0, 0,0,0]); 58 | 59 | P0 = 0.5*eye(mx.alg); 60 | Q = 0.01*eye(mx.alg); % process noi!se 61 | R = 1e-3*eye(mz.alg); % measure noise 62 | 63 | wsigmax = ut_mweights2(mx.group,mx.alg,0.5); 64 | wsigmax.sqrt = @svdsqrt; 65 | 66 | % observation is identity 67 | % process is the integral 68 | dt = 0.1; 69 | 70 | 71 | f_fx = @(Tk,wk,vk) deal(mxt.step(Tk,[wk,vk]),wk,vk); % Xk = (Tk,wk,vk) 72 | h_fx = @(Tk,wk,vk) Tk; 73 | 74 | tic 75 | % loop 76 | deltas = zeros(N,mz.alg); 77 | states = zeros(size(deltas,1),mx.group); 78 | lstates = zeros(size(deltas,1),mx.alg); 79 | for L=1:size(deltas,1) 80 | states(L,:) = x0; 81 | lstates(L,:) = mx.log(x0); 82 | 83 | 84 | [xp,Pp] = manistatestep(mx,x0,P0,f_fx,Q,wsigmax); 85 | [zm,Czz,Cxz] = manievalh(mx,mz,xp,Pp,h_fx,wsigmax); 86 | 87 | if missingdata(L) == 1 88 | % Kalman update with observation noise (additive) 89 | Pvv = Czz + R; 90 | K = Cxz/Pvv; 91 | P0 = (eye(size(P0)) - K * Pvv * K') * P0; 92 | delta = mz.delta(zobs(L),zm); 93 | x0 = mx.step(xp,(K*delta')'); 94 | deltas(L,:) = delta; 95 | end 96 | 97 | 98 | 99 | end 100 | %% plotting graph and data converter 101 | 102 | Rr1=zeros(size(states,1),3); 103 | for i=1:size(states,1) 104 | Rr1(i,:)=quat2eul(states(i,1:4)); 105 | end 106 | 107 | figure('name','measures'); 108 | subplot(6,1,1); plot(deltas(:,1)); grid on;legend('Rx'); xlabel('sec'); ylabel('pixel'); 109 | subplot(6,1,2); plot(deltas(:,2)); grid on;legend('Ry'); xlabel('sec'); ylabel('pixel'); 110 | subplot(6,1,3); plot(deltas(:,3)); grid on;legend('Rz'); xlabel('sec'); ylabel('pixel'); 111 | subplot(6,1,4); plot(deltas(:,4)); grid on;legend('x'); xlabel('sec'); ylabel('pixel'); 112 | subplot(6,1,5); plot(deltas(:,5)); grid on;legend('y'); xlabel('sec'); ylabel('pixel'); 113 | subplot(6,1,6); plot(deltas(:,6)); grid on;legend('z'); xlabel('sec'); ylabel('pixel'); 114 | 115 | 116 | figure('name','stima_rotation'); 117 | subplot(3,1,1); plot(Rr1(:,1)); hold on; plot(Rr(start:stop,1)); grid on; xlabel('sec'); ylabel('pixel'); legend('sR_x','R_x'); 118 | subplot(3,1,2); plot(Rr1(:,2)); hold on; plot(Rr(start:stop,2)); grid on; xlabel('sec'); ylabel('pixel'); legend('sR_y','R_y'); 119 | subplot(3,1,3); plot(Rr1(:,3)); hold on; plot(Rr(start:stop,3)); grid on; xlabel('sec'); ylabel('pixel'); legend('sR_z','R_z'); 120 | 121 | figure('name','stima_traslation'); 122 | subplot(3,1,1); plot(Tr(start:stop,1),'-'); hold on; plot(states(:,8),'--'); grid on; xlabel('sec'); ylabel('pixel'); legend('x','s_x'); 123 | subplot(3,1,2); plot(Tr(start:stop,2),'-'); hold on; plot(states(:,9),'--'); grid on; xlabel('sec'); ylabel('pixel'); legend('y','s_y'); 124 | subplot(3,1,3); plot(Tr(start:stop,3),'-'); hold on; plot(states(:,10),'--'); grid on; xlabel('sec'); ylabel('pixel'); legend('z','s_z'); -------------------------------------------------------------------------------- /manse3mat_e3_e3.m: -------------------------------------------------------------------------------- 1 | 2 | function m = customManifold() 3 | m = []; 4 | m.type = 'special'; 5 | m.inv = @minvert; 6 | m.prod = @mproduct; 7 | m.log = @mlog; 8 | m.exp = @mexp; 9 | 10 | m.delta = @mdelta; 11 | m.step = @mstep; 12 | m.group = 22; 13 | m.alg = 12; 14 | m.count = 3; 15 | m.pack = @mpack; 16 | m.unpack = @munpack; 17 | 18 | end 19 | 20 | function z = minv(x1) 21 | z = zeros(22,1); 22 | z(1:16) = se3mat_inv(x1(1:16)); 23 | z(17:19) = -x1(17:19); 24 | z(20:22) = -x1(20:22); 25 | end 26 | 27 | function z = mproduct(x1,x2) 28 | z = zeros(22,1); 29 | z(1:16) = reshape(reshape(x1(1:16),4,4)*reshape(x2(1:16),4,4),1,[]); 30 | z(17:19) = x1(17:19)+x2(17:19); 31 | z(20:22) = x1(20:22)+x2(20:22); 32 | 33 | end 34 | 35 | function z = mlog(x1) 36 | z = zeros(12,1); 37 | z(1:6) = se3mat_log(x1(1:16)); 38 | z(7:9) = x1(17:19); 39 | z(10:12) = x1(20:22); 40 | 41 | end 42 | 43 | function z = mexp(x1) 44 | z = zeros(22,1); 45 | z(1:16) = se3mat_exp(x1(1:6)); 46 | z(17:19) = x1(7:9); 47 | z(20:22) = x1(10:12); 48 | end 49 | 50 | function z = mdelta(x1,x2) 51 | z = zeros(12,1); 52 | z(1:6) = se3mat_log(reshape(x1(1:16),4,4)*reshape(se3mat_inv(x2(1:16)),4,4)); 53 | z(7:9) = x1(17:19)-x2(17:19); 54 | z(10:12) = x1(20:22)-x2(20:22); 55 | end 56 | 57 | function z = mstep(x1,x2) 58 | z = zeros(22,1); 59 | z(1:16) = reshape(reshape(se3mat_exp(x2(1:6)),4,4)*reshape(x1(1:16),4,4),1,[]); 60 | z(17:19) = x1(17:19)+x2(7:9); 61 | z(20:22) = x1(20:22)+x2(10:12); 62 | end 63 | 64 | function z = mpack(x1) 65 | z = zeros(22,size(x1,2)); 66 | for j=1:size(x1,2) 67 | z(1:16,j) = reshape(x1{1,j},1,[]); 68 | z(17:19,j) = x1{2,j}; 69 | z(20:22,j) = x1{3,j}; 70 | end 71 | end 72 | 73 | function z = munpack(x1) 74 | z = cell(3,size(x1,2)); 75 | for j=1:size(x1,2) 76 | z{1,j} = reshape(x1(1:16,j),4,4); 77 | z{2,j} = x1(17:19,j); 78 | z{3,j} = x1(20:22,j); 79 | end 80 | end 81 | 82 | 83 | function y = se3mat_inv(x) 84 | 85 | M = reshape(x,4,4); 86 | R = M(1:3,1:3); 87 | y = eye(4); 88 | y(1:3,1:3) = R'; 89 | y(1:3,4) = -y(1:3,1:3)*M(1:3,4); 90 | y = reshape(y,1,[]); 91 | end 92 | 93 | 94 | function y = se3mat_log(x) 95 | 96 | M = reshape(x,4,4); 97 | t = M(1:3,4); 98 | R = M(1:3,1:3); 99 | 100 | theta = acos((trace(R)-1)/2); %acos(max(-1,min((trace(R)-1)/2,1))); 101 | if isreal(theta) == 0 102 | R = R/abs(det(R)); 103 | theta = acos((trace(R)-1)/2); 104 | end 105 | 106 | if abs(theta) < 1e-10 107 | B = 0.5; 108 | SO = (1/(2))*(R-R'); % =skew(omega) 109 | iV = eye(3); % use directly skew of omega 110 | else 111 | A = sin(theta)/theta; 112 | B = (1-cos(theta))/(theta*theta); 113 | SO = (1/(2*A))*(R-R'); % =skew(omega) 114 | %?? 115 | % syms x real 116 | % A = sin(x)/x 117 | % B= (1-cos(x))/(x*x) 118 | % Q=1/(x*x)*(1 - A/2/B) 119 | % taylor(Q,x,0) 120 | % x^4/30240 + x^2/720 + 1/12 121 | Q= 1/(theta^2)*(1 - A/2/B); 122 | iV = eye(3) - 1/2*SO + Q*SO^2; % use directly skew of omega 123 | end 124 | omega = [SO(3,2) SO(1,3) SO(2,1)]; 125 | 126 | %y = [m1.log(getrot(x)), getpos(x)]; % not exact 127 | u = iV*t; 128 | y = [omega(:);u(:)]'; 129 | end 130 | 131 | function y = se3mat_exp(x) 132 | 133 | omega = x(1:3); 134 | u = x(4:6); 135 | 136 | theta = norm(omega); 137 | if theta < 1e-12 138 | % Original 139 | % A = 1; 140 | % B = 0.5; 141 | % C = 1/6; 142 | % S = zeros(3); 143 | % R = eye(3) + A*S + B*S^2; 144 | % V = eye(3) + B*S + C*S^2; 145 | 146 | N = 10; 147 | R = eye(3); 148 | xM = eye(3); 149 | cmPhi = skew(omega); 150 | V = eye(3); 151 | pxn = eye(3); 152 | for n = 1:N 153 | xM = xM * (cmPhi / n); 154 | pxn = pxn*cmPhi/(n + 1); 155 | R = R + xM; 156 | V = V + pxn; 157 | end 158 | 159 | % Project the resulting rotation matrix back onto SO(3) 160 | R = R / sqrtm( R'*R ) ; 161 | 162 | else 163 | %Original 164 | if 1==1 165 | A = sin(theta)/theta; 166 | B = (1-cos(theta))/(theta^2); 167 | C = (theta-sin(theta))/(theta^3); 168 | S = skew(omega); 169 | R = eye(3) + A*S + B*S^2; 170 | V = eye(3) + B*S + C*S^2; 171 | else 172 | %Barfoot 173 | 174 | axis = omega/theta; 175 | cp = cos(theta); 176 | sp = sin(theta); 177 | cph = (1 - cos(theta))/theta; 178 | sph = sin(theta)/theta; 179 | sa = skew(axis); 180 | 181 | R = cp*eye(3) + (1-cp)*axis*(axis') + sp*sa; 182 | V = sph * eye(3) + (1 - sph) * axis * (axis') + cph * sa; 183 | end 184 | 185 | end 186 | 187 | y = eye(4); 188 | y(1:3,1:3) = R; 189 | y(1:3,4) = V*u(:); 190 | y = reshape(y,1,[]); 191 | 192 | end 193 | 194 | -------------------------------------------------------------------------------- /task_headpose/headPose_estim2.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | %% load and divide 4 | load 'head_pose'; 5 | 6 | %addpath 'C:\Users\bigDaddy\Dropbox\project_ale\pose_estimator\ukfmanifold-master' 7 | %addpath '/home/cattaneo/Dropbox/project_ale/pose_estimator/ukfmanifold-master' 8 | % addpath C:\Users\ing_Cattaneo\Dropbox\project_ale\pose_estimator\ukfmanifold-master; 9 | % addpath C:\Users\ing_Cattaneo\Documents\MATLAB\lib\quaternions-1.3\quaternions; 10 | Tx=headpose(:,10); 11 | Ty=headpose(:,11); 12 | Tz=headpose(:,12); 13 | Px=headpose(:,13); 14 | Py=headpose(:,14); 15 | Pz=headpose(:,15); 16 | 17 | % build input and output manifolds 18 | 19 | 20 | 21 | mx = manisetup(makeProduct(makeSE3Mat(),makeRn(3),makeRn(3))); 22 | mz = manisetup(makeProduct(makeSE3Mat())); 23 | 24 | mxt = makeSE3Mat(); % helper without the need of setup 25 | %% data 26 | start=20; 27 | stop=1540; 28 | 29 | Rr= headpose(start:stop,10:12); 30 | Tr= headpose(start:stop,13:15); 31 | Tr(:,2)=zeros(size(Tr,1),1); % colonna y tutti nan 32 | %zobs=nan(size(Tr,1),(4+3)); 33 | %missingdata = sum(isnan(headpose(start:stop,10:15)),2) > 0; 34 | t = (1:length(Rr))*(1/30); 35 | 36 | %ZYX 37 | 38 | figure, 39 | subplot(3,1,1); plot(t,Tr(:,1)); grid on; title('x_{traslation}'); xlabel('sec'); ylabel('pixel'); 40 | subplot(3,1,2); plot(t,Tr(:,2)); grid on; title('y_{traslation}'); xlabel('sec'); ylabel('pixel'); 41 | subplot(3,1,3); plot(t,Tr(:,3)); grid on; title('z_{traslation}'); xlabel('sec'); ylabel('pixel'); 42 | 43 | figure, 44 | subplot(3,1,1); plot(t,Rr(:,1)); grid on; title('Z_{rot}'); xlabel('sec'); ylabel('angles (deg)'); 45 | subplot(3,1,2); plot(t,Rr(:,2)); grid on; title('Y_{rot}'); xlabel('sec'); ylabel('angles (deg)'); 46 | subplot(3,1,3); plot(t,Rr(:,3)); grid on; title('X_{rot}'); xlabel('sec'); ylabel('angles (deg)'); 47 | 48 | zobs=zeros(size(Tr,1),16); 49 | val=zeros(4,4); 50 | %% 51 | % ZYX = roll yaw pitch 52 | for i=1:stop-start 53 | val=[eul2rotm(Rr(i,:)),Tr(i,:)';[0,0,0,1]]; 54 | zobs(i,:)=[val(1,:),val(2,:),val(3,:),val(4,:)]; 55 | end 56 | 57 | %% UKF pose 58 | 59 | N=stop-start; 60 | 61 | % initial state and noise definition rotation and lineaqr traslation 62 | initialguess=eul2rotm(Rr(start,:)); 63 | x0 = mx.step(mx.exp([initialguess(1,:), initialguess(2,:),initialguess(3,:), Tr(start,:)]),[Rr(start+1,:), Tr(start+1,:), 0,0,0, 0,0,0]); 64 | 65 | P0 = blkdiag(1e-6*eye(mx.alg-3),1e-6*eye(3)); 66 | Q = blkdiag(1e-2*eye(mx.alg-3),1e-4,1e-4,1e-1); % process noise 67 | R = blkdiag(1e-3*eye(mz.alg-3),1e1,1e-2,1e-1); % measure noise 68 | 69 | wsigmax = ut_mweights2(mx.group,mx.alg,0.5); 70 | wsigmax.sqrt = @svdsqrt; 71 | 72 | % observation is identity 73 | % process is the integral 74 | dt = 1/30*10; 75 | 76 | 77 | f_fx = @(Tk,wk,vk) deal(mxt.step(Tk,[wk,vk]),wk,vk); % Xk = (Tk,wk,vk) 78 | h_fx = @(Tk,wk,vk) Tk; 79 | 80 | tic 81 | % loop 82 | deltas = zeros(N,mz.alg); 83 | states = zeros(size(deltas,1),mx.group); 84 | lstates = zeros(size(deltas,1),mx.alg); 85 | for L=1:N 86 | states(L,:) = x0; 87 | lstates(L,:) = mx.log(x0); 88 | 89 | 90 | [xp,Pp] = manistatestep(mx,x0,P0,f_fx,Q,wsigmax); 91 | % if sum(isnan(xp),2) >0 92 | % xp=zeros(size(xp,1),size(xp,2)); 93 | % end 94 | % 95 | [zm,Czz,Cxz] = manievalh(mx,mz,xp,Pp,h_fx,wsigmax); 96 | 97 | %if missingdata(L) == 1 98 | % Kalman update with observation noise (additive) 99 | Pvv = Czz + R; 100 | K = Cxz/Pvv; 101 | P0 = (eye(size(P0)) - K * Pvv * K') * P0; 102 | delta = mz.delta(zobs(L,:),zm); 103 | x0 = mx.step(xp,(K*delta')'); 104 | deltas(L,:) = delta; 105 | %end 106 | 107 | 108 | 109 | end 110 | %% plotting graph and data converter 111 | toc 112 | Rr1=zeros(size(states,1),3); 113 | for i=1:size(states,1) 114 | Rr1(i,:)=quat2eul(states(i,1:4)); 115 | end 116 | 117 | figure('name','measures'); 118 | subplot(6,1,1); plot(deltas(:,1)); grid on;legend('Rx'); xlabel('sec'); ylabel('pixel'); 119 | subplot(6,1,2); plot(deltas(:,2)); grid on;legend('Ry'); xlabel('sec'); ylabel('pixel'); 120 | subplot(6,1,3); plot(deltas(:,3)); grid on;legend('Rz'); xlabel('sec'); ylabel('pixel'); 121 | subplot(6,1,4); plot(deltas(:,4)); grid on;legend('x'); xlabel('sec'); ylabel('pixel'); 122 | subplot(6,1,5); plot(deltas(:,5)); grid on;legend('y'); xlabel('sec'); ylabel('pixel'); 123 | subplot(6,1,6); plot(deltas(:,6)); grid on;legend('z'); xlabel('sec'); ylabel('pixel'); 124 | 125 | 126 | figure('name','stima_rotation'); 127 | subplot(3,1,1); plot(Rr1(:,1)); hold on; plot(Rr(1:end-1,1)); grid on; xlabel('sec'); ylabel('pixel'); legend('sR_x','R_x'); 128 | subplot(3,1,2); plot(Rr1(:,2)); hold on; plot(Rr(1:end-1,2)); grid on; xlabel('sec'); ylabel('pixel'); legend('sR_y','R_y'); 129 | subplot(3,1,3); plot(Rr1(:,3)); hold on; plot(Rr(1:end-1,3)); grid on; xlabel('sec'); ylabel('pixel'); legend('sR_z','R_z'); 130 | 131 | figure('name','stima_traslation'); 132 | subplot(3,1,1); plot(Tr(1:end-1,1),'-'); hold on; plot(states(:,8),'--'); grid on; xlabel('sec'); ylabel('pixel'); legend('x','s_x'); 133 | subplot(3,1,2); plot(Tr(1:end-1,2),'-'); hold on; plot(states(:,9),'--'); grid on; xlabel('sec'); ylabel('pixel'); legend('y','s_y'); 134 | subplot(3,1,3); plot(Tr(1:end-1,3),'-'); hold on; plot(states(:,10),'--'); grid on; xlabel('sec'); ylabel('pixel'); legend('z','s_z'); -------------------------------------------------------------------------------- /ukfmani/quaternions/test/test_isnormq.m: -------------------------------------------------------------------------------- 1 | %TEST_ISQ runs unit tests for the ISQ function. 2 | 3 | % Release: $Name: quaternions-1_3 $ 4 | % $Revision: 1.6 $ 5 | % $Date: 2009-07-24 19:14:44 $ 6 | 7 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 8 | 9 | test_title = 'isnormq'; 10 | disp_test_title(test_title); 11 | 12 | failures=0; 13 | 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | disp_test_name('Insufficient Arguments'); 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | expected_err = 'isnormq() requires one input argument'; 18 | fct_call = 'isnormq'; 19 | failures=failures+check_err(fct_call, expected_err); 20 | 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | disp_test_name('Input is a scalar'); 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | truth_value = '0'; 25 | test_value = 'isnormq(1)'; 26 | failures=failures+check_value(truth_value, test_value); 27 | 28 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 29 | disp_test_name('Input is 2D, but neither dim is size 4'); 30 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 31 | truth_value = '0'; 32 | test_value = 'isnormq(ones(3,5))'; 33 | failures=failures+check_value(truth_value, test_value); 34 | 35 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 36 | disp_test_name('Input is 3D'); 37 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 38 | truth_value = '0'; 39 | test_value = 'isnormq(ones(3,5,4))'; 40 | failures=failures+check_value(truth_value, test_value); 41 | 42 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 43 | disp_test_name('Input is 4xN, N~=4, columns are not normalized'); 44 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 45 | truth_value = '0'; 46 | test_value = 'isnormq(1+rand(4,5))'; 47 | failures=failures+check_value(truth_value, test_value); 48 | 49 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 50 | disp_test_name('Input is 4xN, N~=4, columns are normalized'); 51 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 52 | set_val('M', 'rand(4,5)') 53 | set_val('qmag', 'sqrt(sum(M.^2,1))') 54 | set_val('qmag', '[qmag; qmag; qmag; qmag]') 55 | set_val('q', 'M./qmag') 56 | truth_value = '1'; 57 | test_value = 'isnormq(q)'; 58 | failures=failures+check_value(truth_value, test_value); 59 | 60 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 61 | disp_test_name('Input is 4x4, only columns are normalized'); 62 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 63 | set_val('M', '1+rand(4)') 64 | set_val('qmag', 'sqrt(sum(M.^2,1))') 65 | set_val('qmag', '[qmag; qmag; qmag; qmag]') 66 | set_val('q', 'M./qmag') 67 | truth_value = '1'; 68 | test_value = 'isnormq(q)'; 69 | failures=failures+check_value(truth_value, test_value); 70 | 71 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 72 | disp_test_name('Input is Nx4, N~=4, rows are not normalized'); 73 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 74 | truth_value = '0'; 75 | test_value = 'isnormq(1+rand(3,4))'; 76 | failures=failures+check_value(truth_value, test_value); 77 | 78 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 79 | disp_test_name('Input is Nx4, N~=4, rows are normalized'); 80 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 81 | set_val('M', '1+rand(5,4)') 82 | set_val('qmag', 'sqrt(sum(M.^2,2))') 83 | set_val('qmag', '[qmag qmag qmag qmag]') 84 | set_val('q', 'M./qmag') 85 | truth_value = '2'; 86 | test_value = 'isnormq(q)'; 87 | failures=failures+check_value(truth_value, test_value); 88 | 89 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 90 | disp_test_name('Input is 4x4, only rows are normalized'); 91 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 92 | set_val('M', '1+rand(4)') 93 | set_val('qmag', 'sqrt(sum(M.^2,2))') 94 | set_val('qmag', '[qmag qmag qmag qmag]') 95 | set_val('q', 'M./qmag') 96 | truth_value = '2'; 97 | test_value = 'isnormq(q)'; 98 | failures=failures+check_value(truth_value, test_value); 99 | 100 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 101 | disp_test_name('Input is 4x4, both rows and columns are normalized'); 102 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 103 | set_val('q', '0.5*ones(4)') 104 | truth_value = '3'; 105 | test_value = 'isnormq(q)'; 106 | failures=failures+check_value(truth_value, test_value); 107 | 108 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 109 | disp_test_name('Input is 4x4, neither rows nor columns are normalized'); 110 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 111 | set_val('q', 'ones(4)') 112 | truth_value = '0'; 113 | test_value = 'isnormq(q)'; 114 | failures=failures+check_value(truth_value, test_value); 115 | 116 | 117 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 118 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 119 | disp_num_failures(test_title, failures) 120 | 121 | if failures; error('FAILED'); end 122 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeProduct.m: -------------------------------------------------------------------------------- 1 | % takes a list of manifolds and builds a single manifold 2 | % 3 | % TODO: nested manifolds 4 | % 5 | % Emanuele Ruffaldi 2017 @ SSSA 6 | function m = makeProduct(varargin) 7 | 8 | 9 | 10 | if nargin == 1 11 | m = varargin{1}; 12 | elseif nargin == 2 13 | m1 = varargin{1}; 14 | m2 = varargin{2}; 15 | 16 | g = m1.group; 17 | a = m1.alg; 18 | 19 | m = []; 20 | m.inv = @(x) [m1.inv(x(1:g)), m2.inv(x(g+1:end))]; 21 | m.prod = @(x1,x2) [m1.prod(x1(1:g),x2(1:g)), m2.prod(x1(g+1:end),x2(g+1:end))]; 22 | if isfield(m1,'log') && isfield(m2,'log') 23 | m.log = @(x) [... 24 | m1.log(x(1:g)), ... 25 | m2.log(x(g+1:end))]; 26 | m.exp = @(x) [... 27 | m1.exp(x(1:a)), ... 28 | m2.exp(x(a+1:end))]; 29 | end 30 | m.islie = m1.islie & m2.islie; 31 | m.delta = @(x,y) [m1.delta(x(1:g),y(1:g)), m2.delta(x(g+1:end),y(g+1:end))]; 32 | m.step =@(X,y) [m1.step(X(1:g),y(1:a)), m2.step(X(g+1:end),y(a+1:end))]; 33 | m.group = m1.group+m2.group; 34 | m.alg = m1.alg+m2.alg; 35 | m.count = m1.count+m2.count; 36 | m.models = {m1,m2}; 37 | m.pack = @(x) [m1.pack(x{1}), m2.pack(x{2})]; % this works with cells 38 | m.unpack = @(x) {m1.unpack(x(1:g)), m2.unpack(x(g+1:end))}; 39 | m.type = {'Product',m1.type,m2.type}; 40 | m.groupinc = [0,m1.group,m.group]; 41 | m.alginc = [0,m1.alg,m.alg]; 42 | 43 | if m.islie 44 | m.transport = @(X,t,Y) t; 45 | else 46 | m.transport = @(X,t,Y) mtransport(m,X,t,Y); 47 | end 48 | 49 | else 50 | if 1==0 51 | warning('makeCom multiple to be completed'); 52 | m = makeProduct(varargin{1},varargin{2}); 53 | 54 | for J=3:nargin 55 | m = makeProduct(m,varargin{J}); 56 | end 57 | 58 | else 59 | 60 | m = []; 61 | m.groupinc = [0,cumsum(cellfun(@(x) x.group,varargin))]; 62 | m.alginc = [0,cumsum(cellfun(@(x) x.alg,varargin))]; 63 | 64 | % used for speeding up the partitioning operations 65 | 66 | m.group = sum(cellfun(@(x) x.group,varargin)); 67 | m.alg = sum(cellfun(@(x) x.alg,varargin)); 68 | m.count = sum(cellfun(@(x) x.count,varargin)); 69 | m.models = varargin; 70 | m.inv = @(x) cominv(m,x); 71 | m.prod = @(x1,x2) comprod(m,x1,x2); 72 | m.islie = all(cellfun(@(x) x.islie,varargin)); 73 | 74 | if all(cellfun(@(x) isfield(x,'log'),varargin)) 75 | m.log = @(x) comlog(m,x); 76 | m.exp = @(x) comexp(m,x); 77 | end 78 | 79 | 80 | m.delta = @(x,y) comdelta(m,x,y); 81 | m.step = @(X,y) comstep(m,X,y); 82 | m.pack = @(x) compack(m,x); 83 | m.unpack = @(x) comunpack(m,x); 84 | if m.islie 85 | m.transport = @(X,t,Y) t; 86 | else 87 | m.transport = @(X,t,Y) mtransport(m,X,t,Y); 88 | end 89 | end 90 | 91 | c = {'Product'}; 92 | for I=1:nargin 93 | c{I+1} = varargin{I}.type; 94 | end 95 | m.type = c; 96 | end 97 | m.s = int_manisetup([],[],m); 98 | 99 | 100 | function ty = mtransport(m,X,tx,Y) 101 | ty = zeros(1,m.alg); 102 | for I=1:length(m.models) 103 | ab = m.groupinc(I)+1:m.groupinc(I+1); 104 | aab = m.alginc(I)+1:m.alginc(I+1); 105 | XI = X(ab); 106 | YI = Y(ab); 107 | ty(aab) = m.models{I}.transport(XI,tx(aab),YI); 108 | end 109 | 110 | 111 | function z = comprod(m,x1,x2) 112 | z = zeros(1,m.group); 113 | for I=1:length(m.models) 114 | ab = m.groupinc(I)+1:m.groupinc(I+1); 115 | w2 = x2(ab); 116 | w1 = x1(ab); 117 | z(ab) = m.models{I}.prod(w1(:)',w2(:)'); 118 | end 119 | 120 | function z = cominv(m,X) 121 | 122 | z = zeros(1,m.group); 123 | for I=1:length(m.models) 124 | ab = m.groupinc(I)+1:m.groupinc(I+1); 125 | z(ab) = m.models{I}.inv(X(ab)); 126 | end 127 | 128 | function z = comlog(m,X) 129 | 130 | z = zeros(1,m.alg); 131 | for I=1:length(m.models) 132 | ab = m.groupinc(I)+1:m.groupinc(I+1); 133 | aab = m.alginc(I)+1:m.alginc(I+1); 134 | z(aab) = m.models{I}.log(X(ab)); 135 | end 136 | 137 | function z = comexp(m,x) 138 | 139 | z = zeros(1,m.group); 140 | for I=1:length(m.models) 141 | ab = m.groupinc(I)+1:m.groupinc(I+1); 142 | aab = m.alginc(I)+1:m.alginc(I+1); 143 | z(ab) = m.models{I}.exp(x(aab)); 144 | end 145 | 146 | function z = comstep(m,X,y) 147 | 148 | z = zeros(1,m.group); 149 | for I=1:length(m.models) 150 | ab = m.groupinc(I)+1:m.groupinc(I+1); 151 | aab = m.alginc(I)+1:m.alginc(I+1); 152 | z(ab) = m.models{I}.step(X(ab),y(aab)); 153 | end 154 | 155 | function z = comdelta(m,X,Y) 156 | 157 | z = zeros(1,m.alg); 158 | for I=1:length(m.models) 159 | ab = m.groupinc(I)+1:m.groupinc(I+1); 160 | aab = m.alginc(I)+1:m.alginc(I+1); 161 | z(aab) = m.models{I}.delta(X(ab),Y(ab)); 162 | end 163 | 164 | function z = compack(m,X) 165 | 166 | z = zeros(1,m.group); 167 | for I=1:length(m.models) 168 | ab = m.groupinc(I)+1:m.groupinc(I+1); 169 | z(ab) = m.models{I}.pack(X{I}); 170 | end 171 | 172 | 173 | function z = comunpack(m,x) 174 | 175 | z = cell(size(x,1),m.count); 176 | for J=1:size(x,1) 177 | for I=1:length(m.models) 178 | ab = m.groupinc(I)+1:m.groupinc(I+1); 179 | z{J,I} = m.models{I}.unpack(x(J,ab)); 180 | end 181 | end 182 | 183 | 184 | 185 | -------------------------------------------------------------------------------- /task_headpose/headPose_estim3.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | %% load and divide 4 | load headPose; 5 | 6 | %addpath 'C:\Users\bigDaddy\Dropbox\project_ale\pose_estimator\ukfmanifold-master' 7 | %addpath '/home/cattaneo/Dropbox/project_ale/pose_estimator/ukfmanifold-master' 8 | % addpath C:\Users\ing_Cattaneo\Dropbox\project_ale\pose_estimator\ukfmanifold-master; 9 | % addpath C:\Users\ing_Cattaneo\Documents\MATLAB\lib\quaternions-1.3\quaternions; 10 | Tx=pose_Tx1; 11 | Ty=pose_Ty1; 12 | Tz=pose_Tz1; 13 | Px=pose_Rx1; 14 | Py=pose_Ry1; 15 | Pz=pose_Rz1; 16 | 17 | % build input and output manifolds 18 | 19 | 20 | 21 | mx = manisetup(makeProduct(makeSE3Mat(),makeRn(3),makeRn(3))); 22 | mz = manisetup(makeProduct(makeSE3Mat())); 23 | 24 | mxt = makeSE3Mat(); % helper without the need of setup 25 | %% data 26 | start=20; 27 | stop=1540; 28 | 29 | Rr= [Px(start:stop,:),Py(start:stop,:),Pz(start:stop,:)]; 30 | Tr= [Tx(start:stop,:),Ty(start:stop,:),Tz(start:stop,:)]; 31 | t = (1:length(Rr))*(1/30); 32 | 33 | %ZYX 34 | 35 | figure, 36 | subplot(3,1,1); plot(t,Tr(:,1)); grid on; title('x_{traslation}'); xlabel('sec'); ylabel('mm'); 37 | subplot(3,1,2); plot(t,Tr(:,2)); grid on; title('y_{traslation}'); xlabel('sec'); ylabel('mm'); 38 | subplot(3,1,3); plot(t,Tr(:,3)); grid on; title('z_{traslation}'); xlabel('sec'); ylabel('mm'); 39 | 40 | figure, 41 | subplot(3,1,1); plot(t,Rr(:,1)); grid on; title('Z_{rot}'); xlabel('sec'); ylabel('angles (rad)'); 42 | subplot(3,1,2); plot(t,Rr(:,2)); grid on; title('Y_{rot}'); xlabel('sec'); ylabel('angles (rad)'); 43 | subplot(3,1,3); plot(t,Rr(:,3)); grid on; title('X_{rot}'); xlabel('sec'); ylabel('angles (rad)'); 44 | 45 | zobs=zeros(size(Tr,1),16); 46 | val=zeros(4,4); 47 | %% 48 | % ZYX = roll yaw pitch terna sinistrosra 49 | for i=1:stop-start 50 | val=[eul2rotm(Rr(i,:)),Tr(i,:)';[0,0,0,0]]; 51 | zobs(i,:)=mz.exp([val(1,:),val(2,:),val(3,:),val(4,:)]); 52 | end 53 | 54 | %% UKF pose 55 | 56 | N=stop-start; 57 | 58 | % initial state and noise definition rotation and lineaqr traslation 59 | initialguess=eul2rotm(Rr(start,:)); 60 | x0 = mx.step(mx.exp([1,0,0, 0,1,0,0,0,1, 0,0,0]),[Rr(start,:), Tr(start,:), 0,0,0, 0,0,0]); 61 | 62 | % P0 = blkdiag(1e-6*eye(mx.alg-3),1e-6*eye(3)); 63 | % Q = blkdiag(1e-2*eye(mx.alg-3),1e-4,1e-4,1e-1); % process noise 64 | % R = blkdiag(1e-3*eye(mz.alg-3),1e1,1e-2,1e-1); % measure noise 65 | 66 | P0 = blkdiag(0.5*eye(mx.alg-3),0.5*eye(3)); 67 | Q = blkdiag(0.01*eye(mx.alg-3),1e-2,1e-2,1e-2); % process noise 68 | R = blkdiag(1e-4*eye(mz.alg-3),1e-1,1e-1,1e-1); % measure noise 69 | 70 | wsigmax = ut_mweights2(mx.group,mx.alg,0.5); 71 | wsigmax.sqrt = @svdsqrt; 72 | 73 | % observation is identity 74 | % process is the integral 75 | dt = 1/30*10; 76 | 77 | 78 | f_fx = @(Tk,wk,vk) deal(mxt.step(Tk,[wk,vk]),wk,vk); % Xk = (Tk,wk,vk) 79 | h_fx = @(Tk,wk,vk) Tk; 80 | 81 | tic 82 | % loop 83 | deltas = zeros(N,mz.alg); 84 | states = zeros(size(deltas,1),mx.group); 85 | lstates = zeros(size(deltas,1),mx.alg); 86 | for L=1:N 87 | states(L,:) = x0; 88 | lstates(L,:) = mx.log(x0); 89 | 90 | 91 | [xp,Pp] = manistatestep(mx,x0,P0,f_fx,Q,wsigmax); 92 | % if sum(isnan(xp),2) >0 93 | % xp=zeros(size(xp,1),size(xp,2)); 94 | % end 95 | % 96 | [zm,Czz,Cxz] = manievalh(mx,mz,xp,Pp,h_fx,wsigmax); 97 | 98 | %if missingdata(L) == 1 99 | % Kalman update with observation noise (additive) 100 | Pvv = Czz + R; 101 | K = Cxz/Pvv; 102 | P0 = (eye(size(P0)) - K * Pvv * K') * P0; 103 | delta = mz.delta(zobs(L,:),zm); 104 | x0 = mx.step(xp,(K*delta')'); 105 | deltas(L,:) = delta; 106 | %end 107 | 108 | 109 | 110 | end 111 | %% plotting graph and data converter from 4x4 to Rotation and traslation 112 | toc 113 | Rr1=zeros(size(states,1),3); 114 | Tr1=zeros(size(states,1),3); 115 | for i=1:size(states,1) 116 | T=[states(i,1:3);states(i,4:6);states(i,7:9)]; 117 | Rr1(i,:)=quat2eul(rotm2quat(T)); 118 | Tr1(i,:)=[states(i,4),states(i,8),states(i,12)]; 119 | end 120 | %% 121 | figure('name','measures'); 122 | subplot(6,1,1); plot(t(1,1:end-1),deltas(:,1)); grid on; xlabel('sec'); ylabel('rad'); 123 | subplot(6,1,2); plot(t(1,1:end-1),deltas(:,2)); grid on; xlabel('sec'); ylabel('rad'); 124 | subplot(6,1,3); plot(t(1,1:end-1),deltas(:,3)); grid on; xlabel('sec'); ylabel('rad'); 125 | subplot(6,1,4); plot(t(1,1:end-1),deltas(:,4)); grid on; xlabel('sec'); ylabel('mm'); 126 | subplot(6,1,5); plot(t(1,1:end-1),deltas(:,5)); grid on; xlabel('sec'); ylabel('mm'); 127 | subplot(6,1,6); plot(t(1,1:end-1),deltas(:,6)); grid on; xlabel('sec'); ylabel('mm'); 128 | 129 | 130 | figure('name','stima_rotation'); 131 | subplot(3,1,1); plot(t(1,1:end-1),Rr1(:,1)); hold on; plot(t(1,1:end-1),Rr(1:end-1,1)); grid on; xlabel('sec'); ylabel('angles (rad)'); legend('sR_z','R_z'); 132 | subplot(3,1,2); plot(t(1,1:end-1),Rr1(:,2)); hold on; plot(t(1,1:end-1),Rr(1:end-1,2)); grid on; xlabel('sec'); ylabel('angles (rad)'); legend('sR_y','R_y'); 133 | subplot(3,1,3); plot(t(1,1:end-1),Rr1(:,3)); hold on; plot(t(1,1:end-1),Rr(1:end-1,3)); grid on; xlabel('sec'); ylabel('angles (rad)'); legend('sR_x','R_x'); 134 | 135 | figure('name','stima_traslation'); 136 | subplot(3,1,1); plot(t(1,1:end-1),Tr(1:end-1,1),'-'); hold on; plot(t(1,1:end-1),Tr1(:,1),'--'); grid on; xlabel('sec'); ylabel('mm'); legend('x','s_x'); 137 | subplot(3,1,2); plot(t(1,1:end-1),Tr(1:end-1,2),'-'); hold on; plot(t(1,1:end-1),Tr1(:,2),'--'); grid on; xlabel('sec'); ylabel('mm'); legend('y','s_y'); 138 | subplot(3,1,3); plot(t(1,1:end-1),Tr(1:end-1,3),'-'); hold on; plot(t(1,1:end-1),Tr1(:,3),'--'); grid on; xlabel('sec'); ylabel('mm'); legend('z','s_z'); -------------------------------------------------------------------------------- /conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # UKF Manifold documentation build configuration file, created by 4 | # sphinx-quickstart on Sun Jan 21 00:33:28 2018. 5 | # 6 | # This file is execfile()d with the current directory set to its 7 | # containing dir. 8 | # 9 | # Note that not all possible configuration values are present in this 10 | # autogenerated file. 11 | # 12 | # All configuration values have a default; values that are commented out 13 | # serve to show the default. 14 | 15 | # If extensions (or modules to document with autodoc) are in another directory, 16 | # add these directories to sys.path here. If the directory is relative to the 17 | # documentation root, use os.path.abspath to make it absolute, like shown here. 18 | # 19 | # import os 20 | # import sys 21 | # sys.path.insert(0, os.path.abspath('.')) 22 | 23 | 24 | # -- General configuration ------------------------------------------------ 25 | 26 | # If your documentation needs a minimal Sphinx version, state it here. 27 | # 28 | # needs_sphinx = '1.0' 29 | 30 | # Add any Sphinx extension module names here, as strings. They can be 31 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 32 | # ones. 33 | extensions = ['sphinx.ext.mathjax','sphinxcontrib.matlab','sphinx.ext.autodoc'] 34 | 35 | # Add any paths that contain templates here, relative to this directory. 36 | templates_path = ['_templates'] 37 | 38 | # The suffix(es) of source filenames. 39 | # You can specify multiple suffix as a list of string: 40 | # 41 | source_suffix = ['.rst', '.md'] 42 | 43 | # The master toctree document. 44 | master_doc = 'index' 45 | 46 | # General information about the project. 47 | project = u'UKF Manifold' 48 | copyright = u'2018, Emanuele Ruffaldi' 49 | author = u'Emanuele Ruffaldi' 50 | 51 | # The version info for the project you're documenting, acts as replacement for 52 | # |version| and |release|, also used in various other places throughout the 53 | # built documents. 54 | # 55 | # The short X.Y version. 56 | version = u'' 57 | # The full version, including alpha/beta/rc tags. 58 | release = u'' 59 | 60 | # The language for content autogenerated by Sphinx. Refer to documentation 61 | # for a list of supported languages. 62 | # 63 | # This is also used if you do content translation via gettext catalogs. 64 | # Usually you set "language" from the command line for these cases. 65 | language = None 66 | 67 | # List of patterns, relative to source directory, that match files and 68 | # directories to ignore when looking for source files. 69 | # This patterns also effect to html_static_path and html_extra_path 70 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] 71 | 72 | # The name of the Pygments (syntax highlighting) style to use. 73 | pygments_style = 'sphinx' 74 | 75 | # If true, `todo` and `todoList` produce output, else they produce nothing. 76 | todo_include_todos = False 77 | 78 | 79 | # -- Options for HTML output ---------------------------------------------- 80 | 81 | # The theme to use for HTML and HTML Help pages. See the documentation for 82 | # a list of builtin themes. 83 | # 84 | html_theme = 'alabaster' 85 | 86 | # Theme options are theme-specific and customize the look and feel of a theme 87 | # further. For a list of options available for each theme, see the 88 | # documentation. 89 | # 90 | # html_theme_options = {} 91 | 92 | # Add any paths that contain custom static files (such as style sheets) here, 93 | # relative to this directory. They are copied after the builtin static files, 94 | # so a file named "default.css" will overwrite the builtin "default.css". 95 | html_static_path = ['_static'] 96 | 97 | # Custom sidebar templates, must be a dictionary that maps document names 98 | # to template names. 99 | # 100 | # This is required for the alabaster theme 101 | # refs: http://alabaster.readthedocs.io/en/latest/installation.html#sidebars 102 | html_sidebars = { 103 | '**': [ 104 | 'relations.html', # needs 'show_related': True theme option to display 105 | 'searchbox.html', 106 | ] 107 | } 108 | 109 | 110 | # -- Options for HTMLHelp output ------------------------------------------ 111 | 112 | # Output file base name for HTML help builder. 113 | htmlhelp_basename = 'UKFManifolddoc' 114 | 115 | 116 | # -- Options for LaTeX output --------------------------------------------- 117 | 118 | latex_elements = { 119 | # The paper size ('letterpaper' or 'a4paper'). 120 | # 121 | # 'papersize': 'letterpaper', 122 | 123 | # The font size ('10pt', '11pt' or '12pt'). 124 | # 125 | # 'pointsize': '10pt', 126 | 127 | # Additional stuff for the LaTeX preamble. 128 | # 129 | # 'preamble': '', 130 | 131 | # Latex figure (float) alignment 132 | # 133 | # 'figure_align': 'htbp', 134 | } 135 | 136 | # Grouping the document tree into LaTeX files. List of tuples 137 | # (source start file, target name, title, 138 | # author, documentclass [howto, manual, or own class]). 139 | latex_documents = [ 140 | (master_doc, 'UKFManifold.tex', u'UKF Manifold Documentation', 141 | u'Emanuele Ruffaldi', 'manual'), 142 | ] 143 | 144 | 145 | # -- Options for manual page output --------------------------------------- 146 | 147 | # One entry per manual page. List of tuples 148 | # (source start file, name, description, authors, manual section). 149 | man_pages = [ 150 | (master_doc, 'ukfmanifold', u'UKF Manifold Documentation', 151 | [author], 1) 152 | ] 153 | 154 | 155 | # -- Options for Texinfo output ------------------------------------------- 156 | 157 | # Grouping the document tree into Texinfo files. List of tuples 158 | # (source start file, target name, title, author, 159 | # dir menu entry, description, category) 160 | texinfo_documents = [ 161 | (master_doc, 'UKFManifold', u'UKF Manifold Documentation', 162 | author, 'UKFManifold', 'One line description of project.', 163 | 'Miscellaneous'), 164 | ] 165 | 166 | 167 | 168 | -------------------------------------------------------------------------------- /examples/exampleSE3MatAugMissingPos.m: -------------------------------------------------------------------------------- 1 | % We build first the manifolds explicitly stating the state manifold x (Mx) 2 | % and the augmented Mxa 3 | Mx = {makeSE3Mat(),makeRn(3),makeRn(3)}; 4 | Mxa = [Mx,makeRn(6)]; 5 | Mz = makeSE3Mat(); 6 | 7 | % Then we proceed to the setup of the manifold in usable way, also using 8 | % the code generation for speedup of the processing 9 | mx = manisetup(makeGenProduct('se3mat_e3_e3',Mx{:})); 10 | mxa = manisetup(makeGenProduct('se3mat_e3_e3_aug',Mxa{:})); % as makeProduct(....,+1) 11 | mz = manisetup(Mz); 12 | mzr = manisetup(makeRot()); % only rotation, loss of position 13 | mxt = makeSE3Mat(); % helper without the need of setup 14 | 15 | % We build these structures for supporting the Unscented transformation. We 16 | % need them for the manifold x in the process, and the augmented xa in the 17 | % observation 18 | mx.wsigma = ut_mweights2(mx.group,mx.alg,0.5); 19 | mx.wsigma.sqrt = @svdsqrt; 20 | mxa.wsigma = ut_mweights2(mxa.group,mxa.alg,0.5); 21 | mxa.wsigma.sqrt = @svdsqrt; 22 | 23 | 24 | % initial state and noise definition 25 | x0 = mx.step(mx.exp([0,0,0, 0,1,0, 0,0,0, 0,0,0]'),[pi/2,0.2,0, 0,0,0, 0,0,0, 0,0,0]'); 26 | P0 = 0.5*eye(mx.alg); 27 | Q = 0.01*eye(mx.alg); % process noi!se 28 | R = 1e-3*eye(mz.alg); % measure noise 29 | Rr = R(1:3,1:3); % only rotations 30 | 31 | z0 = mz.exp([pi/2,0,0, 0,0.1,1]'); 32 | zobsval = zeros(16,200); 33 | 34 | % costant se3 velocity 35 | v0 = zeros(6,1); 36 | v0(4) = 0.1; 37 | v0(1) = 0.2; 38 | zobsval(:,1) = z0; 39 | lzobsval = zeros(mz.alg,size(zobsval,2)); 40 | lzobsval(:,1) = mz.log(z0); 41 | for I=2:size(zobsval,2) 42 | v0(2) = sin(I/100); 43 | zobsval(:,I) = mz.step(zobsval(:,I-1),v0); 44 | lzobsval(:,I) = mz.log(zobsval(:,I)); 45 | end 46 | zobs = @(t) zobsval(:,t); 47 | 48 | 49 | % observation is identity 50 | % process is the integral 51 | dt = 0.1; 52 | 53 | % functions work expanding each primitive manifold with their type (e.g. matrix 4x4) 54 | % if the output is made of multiple e manifolds use deal for returning 55 | % everything 56 | f_fx = @(Tk,wk,vk) deal(mxt.step(Tk,[wk,vk]),wk,vk); % Xk = (Tk,wk,vk) 57 | h_fx = @(Tk,wk,vk,ek) mxt.step(Tk,ek); 58 | hr_fx = @(Tk,wk,vk,ek) mxt.step(Tk(1:3,1:3),ek(1:3)); 59 | tic 60 | % loop 61 | deltas = zeros(200,mz.alg); 62 | states = zeros(size(deltas,1),mx.group); 63 | lstates = zeros(size(deltas,1),mx.alg); 64 | haspos = ones(200,1); 65 | haspos(4:4:end) = 0; 66 | haspos(5:4:end) = 0; 67 | haspos(6:4:end) = 0; 68 | usereduxspace = 0; 69 | Rnonadditive = eye(mxa.alg-mx.alg); % generic formulation 70 | Rnonadditive = R; 71 | R = zeros(size(R)); % for testing augmentation 72 | 73 | for L=1:size(deltas,1) 74 | states(L,:) = x0; 75 | lstates(L,:) = mx.log(x0); % [ rodriguez(R) traslazione velocitàangolar velocitàlinear ] 76 | 77 | [xp,Pp] = manistatestep(mx,x0,P0,f_fx,Q); 78 | assert(size(xp,2)==1); 79 | if haspos(L) == 0 && usereduxspace 80 | % TODO make shorter augmentation due to the fact that we do not 81 | % have a 6D space on Z 82 | Ppa = blkdiag(Pp,Rnonadditive); 83 | xpa = [xp; zeros(mxa.alg-mx.alg,1)]; % mean is zero 84 | [zm,Czz,Cxaz] = manievalh(mxa,mzr,xpa,Ppa,hr_fx); 85 | assert(size(zm,2)==1); 86 | 87 | % Kalman update with observation noise (additive) 88 | Pvv = Czz + Rr; 89 | K = Cxaz/Pvv; 90 | P0 = (Pp - K * Pvv * K'); 91 | 92 | fullobs_mat = mz.unpack( zobs(L)); % from SE3 16x1 to SE3 4x4 93 | 94 | deltar = mzr.delta(mzr.pack(fullobs_mat(1:3,1:3)),zm); 95 | x0 = mx.step(xp,(K*deltar')); 96 | delta = [deltar, NaN,NaN,NaN]; % ONLY for viz 97 | else 98 | % Build the augmented state by expanding the state with noise with 99 | % zero mean and provided covariance 100 | Ppa = blkdiag(Pp,Rnonadditive); 101 | xpa = [xp; zeros(mxa.alg-mx.alg,1)]; % mean is zero 102 | [zm,Czz,Cxaz] = manievalh(mxa,mz,xpa,Ppa,h_fx); 103 | 104 | % Kalman update with observation noise (additive) 105 | if haspos(L) == 0 106 | % maximal noise except for the rotation part 107 | Rr = 1e10*ones(size(R)); 108 | Rr(1:3,1:3) = R(1:3,1:3); 109 | Rr(1:3,4:6) = 0; 110 | Czz(1:3,4:6) = 0; 111 | Czz(4:6,1:3) = 0; 112 | Pvv = Czz + Rr; 113 | % zero out the observation (cannot use NaN) 114 | zc = mz.unpack(zobs(L)); 115 | zc{1}(1:3,4) = 0; 116 | z = mz.pack(zc); 117 | else 118 | Pvv = Czz + R; 119 | z = zobs(L); 120 | end 121 | assert(size(z,1) == mz.group); 122 | assert(size(z,2) == 1); 123 | 124 | K = Cxaz/Pvv; 125 | Ppa = (PPa - K * Pvv * K'); 126 | delta = mz.delta(z,zm); 127 | xpanew = mx.step(xpa,(K*delta')); 128 | % extract x0 from xpanew because we discard the augmentation 129 | % extract P0 from Ppa because we discard augmentation 130 | x0 = xpanew(1:mx.group); 131 | P0 = Ppa(1:mx.alg,1:mx.alg); 132 | 133 | end 134 | deltas(L,:) = delta; 135 | end 136 | %% 137 | toc 138 | figure(1) 139 | subplot(3,1,1); 140 | plot(deltas(:,1:3)) 141 | title('Deltas observation-prediction'); 142 | xlabel('sample'); 143 | subplot(3,1,2); 144 | plot(deltas(:,4:6)) 145 | title('Deltas observation-prediction'); 146 | xlabel('sample'); 147 | subplot(3,1,3); 148 | plot(sum(deltas.^2,2)); 149 | title('Norm of error'); 150 | xlabel('sample'); 151 | figure(2) 152 | plot(states(10:end,:)) 153 | title('All states as matrix'); 154 | 155 | %% Latency estimation 156 | dd = zeros(1,6); 157 | for J=1:6 158 | figure(2+J); 159 | dd(J) = finddelay(lstates(J,:),lzobsval(:,J)); 160 | 161 | plot([lstates(J,:)';lzobsval(:,J)]); 162 | end 163 | disp('delay') 164 | dd 165 | disp('error') 166 | sqrt(meannonan(deltas.^2,1))' 167 | -------------------------------------------------------------------------------- /ukfmani/quaternions/test/test_qdecomp.m: -------------------------------------------------------------------------------- 1 | %TEST_QDECOMP runs unit tests for the QDECOMP function. 2 | 3 | % Release: $Name: quaternions-1_3 $ 4 | % $Revision: 1.7 $ 5 | % $Date: 2009-07-26 20:05:13 $ 6 | 7 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 8 | 9 | test_title = 'qdecomp'; 10 | disp_test_title(test_title); 11 | 12 | failures=0; 13 | 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | disp_test_name('Insufficient Arguments'); 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | expected_err = 'qdecomp() requires one input argument'; 18 | fct_call = 'qdecomp'; 19 | failures=failures+check_err(fct_call, expected_err); 20 | 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | disp_test_name('Invalid Input: scalar'); 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | expected_err = ... 25 | 'Input Q must be a quaternion or a vector of quaternions'; 26 | fct_call = 'qdecomp(1)'; 27 | failures=failures+check_err(fct_call, expected_err); 28 | 29 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 30 | disp_test_name('q=[0 0 0 1]'); 31 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 32 | % Test values 33 | test_string='[test_v, test_phi] = qdecomp([0 0 0 1])'; 34 | disp(test_string), eval(test_string), disp(' ') 35 | % phi 36 | truth_value = '0'; 37 | test_value = 'test_phi'; 38 | failures=failures+check_float(truth_value, test_value, 1e-15); 39 | % v 40 | truth_value = '[0 0 0]'; 41 | test_value = 'test_v'; 42 | failures=failures+check_float(truth_value, test_value, 1e-15); 43 | 44 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 45 | disp_test_name('Column of three quaternions'); 46 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 47 | q1=qnorm([ 1 2.0 3 4]),disp (' ') %#ok 48 | q2=[0 0 0 1],disp (' ') %#ok 49 | q3=qnorm([-1 0.5 2 -2]),disp (' ') %#ok 50 | % Test values 51 | test_string='[test_v, test_phi] = qdecomp([q1; q2; q3])'; 52 | disp(test_string), eval(test_string), disp(' ') 53 | % phi 54 | truth_phi = [ ... 55 | '[ 2*acos(q1(4))', 10, ... 56 | ' 0', 10, ... 57 | ' 2*acos(q3(4)) ]'], disp(' ') %#ok 58 | truth_phi = eval(truth_phi); %#ok 59 | truth_value = 'truth_phi'; 60 | test_value = 'test_phi'; 61 | failures=failures+check_float(truth_value, test_value, 1e-15); 62 | % v 63 | truth_v = [ ... 64 | '[ [q1(1) q1(2) q1(3)]/sin(acos(q1(4)))', 10, ... 65 | ' [0 0 0]', 10, ... 66 | ' [q3(1) q3(2) q3(3)]/sin(acos(q3(4))) ]'], disp(' ') %#ok 67 | truth_v = eval(truth_v); %#ok 68 | truth_value = 'truth_v'; 69 | test_value = 'test_v'; 70 | failures=failures+check_float(truth_value, test_value, 1e-15); 71 | 72 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 73 | disp_test_name('Row of 6 quaternions'); 74 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 75 | % Test values 76 | test_string='[test_v, test_phi] = qdecomp([q1; q2; q3; q3; q2; q1].'')'; 77 | disp(test_string), eval(test_string), disp(' ') 78 | % phi 79 | truth_phi = [ ... 80 | '[ 2*acos(q1(4))', 10, ... 81 | ' 0', 10, ... 82 | ' 2*acos(q3(4))', 10, ... 83 | ' 2*acos(q3(4))', 10, ... 84 | ' 0', 10, ... 85 | ' 2*acos(q1(4)) ]'], disp(' ') %#ok 86 | truth_phi = eval(truth_phi); %#ok 87 | truth_value = 'truth_phi'; 88 | test_value = 'test_phi'; 89 | failures=failures+check_float(truth_value, test_value, 1e-15); 90 | % v 91 | truth_v = [ ... 92 | '[ [q1(1) q1(2) q1(3)]/sin(acos(q1(4)))', 10, ... 93 | ' [0 0 0]', 10, ... 94 | ' [q3(1) q3(2) q3(3)]/sin(acos(q3(4)))', 10, ... 95 | ' [q3(1) q3(2) q3(3)]/sin(acos(q3(4)))', 10, ... 96 | ' [0 0 0]', 10, ... 97 | ' [q1(1) q1(2) q1(3)]/sin(acos(q1(4))) ]'], disp(' ') %#ok 98 | truth_v = eval(truth_v); %#ok 99 | truth_value = 'truth_v'; 100 | test_value = 'test_v'; 101 | failures=failures+check_float(truth_value, test_value, 1e-15); 102 | 103 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 104 | disp_test_name('Ambiguous Input: 4x4, check result for validity'); 105 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 106 | % Test values 107 | test_string='[test_v, test_phi] = qdecomp([q1; q3; q1; q2])'; 108 | disp(test_string), eval(test_string), disp(' ') 109 | % phi 110 | truth_phi = [ ... 111 | '[ 2*acos(q1(4)) ', 10, ... 112 | ' 2*acos(q3(4)) ', 10, ... 113 | ' 2*acos(q1(4)) ', 10, ... 114 | ' 0 ]'], disp(' ') %#ok 115 | truth_phi = eval(truth_phi); 116 | truth_value = 'truth_phi'; 117 | test_value = 'test_phi'; 118 | failures=failures+check_float(truth_value, test_value, 1e-15); 119 | % v 120 | truth_v = [ ... 121 | '[ [q1(1) q1(2) q1(3)]/sin(acos(q1(4))) ', 10, ... 122 | ' [q3(1) q3(2) q3(3)]/sin(acos(q3(4))) ', 10, ... 123 | ' [q1(1) q1(2) q1(3)]/sin(acos(q1(4))) ', 10, ... 124 | ' [0 0 0] ]'], disp(' ') %#ok 125 | truth_v = eval(truth_v); 126 | truth_value = 'truth_v'; 127 | test_value = 'test_v'; 128 | failures=failures+check_float(truth_value, test_value, 1e-15); 129 | 130 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 131 | disp_test_name('Ambiguous Input: 4x4'); 132 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 133 | expected_warn = ['Component quaternion shape indeterminate, assuming' ... 134 | ' row vectors']; 135 | fct_call = 'qdecomp(0.5*ones(4,4));'; 136 | failures=failures+check_warn(fct_call, expected_warn); 137 | 138 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 139 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 140 | 141 | disp_num_failures(test_title, failures) 142 | 143 | 144 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | # Unscented Kalman Filtering (UKF) over Manifolds 3 | 4 | This Matlab toolbox aims at supporting non-linear transformations of Multivariate Normal (MVN) variables and bayesian filtering of such variable. The toolbox relies on the concept of Unscented Transformation that is at the basis of the Unscented Kalman Filtering. For a visual comparison between Extended Kalman Filtring and Unscented Kalman filtering look at https://github.com/eruffaldi/compare-mvn-transform 5 | 6 | Look at related publications supporting the theory of this: Quaternion UKF [1], UKF over Riemman Manifolds [2] and manifolds [3]. 7 | 8 | # Concepts 9 | 10 | * state is in a manfold (e.g Lie Group), or combination of there of with dimension n 11 | * noise is expressed in the tangent space (e.g. Lie Algebra): dimension m <= n 12 | * we express a multivariate Gaussian as N(mu, Sigma) with mean over manifold and covariance tangent space 13 | 14 | General manifolds requires this: 15 | 16 | * delta(Group,Group) -> tangent 17 | * step(Group,tangent) -> Group 18 | 19 | Lie Groups require this: 20 | 21 | * prod(X,Y) = X * Y 22 | * inv(X) 23 | 24 | And Lie Groups define delta and steps as: 25 | 26 | * delta(X, Y) = log(X * inv(Y)) 27 | * step(X, y) = exp(y) * X 28 | 29 | First we define the sigma point construction and synthesis: 30 | 31 | Chi_i = step(mu,[ 0_alg +v1 ... +vm -v1...-vm ]) 32 | mu = mean(Chi_i) 33 | Chi_not_mu_i = delta(Chi_i,mu) 34 | C = 1/(2m+) Sum (Chi_not_mu_i' Chi_not_mu_i) 35 | 36 | The mean(Chi_i) is an iterative algorithm: 37 | 38 | mu_0 = x0 39 | v_{i,k} = delta(x_i,mu_k) 40 | mu_{k+1} = step(mu_k,1/N Sum v_{i,k}) 41 | 42 | Then we deal with the Kalman aspect in particular the correction: 43 | 44 | x_i = step(x_est_{i-1},K delta(z,z_{obs})) 45 | 46 | ## Representations 47 | 48 | We have these representation of the manifold data 49 | 50 | - packed as vector [G,1]: each manifold is packed as a vector, not in the minimal representation (e.g. a matrix 3x3 is 9x1, matrix 4x4 is 16x1) 51 | - tangent as vector [A,1] 52 | - expanded as matrix or vector (e.g. 3x3 for matrix of rotation). Products of manifolds are expressed as cell array with the contained type, eventually in nested form 53 | 54 | When considering multiple samples we use matlab convention: [G,N] and [A,N] and cell [C,N] 55 | 56 | %TODO: for nested manifold decide if nested cell arrays or keep cell arrays expanded 57 | 58 | Example: 59 | - SO3 quaternion: expanded group 4x1, packed group 4x1, algebra 3 60 | - SO3 matrix: expanded group 3x3, packed group 9, algebra 3 61 | - SE3 matrix: expanded group 4x4, packed group 16, algebra 6 62 | - (SE3,SE3): expanded group {4x4,4x4}, packed group 32, algebra 12 63 | 64 | 65 | Each Manifold is represented as a struct with: 66 | - group as the size of the packed group 67 | - alg as the size of the algrebra space 68 | - count as the number of contained native manifolds, corresponding to the cell size 69 | - function handles for each operation 70 | - models as sub-models if present 71 | 72 | # Usage 73 | 74 | Initialize the toolbox running init.m 75 | 76 | # Examples of Manifold operations 77 | 78 | Case of SO3 79 | - group: Rotation 3x3 80 | - algenra: vector R3 81 | - product: R1 R2 82 | - inverse: R' 83 | - logarithm/exponential: rodriguez and its inverse 84 | 85 | Case of quaternion 86 | - group: quaternion 4 87 | - algenra: vector R3 88 | - product: q1 q2 89 | - inverse: q* 90 | - delta: axis angle between quaternions as vector (aka derivative) 91 | - step: integral 92 | - logarithm/exponential: rodriguez and its inverse 93 | 94 | # Code Generation 95 | 96 | The makeGenProduct(name,m1...mN) produces a Matlab code that contains the efficient composition of the provided manifolds. In this way the singl operations are executed efficiently (e.g. testmakegen for a test) 97 | 98 | On my tests a composition of 4 manifolds give 30% speed up 99 | 100 | TODO: support nested manifolds code generation 101 | 102 | # Generate Documentation 103 | 104 | - Sphinx 105 | - m2html 106 | 107 | m2html('mfiles','ukfmani', 'htmldir','doc', 'recursive','on', 'global','on','todo','on');%,'source','off'); 108 | 109 | # References 110 | 111 | [1] Quaternion UKF: http://kodlab.seas.upenn.edu/uploads/Arun/UKFpaper.pdf (http://ieeexplore.ieee.org/document/1257247/) 112 | Kraft, E. (2003, July). A quaternion-based unscented Kalman filter for orientation tracking. In Proceedings of the Sixth International Conference of Information Fusion (Vol. 1, pp. 47-54). 113 | 114 | [2] Riemman Manifold UKF 2013: http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.646.893&rep=rep1&type=pdf 115 | Hauberg, Søren, François Lauze, and Kim Steenstrup Pedersen. "Unscented Kalman filtering on Riemannian manifolds." Journal of mathematical imaging and vision 46.1 (2013): 103-120. 116 | 117 | [3] Hertzberg, Christoph, et al. "Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds." Information Fusion 14.1 (2013): 57-77. 118 | 119 | [4] Our variant for robotics: http://www.eruffaldi.com/papers/2017_C_ETFADiStefano.pdf 120 | Di Stefano, Erika, Emanuele Ruffaldi, and Carlo Alberto Avizzano. "A Multi-Camera Framework for Visual Servoing of a Collaborative Robot in Industrial Environments." 121 | 122 | Note for Scaled UKF 123 | * Bonus for large dimensions: http://www.cs.unc.edu/~welch/kalman/media/pdf/ACC02-IEEE1357.PDF 124 | 125 | Note for Averaging 126 | * Averaging: http://www.acsu.buffalo.edu/~johnc/uf_att.pdf 127 | * Italian: https://re.public.polimi.it/retrieve/handle/11311/961634/40508/1_Manuscript.pdf 128 | 129 | References for research starting from this: 130 | - Bourmaud, Guillaume, et al. "Continuous-discrete extended Kalman filter on matrix Lie groups using concentrated Gaussian distributions." Journal of Mathematical Imaging and Vision 51.1 (2015): 209-228. 131 | - Windle, Jesse, and Carlos M. Carvalho. "A tractable state-space model for symmetric positive-definite matrices." Bayesian Analysis 9.4 (2014): 759-792. 132 | - Freifeld, Oren, Soren Hauberg, and Michael J. Black. "Model transport: Towards scalable transfer learning on manifolds." Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2014. 133 | - Hauberg, Søren. "Principal curves on Riemannian manifolds." IEEE transactions on pattern analysis and machine intelligence 38.9 (2016): 1915-1921. 134 | - Srivatsan, Rangaprasad Arun, et al. "Estimating SE (3) elements using a dual quaternion based linear Kalman filter." Robotics: Science and Systems. 2016. 135 | 136 | Geometric integration of quat: 137 | - https://www.researchgate.net/profile/John_Crassidis/publication/260466470_Geometric_Integration_of_Quaternions/links/557acc0a08ae8d0481931b51/Geometric-Integration-of-Quaternions.pdf -------------------------------------------------------------------------------- /ukfmani/quaternions/test/test_qmult.m: -------------------------------------------------------------------------------- 1 | %TEST_QMULT runs unit tests for the QMULT function. 2 | 3 | % Release: $Name: quaternions-1_3 $ 4 | % $Revision: 1.8 $ 5 | % $Date: 2009-07-26 20:05:13 $ 6 | 7 | % Copyright (c) 2000-2009, Jay A. St. Pierre. All rights reserved. 8 | 9 | test_title = 'qmult'; 10 | disp_test_title(test_title); 11 | 12 | failures=0; 13 | 14 | q=[0 0 0 1], disp(' ') %#ok 15 | 16 | 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18 | %% Invalid inputs 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 20 | 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | disp_test_name('Insufficient Arguments: no arguments'); 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | expected_err = 'qmult() requires two input arguments'; 25 | fct_call = 'qmult'; 26 | failures=failures+check_err(fct_call, expected_err); 27 | 28 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 29 | disp_test_name('Insufficient Arguments: one argument'); 30 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 31 | expected_err = 'qmult() requires two input arguments'; 32 | fct_call = 'qmult(1)'; 33 | failures=failures+check_err(fct_call, expected_err); 34 | 35 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 36 | disp_test_name('q1 is invalid'); 37 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 38 | expected_err = ... 39 | ['Invalid input: q1 must be a quaternion or a vector of',... 40 | ' quaternions']; 41 | fct_call = 'qmult(1, q)'; 42 | failures=failures+check_err(fct_call, expected_err); 43 | 44 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 45 | disp_test_name('q2 is invalid'); 46 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 47 | expected_err = ... 48 | ['Invalid input: q2 must be a quaternion or a vector of',... 49 | ' quaternions']; 50 | fct_call = 'qmult(q, 1)'; 51 | failures=failures+check_err(fct_call, expected_err); 52 | 53 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 54 | disp_test_name('q1 and q2 are vectors of different lengths'); 55 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 56 | expected_err = ... 57 | ['Inputs do not have the same number of elements:', 10, ... 58 | ' number of quaternions in q1 = 5', 10,... 59 | ' number of quaternions in q2 = 3', 10,... 60 | 'Inputs must have the same number of elements, or', 10, ... 61 | 'one of the inputs must be a single quaternion (not a', 10, ... 62 | 'vector of quaternions).']; 63 | fct_call = 'qmult(ones(4,5), ones(3,4));'; 64 | failures=failures+check_err(fct_call, expected_err); 65 | 66 | 67 | 68 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 69 | %% Products 70 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 71 | 72 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 73 | disp_test_name('quaternions are row vectors'); 74 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 75 | q1=qnorm([ 1 2.0 3 4]), disp(' ') %#ok 76 | q2=qnorm([-1 0.5 2 -2]), disp(' ') %#ok 77 | q1q2 = [ ... 78 | 'q1*[ q2(4) -q2(3) q2(2) -q2(1)', 10, ... 79 | ' q2(3) q2(4) -q2(1) -q2(2)', 10, ... 80 | ' -q2(2) q2(1) q2(4) -q2(3)', 10, ... 81 | ' q2(1) q2(2) q2(3) q2(4) ]'],disp(' ') %#ok 82 | q1q2 = eval(q1q2); 83 | truth_value = 'q1q2'; 84 | test_value = 'qmult(q1, q2)'; 85 | failures=failures+check_float(truth_value, test_value, 1e-15); 86 | 87 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 88 | disp_test_name('q1 is a column vector, q2 is a row vector'); 89 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 90 | truth_value = 'q1q2.'''; 91 | test_value = 'qmult(q1.'', q2)'; 92 | failures=failures+check_float(truth_value, test_value, 1e-15); 93 | 94 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 95 | disp_test_name('q1 is a row vector, q2 is a column vector'); 96 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 97 | truth_value = 'q1q2'; 98 | test_value = 'qmult(q1, q2.'')'; 99 | failures=failures+check_float(truth_value, test_value, 1e-15); 100 | 101 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 102 | disp_test_name('quaternions are column vectors'); 103 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 104 | q3=qnorm([0.2 -1.0 0.5 0.3].'),disp(' ') %#ok 105 | q4=qnorm([2.0 1.5 -1.0 0.5].'),disp(' ') %#ok 106 | q3q4 = [ ... 107 | '(q3.''*[ q4(4) -q4(3) q4(2) -q4(1)', 10, ... 108 | ' q4(3) q4(4) -q4(1) -q4(2)', 10, ... 109 | ' -q4(2) q4(1) q4(4) -q4(3)', 10, ... 110 | ' q4(1) q4(2) q4(3) q4(4) ]).'''],disp(' ') %#ok 111 | q3q4 = eval(q3q4); 112 | truth_value = 'q3q4'; 113 | test_value = 'qmult(q3, q4)'; 114 | failures=failures+check_float(truth_value, test_value, 1e-15); 115 | 116 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 117 | disp_test_name('Multiply two vectors of quaternions'); 118 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 119 | Q1=[q1.' q3 ], disp(' ') %#ok 120 | Q2=[q2; q4.'], disp(' ') %#ok 121 | truth_value = '[q1q2.'' q3q4]'; 122 | test_value = 'qmult(Q1, Q2)'; 123 | failures=failures+check_float(truth_value, test_value, 1e-15); 124 | 125 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 126 | disp_test_name('Multiply vector of quaternions by a single quaternion'); 127 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 128 | Q1=q1, disp(' ') %#ok 129 | Q2=[q2.' q4], disp(' ') %#ok 130 | q1q4 = [ ... 131 | '(q1*[ q4(4) -q4(3) q4(2) -q4(1)', 10, ... 132 | ' q4(3) q4(4) -q4(1) -q4(2)', 10, ... 133 | ' -q4(2) q4(1) q4(4) -q4(3)', 10, ... 134 | ' q4(1) q4(2) q4(3) q4(4) ])'],disp(' ') %#ok 135 | q1q4 = eval(q1q4); 136 | truth_value = '[q1q2; q1q4]'; 137 | test_value = 'qmult(Q1, Q2)'; 138 | failures=failures+check_float(truth_value, test_value, 1e-15); 139 | 140 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 141 | disp_test_name('Q1 is of indeterminate shape, Q2 is a row'); 142 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 143 | Q1=[q1; ones(3,4)],disp(' ') %#ok 144 | Q2=q2,disp(' ') %#ok 145 | truth_value = 'q1q2'; 146 | Q1Q2 = 'qmult(Q1, Q2)',disp(' ') %#ok 147 | Q1Q2 = eval(Q1Q2); %#ok 148 | test_value = 'Q1Q2(1,:)'; 149 | failures=failures+check_float(truth_value, test_value, 1e-15); 150 | 151 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 152 | disp_test_name('Q1 is of indeterminate shape, Q2 is a column'); 153 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 154 | Q1=[q3 ones(4,3)],disp(' ') %#ok 155 | Q2=q4,disp(' ') %#ok 156 | truth_value = 'q3q4'; 157 | Q1Q2 = 'qmult(Q1, Q2)',disp(' ') %#ok 158 | Q1Q2 = eval(Q1Q2); 159 | test_value = 'Q1Q2(:,1)'; 160 | failures=failures+check_float(truth_value, test_value, 1e-15); 161 | 162 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 163 | disp_test_name('Both inputs 4x4, normalized differently'); 164 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 165 | Q1=[q1; q3.'; q1; q3.'],disp(' ') %#ok 166 | Q2=[q2; q4.'; q2; q4.'].',disp(' ') %#ok 167 | truth_value = '[q1q2; q3q4.''; q1q2; q3q4.'']'; 168 | test_value = 'qmult(Q1, Q2)'; 169 | failures=failures+check_float(truth_value, test_value, 1e-15); 170 | 171 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 172 | disp_test_name('Both inputs 4x4, and of indeterminate shape'); 173 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 174 | Q1=ones(4),disp(' ') %#ok 175 | Q2=ones(4),disp(' ') %#ok 176 | truth_value = '2*[ones(4,3) -ones(4,1)]'; 177 | test_value = 'qmult(Q1, Q2)'; 178 | failures=failures+check_float(truth_value, test_value, 1e-15); 179 | 180 | 181 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 182 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 183 | 184 | disp_num_failures(test_title, failures) 185 | 186 | -------------------------------------------------------------------------------- /ukfmani/coreclass/AGaussian.m: -------------------------------------------------------------------------------- 1 | classdef AGaussian 2 | % A Gaussian in a given manifold 3 | properties 4 | model % belongs to AManifold 5 | amean % G 6 | acov % AxA 7 | end 8 | 9 | methods 10 | function obj = AGaussian(amanifold,amean,acov) 11 | obj.model = amanifold; 12 | if nargin == 1 13 | obj.amean = zeros(1,obj.model.group); 14 | obj.acov = zeros(obj.model.alg); 15 | else 16 | assert(all(size(amean) == [1,obj.model.group]),'mismatch mean group size in AGaussian'); 17 | assert(all(size(acov) == [obj.model.alg,obj.model.alg]),'mismatch cov alg size in AGaussian'); 18 | obj.amean = amean; 19 | obj.acov = acov; 20 | end 21 | end 22 | 23 | function n = length(obj) 24 | n = size(obj.amean,1); 25 | end 26 | 27 | function m = mean(obj) 28 | m = obj.amean; 29 | end 30 | 31 | function c = cov(obj) 32 | c = obj.acov; 33 | end 34 | 35 | % compute sigma points for each Gaussian 36 | % Gaussian => [q,G] Sigma points 37 | function [Chi,vChi] = sigmas(obj,sigmainfo) 38 | if nargin == 1 39 | sigmainfo = obj.model.wsigma; 40 | end 41 | % decompose the covariance 42 | model = obj.model; 43 | S = obj.acov; 44 | mu = obj.amean; 45 | C = sigmainfo.sqrt(S); 46 | if size(C,1) ~= size(S,1) 47 | C = eye(size(S,1)); 48 | end 49 | k = size(C,1); 50 | Chi = zeros(2*k+1,model.group); % flattened 51 | Chi(1,:) = mu; % not weighted 52 | vChi = zeros(2*k+1,model.alg); % delta 53 | 54 | c = sigmainfo.c; 55 | for I=1:k 56 | psi = c*C(I,:); % ROW as ALL ALG 57 | vChi(I+1,:) = psi; 58 | vChi(I+1+k,:) = -psi; 59 | Chi(I+1,:) = model.step(mu,psi); 60 | Chi(I+1+k,:) = model.step(mu,-psi); 61 | end 62 | end 63 | 64 | % given the sigma points asscoated to the current object compute 65 | % the relative vector 66 | function vChi = vchifromchi(obj,Chi) 67 | model = obj.model; 68 | k = size(C,1); 69 | vChi = zeros(2*k+1,model.alg); % delta 70 | for I=1:k 71 | vChi(I+1,:) = model.delta(obj.mu,Chi(I+1,:)); 72 | vChi(I+1+k,:) = model.delta(obj.mu,Chi(I+1+k,:)); 73 | end 74 | 75 | end 76 | 77 | % marginalize the current Gaussian using the provided target 78 | % manifold and the indices that extract data from mean (group) and 79 | % covariance (algebra) 80 | function x = marginalize(obj,mx,indices,aindices) 81 | x = AGaussian(mx,obj.amean(indices),obj.acov(aindices,aindices)); 82 | end 83 | 84 | % given a multivariate manifold gaussian (xy) that is this object 85 | % 86 | % assigns the mean and covariance of the partition defined by (indices,aindices) using the Gaussian x 87 | function xy = assign(xy,x,indices,aindices) 88 | xy.amean(indices) = mean(x); 89 | xy.acov(aindices,aindices) = cov(x); 90 | end 91 | 92 | % applies the Kalman correction: 93 | % obj is the predicted state 94 | % z is the estimated observation 95 | % Cxz is the cross variance of xp and z from the h(x) computation 96 | % zvalur is the value of the observation 97 | % 98 | % note that if z has an additive noise R it has to be applied 99 | % BEFORE this condition 100 | function [xc,deltax] = condition(obj,z,Cxz,zvalue) 101 | Czz = cov(z); 102 | Cxx_p = cov(obj); 103 | K = Cxz/Czz; 104 | Cxx_c = (Cxx_p - K * Czz * K'); 105 | dz = z.model.delta(zvalue,mean(z)); 106 | deltax = (K*dz')'; 107 | mx_c = obj.model.step(mean(obj),deltax); 108 | 109 | xc = AGaussian(obj.model,mx_c,Cxx_c); 110 | end 111 | 112 | % transform all Gaussians using f and outputs new model 113 | % if model is not provided we assume that is the current model 114 | function [z,Cxz,Chiz] = transform(obj,f,outmodel) 115 | if nargin < 3 116 | outmodel = obj.model; 117 | end 118 | sigmainfo = obj.model.wsigma; 119 | [Chi,vChi] = obj.sigmas(sigmainfo); % Chi is [sigmas,packed group] 120 | assert(size(Chi,1) == size(vChi,1),'same group and vchi'); 121 | assert(size(Chi,2) == obj.model.group,'correct model group size'); 122 | assert(size(vChi,2) == obj.model.alg,'correct model alg size'); 123 | cXs = obj.model.unpack(Chi); % [sigmas, unpacked group] 124 | assert(size(cXs,1) == size(Chi,1),'same rows'); 125 | cZs = cell(size(cXs,1),outmodel.count); % [unpacked z group,sigmas] cell 126 | for I=1:size(cXs,1) % for every sigma point 127 | [cZs{I,:}] = f(cXs{I,:}); 128 | end 129 | % manipack wants [sigmas, unpacked z group] 130 | Chiz = manipack(outmodel,cZs); 131 | [zm,Czz,Cxz] = maniunsigma(outmodel,Chiz,sigmainfo,vChi); % [Gz,Gx] 132 | z = AGaussian(outmodel,zm,Czz); 133 | end 134 | 135 | 136 | 137 | % transform all Gaussians using f and outputs new model 138 | % if model is not provided we assume that is the current model 139 | function [z,Cxz,Chiz] = transformSigma(obj,Chix,f,outmodel) 140 | if nargin < 4 141 | outmodel = obj.model; 142 | end 143 | sigmainfo = obj.model.wsigma; 144 | cXs = obj.model.unpack(Chix); % cXs is [unpacked group,sigmas] cell 145 | cZs = cell(size(cXs,2),outmodel.count); % [unpacked z group,sigmas] cell 146 | for I=1:size(cXs,1) % for every sigma point 147 | [cZs{I,:}] = f(cXs{I,:}); 148 | end 149 | % manipack wants [sigmas, unpacked z group] 150 | Chiz = manipack(outmodel,cZs'); 151 | vChi = obj.vchifromchi(Chix); 152 | [zm,Czz,Cxz] = maniunsigma(outmodel,Chiz,sigmainfo,vChi); % [Gz,Gx] 153 | z = AGaussian(outmodel,zm,Czz); 154 | end 155 | 156 | % r = y * x 157 | function r = prod(y,x) 158 | assert(x.model.islie && y.model.islie,'needed lie'); 159 | %assert(x.model == y.model,'needed same model'); 160 | 161 | ya = y.model.adj(y.amean); 162 | r = AGaussian(y.model,y.model.prod(y.amean,x.amean),ya*x.acov*ya'+y.acov); 163 | end 164 | 165 | % Assuming this is a Lie Group performs the inversion of the 166 | % uncertain transformation 167 | function r = inv(obj) 168 | assert(obj.model.islie,'needed lie'); 169 | gi = obj.model.inv(obj.amean); 170 | gia = obj.model.adj(gi); 171 | r = AGaussian(obj.model,gi,gia*obj.acov*gia'); 172 | end 173 | 174 | % returns n samples that is [n,G] 175 | function r = sample(obj,n) 176 | p = randn(n(:)); 177 | r = AGaussian(obj.model); 178 | for I=length(p) 179 | 180 | end 181 | end 182 | end 183 | 184 | methods(Static) 185 | % assembles 1 Gaussian from sigma points 186 | function [r,Cxz] = unsigma(model,Chi,sigmainfo,vChi,steps) 187 | if nargin < 5 188 | steps = 20; 189 | end 190 | % estimates the mean in a weighted way 191 | N=size(Chi,1); 192 | 193 | v = zeros(size(Chi,1),model.alg); % preallocated 194 | mz = Chi(1,:)'; % COLUMN vector 195 | 196 | % for lie group we make a little optimization using inv 197 | if isfield(model,'log') 198 | % estimate mean but weighted of WM 199 | for k=1:steps 200 | imz = model.inv(mz); 201 | for i=1:N 202 | v(i,:) = model.log(model.prod(Chi(i,:)',imz)); 203 | end 204 | % update mz by weighted v 205 | mz = model.prod(model.exp(v'*sigmainfo.WM),mz); 206 | end 207 | 208 | % update v for computing covariance, skips the log 209 | imz = model.inv(mz); 210 | for i=1:N 211 | v(i,:) = model.log(model.prod(Chi(i,:)',imz)); 212 | end 213 | else 214 | % estimate mean but weighted of WM 215 | for k=1:steps 216 | for i=1:N 217 | % same as: se3_logdelta but with igk once 218 | v(i,:) = model.delta(Chi(i,:)',mz); 219 | end 220 | mz = model.step(mz,(v'*sigmainfo.WM)); % [A,S] [S,1] 221 | end 222 | % update v for computing covariance 223 | for i=1:N 224 | v(i,:) = model.delta(Chi(i,:)',mz); 225 | end 226 | end 227 | 228 | Czz = v'*sigmainfo.WC*v; % covariance ZZ - NOTE THAT WE USE DIFFERENTIAL 229 | if nargin >= 4 && nargout > 1 230 | Cxz = vChi'*sigmainfo.WC*v; % cross XZ - NOTE THAT WE USE DIFFERENTIAL 231 | end 232 | r = AGaussian(model,mz,Czz); 233 | end 234 | 235 | % given [n,G] points estimates 1 Gaussian 236 | function r = estimate(model,X,steps) 237 | if nargin < 3 238 | steps = 10; 239 | end 240 | 241 | % estimates the mean in a weighted way 242 | N=size(X,1); 243 | 244 | v = zeros(size(X,1),model.alg); % preallocated 245 | mz = X(1,:); % first is picked as mean 246 | 247 | % for lie group we make a little optimization using inv 248 | if nargout > 2 249 | nv = zeros(steps,3); 250 | end 251 | if isfield(model,'log') 252 | % estimate mean but weighted of WM 253 | for k=1:steps 254 | imz = model.inv(mz); 255 | for i=1:N 256 | v(i,:) = model.log(model.prod(X(i,:),imz)); 257 | end 258 | if nargout > 2 259 | p = sqrt(sum(v.^2,2)); 260 | nv(k,:) = [norm(mean(v,1)),max(p),mean(p)]; 261 | end 262 | % update mz by weighted v 263 | mz = model.prod(model.exp(mean(v,1)'),mz); 264 | end 265 | 266 | % update v for computing covariance, skips the log 267 | imz = model.inv(mz); 268 | for i=1:N 269 | v(i,:) = model.log(model.prod(X(i,:),imz)); 270 | end 271 | else 272 | % estimate mean but weighted of WM 273 | for k=1:steps 274 | for i=1:N 275 | % same as: se3_logdelta but with igk once 276 | v(i,:) = model.delta(X(i,:),mz); 277 | end 278 | if nargout > 2 279 | p = sqrt(sum(v.^2,2)); % norm of each vector, sized as Nx1 280 | nv(k,:) = [norm(mean(v,1)),max(p),mean(p)]; 281 | end 282 | mz = model.step(mz,mean(v,1)); % [A,S] [S,1] 283 | end 284 | 285 | % update v for computing covariance 286 | for i=1:N 287 | v(i,:) = model.delta(X(i,:),mz); 288 | end 289 | end 290 | 291 | S = 1/N*(v'*v); % covariance ZZ 292 | r = AGaussian(model,mz,S); 293 | end 294 | 295 | % given n Gaussians computes the fusion 296 | function r = fusion(ma,pts) 297 | end 298 | 299 | end 300 | end 301 | 302 | -------------------------------------------------------------------------------- /ukfmani/manifolds/makeGenProduct.py: -------------------------------------------------------------------------------- 1 | # TODO 2 | # Support Projection 3 | # Support SPD 4 | # Support transport 5 | import json 6 | import sys 7 | 8 | def makeop2(fx,sourcerange,sourcerange2,destrange): 9 | x1 = "x1({start}:{end})".format(start=sourcerange[0],end=sourcerange[1]) 10 | x2 = "x2({start}:{end})".format(start=sourcerange2[0],end=sourcerange2[1]) 11 | 12 | if type(fx) is str: 13 | content= fx.format(x1=x1,x2=x2) 14 | else: 15 | content = fx(x1,x2) 16 | return "\t\t\tz({start}:{end}) = {content};\n".format(start=destrange[0],end=destrange[1],content=content) 17 | 18 | def makeop1(fx,sourcerange,destrange): 19 | x = "x1({start}:{end})".format(start=sourcerange[0],end=sourcerange[1]) 20 | if type(fx) is str: 21 | content= fx.format(x=x) 22 | else: 23 | content = fx(x) 24 | return "\t\t\tz({start}:{end}) = {content};\n".format(start=destrange[0],end=destrange[1],content=content) 25 | 26 | rotextra=""" 27 | function omega = so3log(R) 28 | 29 | theta = acos((trace(R)-1)/2); %acos(max(-1,min((trace(R)-1)/2,1))); 30 | if isreal(theta) == 0 31 | R = R/abs(det(R)); 32 | theta = acos((trace(R)-1)/2); 33 | end 34 | 35 | if abs(theta) < 1e-10 36 | B = 0.5; 37 | SO = (1/(2))*(R-R'); % =skew(omega) 38 | iV = eye(3); % use directly skew of omega 39 | else 40 | A = sin(theta)/theta; 41 | B = (1-cos(theta))/(theta*theta); 42 | SO = (1/(2*A))*(R-R'); % =skew(omega) 43 | %?? 44 | % syms x real 45 | % A = sin(x)/x 46 | % B= (1-cos(x))/(x*x) 47 | % Q=1/(x*x)*(1 - A/2/B) 48 | % taylor(Q,x,0) 49 | % x^4/30240 + x^2/720 + 1/12 50 | Q= 1/(theta^2)*(1 - A/2/B); 51 | iV = eye(3) - 1/2*SO + Q*SO^2; % use directly skew of omega 52 | end 53 | 54 | omega = [SO(3,2) SO(1,3) SO(2,1)]; 55 | 56 | end 57 | 58 | % Emanuele Ruffaldi 2017 @ SSSA 59 | function R = so3exp(omega) 60 | 61 | theta = norm(omega); 62 | if theta < 1e-12 63 | % Original 64 | % A = 1; 65 | % B = 0.5; 66 | % C = 1/6; 67 | % S = zeros(3); 68 | % R = eye(3) + A*S + B*S^2; 69 | % V = eye(3) + B*S + C*S^2; 70 | 71 | N = 10; 72 | R = eye(3); 73 | xM = eye(3); 74 | cmPhi = skew(omega); 75 | for n = 1:N 76 | xM = xM * (cmPhi / n); 77 | R = R + xM; 78 | end 79 | 80 | % Project the resulting rotation matrix back onto SO(3) 81 | R = R / sqrtm( R'*R ) ; 82 | 83 | else 84 | %Original 85 | A = sin(theta)/theta; 86 | B = (1-cos(theta))/(theta^2); 87 | C = (theta-sin(theta))/(theta^3); 88 | S = skew(omega); 89 | R = eye(3) + A*S + B*S^2; 90 | %V = eye(3) + B*S + C*S^2; 91 | 92 | %Barfoot 93 | if 0==1 94 | axis = omega/theta; 95 | cp = cos(theta); 96 | sp = sin(theta); 97 | sa = skew(axis); 98 | 99 | R = cp*eye(3) + (1-cp)*axis*(axis') + sp*sa; 100 | end 101 | assert(abs(det(R)-1) < 1e-5,'unitary'); 102 | end 103 | end 104 | """ 105 | se3matextra=""" 106 | 107 | function y = se3mat_inv(x) 108 | 109 | M = reshape(x,4,4); 110 | R = M(1:3,1:3); 111 | y = eye(4); 112 | y(1:3,1:3) = R'; 113 | y(1:3,4) = -y(1:3,1:3)*M(1:3,4); 114 | y = reshape(y,1,[]); 115 | end 116 | 117 | 118 | function y = se3mat_log(x) 119 | 120 | M = reshape(x,4,4); 121 | t = M(1:3,4); 122 | R = M(1:3,1:3); 123 | 124 | theta = acos((trace(R)-1)/2); %acos(max(-1,min((trace(R)-1)/2,1))); 125 | if isreal(theta) == 0 126 | R = R/abs(det(R)); 127 | theta = acos((trace(R)-1)/2); 128 | end 129 | 130 | if abs(theta) < 1e-10 131 | B = 0.5; 132 | SO = (1/(2))*(R-R'); % =skew(omega) 133 | iV = eye(3); % use directly skew of omega 134 | else 135 | A = sin(theta)/theta; 136 | B = (1-cos(theta))/(theta*theta); 137 | SO = (1/(2*A))*(R-R'); % =skew(omega) 138 | %?? 139 | % syms x real 140 | % A = sin(x)/x 141 | % B= (1-cos(x))/(x*x) 142 | % Q=1/(x*x)*(1 - A/2/B) 143 | % taylor(Q,x,0) 144 | % x^4/30240 + x^2/720 + 1/12 145 | Q= 1/(theta^2)*(1 - A/2/B); 146 | iV = eye(3) - 1/2*SO + Q*SO^2; % use directly skew of omega 147 | end 148 | omega = [SO(3,2) SO(1,3) SO(2,1)]; 149 | 150 | %y = [m1.log(getrot(x)), getpos(x)]; % not exact 151 | u = iV*t; 152 | y = [omega(:);u(:)]'; 153 | end 154 | 155 | function y = se3mat_exp(x) 156 | 157 | omega = x(1:3); 158 | u = x(4:6); 159 | 160 | theta = norm(omega); 161 | if theta < 1e-12 162 | % Original 163 | % A = 1; 164 | % B = 0.5; 165 | % C = 1/6; 166 | % S = zeros(3); 167 | % R = eye(3) + A*S + B*S^2; 168 | % V = eye(3) + B*S + C*S^2; 169 | 170 | N = 10; 171 | R = eye(3); 172 | xM = eye(3); 173 | cmPhi = skew(omega); 174 | V = eye(3); 175 | pxn = eye(3); 176 | for n = 1:N 177 | xM = xM * (cmPhi / n); 178 | pxn = pxn*cmPhi/(n + 1); 179 | R = R + xM; 180 | V = V + pxn; 181 | end 182 | 183 | % Project the resulting rotation matrix back onto SO(3) 184 | R = R / sqrtm( R'*R ) ; 185 | 186 | else 187 | %Original 188 | if 1==1 189 | A = sin(theta)/theta; 190 | B = (1-cos(theta))/(theta^2); 191 | C = (theta-sin(theta))/(theta^3); 192 | S = skew(omega); 193 | R = eye(3) + A*S + B*S^2; 194 | V = eye(3) + B*S + C*S^2; 195 | else 196 | %Barfoot 197 | 198 | axis = omega/theta; 199 | cp = cos(theta); 200 | sp = sin(theta); 201 | cph = (1 - cos(theta))/theta; 202 | sph = sin(theta)/theta; 203 | sa = skew(axis); 204 | 205 | R = cp*eye(3) + (1-cp)*axis*(axis') + sp*sa; 206 | V = sph * eye(3) + (1 - sph) * axis * (axis') + cph * sa; 207 | end 208 | 209 | end 210 | 211 | y = eye(4); 212 | y(1:3,1:3) = R; 213 | y(1:3,4) = V*u(:); 214 | y = reshape(y,1,[]); 215 | 216 | end 217 | 218 | """ 219 | 220 | se3quatextra="" 221 | 222 | def solvemodel(m): 223 | if m["type"][0] == "Quat": 224 | return dict(product="qmult({x1},{x2})",delta="qmdelta({x1},{x2})",step="qmstep({x1},{x2})",inv="qconj({x})",pack="{x}",unpack="{x}") 225 | elif m["type"][0] == "Rot": 226 | return dict(extra=rotextra,product="reshape(reshape({x1},3,3)*reshape({x2},3,3),1,[])",delta="so3log(reshape({x1},3,3)*reshape({x2},3,3)')",step="reshape(so3exp({x2})*reshape({x1},3,3),1,[])",inv="reshape(reshap({x},3,3)',1,[])",exp="reshape(so3exp({x}),1,[])",log="so3log(reshape({x},3,3))",pack="reshape({x},1,[])",unpack="reshape({x},3,3)") 227 | elif m["type"][0] == "SE3Mat" or m["type"][0] == "SE3MatGlobal": 228 | return dict(extra=se3matextra,product="reshape(reshape({x1},4,4)*reshape({x2},4,4),1,[])",delta="se3mat_log(reshape({x1},4,4)*reshape(se3mat_inv({x2}),4,4))",step="reshape(reshape(se3mat_exp({x2}),4,4)*reshape({x1},4,4),1,[])",inv="se3mat_inv({x})",log="se3mat_log({x})",exp="se3mat_exp({x})",pack="reshape({x},1,[])",unpack="reshape({x},4,4)") 229 | #elif m["type"][0] == "SE3Quat": 230 | # return dict(extra=se3quatextra,product="se3quat_prod({x1},{x2})",delta="se3log(se3mat_munflat({x1})*se3mat_munflat(se3inv({x2})))",step="se3mat_mflat(se3mat_munflat(m.exp({x2}))*se3mat_munflat({x1}))",inv="se3mat_inv({x})",log="se3mat_log({x})",exp="se3mat_exp({x})",pack="se3mat_mflat({x1})",unpack="se3mat_munflat({x1})") 231 | elif m["type"][0] == "Rn": 232 | n = m["type"][1] 233 | return dict(product="{x1}+{x2}",delta="{x1}-{x2}",step="{x1}+{x2}",inv="-{x}",log="{x}",exp="{x}",pack=lambda x: x,unpack=lambda x:x) 234 | else: 235 | raise Exception("!!! Unsupported %s %s" % (m["type"],m)) 236 | #return dict(product="anyprod",delta="anydelta",step="anystep",inv="anyinv",log="anylog",exp="anyexp",pack=lambda x: x,unpack=lambda x:x) 237 | def main(): 238 | if len(sys.argv) != 2: 239 | print "expected input JSON"; 240 | return 241 | jq = json.load(open(sys.argv[1],"rb")) 242 | output = jq.get("output","-") 243 | group = jq["group"] 244 | alg = jq["alg"] 245 | count = jq["count"] 246 | models = jq["models"] 247 | ginc = jq["groupinc"] 248 | ainc = jq["alginc"] 249 | print jq["type"],group,alg,models,jq["groupinc"],jq["alginc"] 250 | 251 | # check if 252 | maybeexp="""m.log = @mlog; 253 | m.exp = @mexp; 254 | """ if jq.get("withlog",False) else "" 255 | 256 | solvedmodels = [solvemodel(m) for m in jq["models"]] 257 | output = """ 258 | function m = customManifold() 259 | m = []; 260 | m.type = 'special'; 261 | m.inv = @minvert; 262 | m.prod = @mproduct; 263 | {maybeexp} 264 | m.delta = @mdelta; 265 | m.step = @mstep; 266 | m.group = {group}; 267 | m.alg = {alg}; 268 | m.count = {count}; 269 | m.pack = @mpack; 270 | m.unpack = @munpack; 271 | m.s = int_manisetup([],[],m); 272 | end 273 | """.format(group=group,maybeexp=maybeexp,alg=alg,count=jq["count"]) 274 | 275 | # output inv: group -> group 276 | output += """ 277 | function z = minvert(x1) 278 | z = zeros({group},1); 279 | """.format(group=group) 280 | for i in range(0,len(jq["models"])): 281 | output += makeop1(solvedmodels[i]["inv"],(ginc[i]+1,ginc[i+1]),(ginc[i]+1,ginc[i+1])) 282 | output += """ end 283 | """ 284 | # output prod: group,group -> group 285 | output += """ 286 | function z = mproduct(x1,x2) 287 | z = zeros({group},1); 288 | """.format(group=group) 289 | for i in range(0,len(jq["models"])): 290 | output += makeop2(solvedmodels[i]["product"],(ginc[i]+1,ginc[i+1]),(ginc[i]+1,ginc[i+1]),(ginc[i]+1,ginc[i+1])) 291 | output += """ 292 | end 293 | """ 294 | # output log: group -> alg 295 | if maybeexp != "": 296 | output += """ 297 | function z = mlog(x1) 298 | z = zeros({alg},1); 299 | """.format(alg=alg) 300 | for i in range(0,len(jq["models"])): 301 | output += makeop1(solvedmodels[i]["log"],(ginc[i]+1,ginc[i+1]),(ainc[i]+1,ainc[i+1])) 302 | output += """ 303 | end 304 | """ 305 | # output exp: alg -> group 306 | if maybeexp != "": 307 | output += """ 308 | function z = mexp(x1) 309 | z = zeros({group},1); 310 | """.format(alg=alg,group=group) 311 | for i in range(0,len(jq["models"])): 312 | output += makeop1(solvedmodels[i]["exp"],(ainc[i]+1,ainc[i+1]),(ginc[i]+1,ginc[i+1])) 313 | output += """ end 314 | """ 315 | # output delta: group,group -> alg 316 | output += """ 317 | function z = mdelta(x1,x2) 318 | z = zeros({alg},1); 319 | """.format(alg=alg) 320 | for i in range(0,len(jq["models"])): 321 | output += makeop2(solvedmodels[i]["delta"],(ginc[i]+1,ginc[i+1]),(ginc[i]+1,ginc[i+1]),(ainc[i]+1,ainc[i+1])) 322 | output += """ end 323 | """ 324 | # output step: group,alg -> group 325 | output += """ 326 | function z = mstep(x1,x2) 327 | z = zeros({group},1); 328 | """.format(group=group) 329 | for i in range(0,len(jq["models"])): 330 | output += makeop2(solvedmodels[i]["step"],(ginc[i]+1,ginc[i+1]),(ainc[i]+1,ainc[i+1]),(ginc[i]+1,ginc[i+1])) 331 | output += """ end 332 | """ 333 | # output pack: cell array -> group 334 | output += """ 335 | function z = mpack(x1) 336 | z = zeros({group},size(x1,2)); 337 | for j=1:size(x1,2) 338 | """.format(group=group) 339 | for i in range(0,len(jq["models"])): 340 | x = "x1{pre}{i},j{post}".format(pre="{",post="}",i=i+1) 341 | fx = solvedmodels[i]["pack"] 342 | if type(fx) is str: 343 | content = fx.format(x=x) 344 | else: 345 | content = fx(x) 346 | output += "\t\t\t\tz({start}:{end},j) = {content};\n".format(astart=ainc[i]+1,aend=ainc[i+1],start=ginc[i]+1,end=ginc[i+1],content=content) 347 | 348 | output += """ end 349 | end 350 | """ 351 | # output unpack: group -> cell 352 | output += """ 353 | function z = munpack(x1) 354 | z = cell({count},size(x1,2)); 355 | for j=1:size(x1,2) 356 | """.format(group=group,count=count) 357 | for i in range(0,len(jq["models"])): 358 | x = "x1({start}:{end},j)".format(start=ginc[i]+1,end=ginc[i+1]) 359 | fx = solvedmodels[i]["unpack"] 360 | if type(fx) is str: 361 | content = fx.format(x=x) 362 | else: 363 | content = fx(x) 364 | output += "\t\t\t\tz{pre}{i},j{post} = {content};\n".format(pre="{",post="}",i=i+1,astart=ainc[i]+1,aend=ainc[i+1],start=ginc[i]+1,end=ginc[i+1],content=content) 365 | output += """ end 366 | end 367 | """ 368 | 369 | ee = set() 370 | for m in solvedmodels: 371 | q= m.get("extra","") 372 | ee.add(q) 373 | for q in ee: 374 | output += q 375 | output += "\n" 376 | if output == "-": 377 | print output 378 | else: 379 | open(jq["output"],"w").write(output) 380 | 381 | if __name__ == '__main__': 382 | main() --------------------------------------------------------------------------------