├── .gitignore ├── matlab ├── math │ ├── getFLE.m │ ├── getInvQuat.m │ ├── getXfrmPointQuat.m │ ├── getRotMatrixd.m │ ├── getConjQuat.m │ ├── getQuatNormalized.m │ ├── getNDIRelXfrm.m │ ├── getPointtoPlane.m │ ├── rotateCovQuat.m │ ├── getInvXfrmQuat.m │ ├── getCombXfrm.m │ ├── registration │ │ ├── AnisotropicWeighted │ │ │ ├── e_maker.m │ │ │ ├── C_maker.m │ │ │ ├── point_register.m │ │ │ └── anisotropic_point_register.m │ │ ├── updateRegistration.m │ │ ├── getFRE.m │ │ ├── getRigidXfrmSVD.m │ │ └── getRigidXfrm.m │ ├── isIsotropic.m │ ├── getRotMatrix.m │ ├── getEulerAngles.m │ ├── screw2rot.m │ ├── getLinePlaneIntersection.m │ ├── getRandomXfrm.m │ ├── quat2rm.m │ ├── getRotPointQuat.m │ ├── getPA.m │ ├── TransformRigidBody.m │ ├── getRelativeMeasPoints.m │ ├── solveellipse.m │ ├── getSphereFit.m │ ├── getQuatMultiply.m │ ├── fitellip.m │ └── rm2quat.m ├── stats │ ├── HsuSpatialError │ │ ├── book1e.log │ │ ├── nsd.dat │ │ ├── nsd1.dat │ │ ├── rf.mat │ │ ├── atp.mat │ │ ├── bf5.mat │ │ ├── chi.mat │ │ ├── chi6.mat │ │ ├── cnf.mat │ │ ├── cup.mat │ │ ├── drms.mat │ │ ├── dtms.mat │ │ ├── ee4.mat │ │ ├── ee8.mat │ │ ├── mucnf.m │ │ ├── p2x1.mat │ │ ├── p2x2.mat │ │ ├── p2x3.mat │ │ ├── p2x4.mat │ │ ├── p2x5.mat │ │ ├── p2x6.mat │ │ ├── p2x7.mat │ │ ├── p2x8.mat │ │ ├── p2x9.mat │ │ ├── p83.mat │ │ ├── pe4.mat │ │ ├── pe8.mat │ │ ├── pf5.mat │ │ ├── pf50.mat │ │ ├── pf90.mat │ │ ├── pfr5.mat │ │ ├── pfr6.mat │ │ ├── pfr7.mat │ │ ├── pfr8.mat │ │ ├── pfr9.mat │ │ ├── psi.mat │ │ ├── psi6.mat │ │ ├── x2cdf.m │ │ ├── book1e.pdf │ │ ├── book1e.ps │ │ ├── drmsr.mat │ │ ├── drmsr4.mat │ │ ├── ee3_50.mat │ │ ├── ee3_90.mat │ │ ├── eg1p2.mat │ │ ├── eg3p2.mat │ │ ├── eg6.1 │ │ ├── ellp1.mat │ │ ├── ellp1a.mat │ │ ├── ellp2.mat │ │ ├── ellp2a.mat │ │ ├── fig4_10.mat │ │ ├── fig4_13.mat │ │ ├── fig4_7.mat │ │ ├── fig4_8.mat │ │ ├── fig4_9.mat │ │ ├── inter6.mat │ │ ├── may13.mat │ │ ├── mucnf.mat │ │ ├── p2x10.mat │ │ ├── p2x11.mat │ │ ├── p2x12.mat │ │ ├── p2x13.mat │ │ ├── p2x14.mat │ │ ├── p2x15.mat │ │ ├── p2x16.mat │ │ ├── p2x17.mat │ │ ├── p2x18.mat │ │ ├── p2x19.mat │ │ ├── p2x20.mat │ │ ├── p2x21.mat │ │ ├── p2x22.mat │ │ ├── p2x23.mat │ │ ├── p2x24.mat │ │ ├── p2x25.mat │ │ ├── p2x26.mat │ │ ├── p2x27.mat │ │ ├── p2x28.mat │ │ ├── p2x29.mat │ │ ├── p2x30.mat │ │ ├── p2x31.mat │ │ ├── p2x32.mat │ │ ├── p2x33.mat │ │ ├── p2x34.mat │ │ ├── p2x35.mat │ │ ├── p2x36.mat │ │ ├── p2x37.mat │ │ ├── p2x38.mat │ │ ├── p2x39.mat │ │ ├── p2x40.mat │ │ ├── pe3_50.mat │ │ ├── pe3_90.mat │ │ ├── pe7_50.mat │ │ ├── pe7_90.mat │ │ ├── pf5_50.mat │ │ ├── pf5_95.mat │ │ ├── pfr5a.mat │ │ ├── psichi.mat │ │ ├── psichi6.mat │ │ ├── rmsed1.mat │ │ ├── rmsed2.mat │ │ ├── tf5_50.mat │ │ ├── tf5_95.mat │ │ ├── x2disc2.m │ │ ├── drms2cep.mat │ │ ├── eqcir3_5.mat │ │ ├── eqcir3_9.mat │ │ ├── fig4_13a.mat │ │ ├── eg6.2 │ │ ├── sdsvui.m │ │ ├── n2gmrms.m │ │ ├── eg6.3 │ │ ├── myfun1.m │ │ ├── rmsx.m │ │ ├── sf1.m │ │ ├── x2dg.m │ │ ├── ichi2.m │ │ ├── cdfchi2.m │ │ ├── bf3.m │ │ ├── psi.m │ │ ├── simprule.m │ │ ├── cf1.m │ │ ├── nf6.m │ │ ├── nf1.m │ │ ├── pf3.m │ │ ├── gofu.m │ │ ├── tdis.m │ │ ├── nf2.m │ │ ├── sf3a.m │ │ ├── x2df.m │ │ ├── gmrms2n.m │ │ ├── cln2a.m │ │ ├── ang2p.m │ │ ├── sf2.m │ │ ├── abc2ac.m │ │ ├── edevud.m │ │ ├── an2ka.m │ │ ├── cf2.m │ │ ├── sem1.m │ │ ├── sdsvud.m │ │ ├── cdcaui.m │ │ ├── cf3.m │ │ ├── cdcaud.m │ │ ├── edsvud.m │ │ ├── gf2.m │ │ ├── pdft.m │ │ ├── nf3.m │ │ ├── r2pc6.m │ │ ├── edcaud.m │ │ ├── gmrms.m │ │ ├── tandn.m │ │ ├── bf1.m │ │ ├── edraud.m │ │ ├── bf2.m │ │ ├── anl2b.m │ │ ├── newton.m │ │ ├── edbvud.m │ │ ├── edcaci.m │ │ ├── edcaui.m │ │ ├── xandn.m │ │ ├── cln2b.m │ │ ├── fsca.m │ │ ├── bf4.m │ │ ├── psi.dat │ │ ├── tf3.m │ │ ├── sf4.m │ │ ├── edsvui.m │ │ ├── edeaud.m │ │ ├── sf3.m │ │ ├── edcacd.m │ │ ├── edraui.m │ │ ├── edsvci.m │ │ ├── psi1.m │ │ ├── edeaui.m │ │ ├── edsv.m │ │ ├── psinh.m │ │ ├── edbvui.m │ │ ├── sem2.m │ │ ├── nf7.m │ │ ├── nf2a.m │ │ ├── psin.m │ │ ├── chi2.m │ │ ├── p2r1d.m │ │ ├── edsvcd.m │ │ ├── edevci.m │ │ ├── edevui.m │ │ ├── noval.m │ │ ├── p2rc6.m │ │ ├── edeaci.m │ │ ├── edevcd.m │ │ ├── p2r3d.m │ │ ├── p2r2d.m │ │ ├── cnf.m │ │ ├── edca.m │ │ ├── edeacd.m │ │ ├── psichi.dat │ │ ├── noval1.m │ │ ├── r2p1d.m │ │ ├── tf2.m │ │ ├── chi.dat │ │ ├── r2p3d.m │ │ ├── r2p2d.m │ │ ├── chi1.m │ │ ├── cf4a.dat │ │ ├── cf4b.dat │ │ ├── nf5a.dat │ │ ├── nf5b.dat │ │ ├── sf5a.dat │ │ └── sf5b.dat │ ├── chidist.m │ ├── nansuite │ │ ├── nanmax.m │ │ ├── nanmin.m │ │ ├── nansem.m │ │ ├── nanstd.m │ │ ├── nansum.m │ │ ├── nanmean.m │ │ └── nanmedian.m │ ├── normdist.m │ ├── maxdist.m │ ├── dchisq.m │ ├── getSkewness.m │ ├── getKurtosis.m │ ├── qchisq.m │ ├── radius3Ddist.m │ ├── getPercentile.m │ ├── pchisq.m │ ├── checkChi2Test.m │ ├── getCentralMoments.m │ ├── WishartHypTest.m │ └── get3DErrorStats.m ├── plotting │ ├── shared │ │ ├── rgb.m │ │ └── license.txt │ ├── plotRadius3DDist.m │ ├── plotSphere.m │ ├── plotDistance3DDist.m │ ├── plotCumHist.m │ ├── drawCube.m │ └── getOrderedPlotColor.m ├── email_setup.m └── target_error │ ├── montecarlo │ ├── weightMatrix.m │ ├── getMeasNoise.m │ ├── getRandOrientation.m │ ├── getQuadraticFLE.m │ ├── getFLEMatrix.m │ ├── computeStats.m │ ├── getWestToolDesign.m │ ├── generateRandProbeXfrm.m │ └── generateRandFiducialTargets.m │ ├── misc │ ├── old_possibly_obsolete │ │ └── runEstimateFRE.m │ ├── TREWest.m │ ├── stochTest.m │ ├── square_tool.m │ ├── toolPA.m │ ├── TREtest2.m │ └── TREtestNonUniform.m │ ├── estimateFLE │ ├── xfrmToPAWrapper.m │ └── updateFREStats.m │ ├── int2charXLS.m │ ├── convertPathForSimulink.m │ └── experiments │ ├── estimateTTE │ ├── readXfrmFile.m │ ├── runPlotTTEMonteCarlo.m │ ├── runAllTTEMonteCarloPlots.m │ └── readFTECovariance.m │ ├── estimateFLERegisterRealData │ └── readFromXLS.m │ ├── estimateTRE │ ├── runTRESimScript.m │ ├── plotRandProbeXfrmExperiment.m │ ├── readme.txt │ └── plotRandFiducialTargetExperiment.m │ └── estimateFLE │ ├── runEstimateFLEDynamicSingleRun.m │ ├── testRandomWalk.m │ └── runEstimateFLEDynamic.m └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | *.asv 2 | -------------------------------------------------------------------------------- /matlab/math/getFLE.m: -------------------------------------------------------------------------------- 1 | function FLE = getFLE(Sigma); 2 | 3 | FLE = trace(Sigma); -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/README.md -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/book1e.log: -------------------------------------------------------------------------------- 1 | %%[ Page: 1 ]%% 2 | %%[ LastPage ]%% 3 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/nsd.dat: -------------------------------------------------------------------------------- 1 | 3 40 2 | 10 15 3 | 15 20 4 | 30 10 5 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/nsd1.dat: -------------------------------------------------------------------------------- 1 | 15 10 45 2 | 10 20 60 3 | 10 20 150 4 | -------------------------------------------------------------------------------- /matlab/stats/chidist.m: -------------------------------------------------------------------------------- 1 | function p = chidist(x,k) 2 | 3 | p = 0.5^(k/2)/gamma(k/2)*x.^(k/2 - 1) .* exp(-x/2); -------------------------------------------------------------------------------- /matlab/math/getInvQuat.m: -------------------------------------------------------------------------------- 1 | function q = getInvQuat( q0 ); 2 | 3 | qNorm = sum(q0.^2); 4 | q = (1/qNorm)*getConjQuat(q0); -------------------------------------------------------------------------------- /matlab/plotting/shared/rgb.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/plotting/shared/rgb.m -------------------------------------------------------------------------------- /matlab/stats/nansuite/nanmax.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/nansuite/nanmax.m -------------------------------------------------------------------------------- /matlab/stats/nansuite/nanmin.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/nansuite/nanmin.m -------------------------------------------------------------------------------- /matlab/stats/nansuite/nansem.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/nansuite/nansem.m -------------------------------------------------------------------------------- /matlab/stats/nansuite/nanstd.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/nansuite/nanstd.m -------------------------------------------------------------------------------- /matlab/stats/nansuite/nansum.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/nansuite/nansum.m -------------------------------------------------------------------------------- /matlab/stats/nansuite/nanmean.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/nansuite/nanmean.m -------------------------------------------------------------------------------- /matlab/math/getXfrmPointQuat.m: -------------------------------------------------------------------------------- 1 | function pos = getXfrmPointQuat(xfrm, pos0) 2 | 3 | pos = getRotPointQuat( xfrm.rot, pos0) + xfrm.pos; -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/rf.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/rf.mat -------------------------------------------------------------------------------- /matlab/stats/nansuite/nanmedian.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/nansuite/nanmedian.m -------------------------------------------------------------------------------- /matlab/math/getRotMatrixd.m: -------------------------------------------------------------------------------- 1 | function rot = getRotMatrixd(euler) 2 | 3 | euler = (pi/180) * euler; 4 | 5 | rot = getRotMatrix( euler ); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/atp.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/atp.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/bf5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/bf5.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/chi.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/chi.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/chi6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/chi6.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cnf.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/cnf.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cup.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/cup.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/drms.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/drms.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/dtms.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/dtms.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/ee4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/ee4.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/ee8.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/ee8.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/mucnf.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/mucnf.m -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x1.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x2.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x3.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x4.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x5.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x6.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x7.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x7.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x8.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x8.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x9.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x9.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p83.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p83.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pe4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pe4.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pe8.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pe8.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pf5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pf5.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pf50.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pf50.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pf90.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pf90.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pfr5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pfr5.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pfr6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pfr6.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pfr7.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pfr7.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pfr8.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pfr8.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pfr9.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pfr9.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/psi.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/psi.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/psi6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/psi6.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/x2cdf.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/x2cdf.m -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/book1e.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/book1e.pdf -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/book1e.ps: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/book1e.ps -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/drmsr.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/drmsr.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/drmsr4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/drmsr4.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/ee3_50.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/ee3_50.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/ee3_90.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/ee3_90.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/eg1p2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/eg1p2.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/eg3p2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/eg3p2.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/eg6.1: -------------------------------------------------------------------------------- 1 | 2.38 2 | 5.18 3 | 8.17 4 | 13.43 5 | 4.43 6 | 0.54 7 | 6.23 8 | 1.08 9 | 2.25 10 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/ellp1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/ellp1.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/ellp1a.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/ellp1a.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/ellp2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/ellp2.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/ellp2a.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/ellp2a.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/fig4_10.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/fig4_10.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/fig4_13.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/fig4_13.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/fig4_7.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/fig4_7.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/fig4_8.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/fig4_8.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/fig4_9.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/fig4_9.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/inter6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/inter6.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/may13.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/may13.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/mucnf.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/mucnf.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x10.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x10.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x11.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x11.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x12.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x12.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x13.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x13.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x14.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x14.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x15.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x15.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x16.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x16.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x17.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x17.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x18.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x18.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x19.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x19.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x20.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x20.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x21.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x21.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x22.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x22.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x23.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x23.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x24.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x24.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x25.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x25.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x26.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x26.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x27.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x27.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x28.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x28.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x29.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x29.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x30.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x30.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x31.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x31.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x32.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x32.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x33.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x33.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x34.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x34.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x35.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x35.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x36.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x36.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x37.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x37.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x38.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x38.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x39.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x39.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2x40.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/p2x40.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pe3_50.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pe3_50.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pe3_90.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pe3_90.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pe7_50.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pe7_50.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pe7_90.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pe7_90.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pf5_50.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pf5_50.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pf5_95.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pf5_95.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pfr5a.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/pfr5a.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/psichi.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/psichi.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/psichi6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/psichi6.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/rmsed1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/rmsed1.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/rmsed2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/rmsed2.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/tf5_50.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/tf5_50.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/tf5_95.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/tf5_95.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/x2disc2.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/x2disc2.m -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/drms2cep.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/drms2cep.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/eqcir3_5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/eqcir3_5.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/eqcir3_9.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/eqcir3_9.mat -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/fig4_13a.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awiles/ErrorAnalysisToolkit/HEAD/matlab/stats/HsuSpatialError/fig4_13a.mat -------------------------------------------------------------------------------- /matlab/email_setup.m: -------------------------------------------------------------------------------- 1 | function email_setup(smtp, email) 2 | % email set up. 3 | 4 | setpref('Internet','SMTP_Server',smtp); 5 | setpref('Internet','E_mail',email); -------------------------------------------------------------------------------- /matlab/math/getConjQuat.m: -------------------------------------------------------------------------------- 1 | function q = getConjQuat(q0); 2 | %#eml 3 | q = zeros(1,4); 4 | q(1) = q0(1); 5 | q(2) = -q0(2); 6 | q(3) = -q0(3); 7 | q(4) = -q0(4); -------------------------------------------------------------------------------- /matlab/math/getQuatNormalized.m: -------------------------------------------------------------------------------- 1 | function q = getQuatNormalized(q0); 2 | %#eml 3 | qNorm = sqrt(sum(q0.^2)); 4 | if (q0(1) < 0) 5 | qNorm = -1*qNorm; 6 | end 7 | 8 | q = (1/qNorm) * q0; -------------------------------------------------------------------------------- /matlab/target_error/montecarlo/weightMatrix.m: -------------------------------------------------------------------------------- 1 | function sigma = weightMatrix(rms2, w) 2 | 3 | rw2 = ((1/sum(w)) * w).^2; 4 | sigma2 = (1/sum(rw2))*rms2; 5 | 6 | sigma = diag(sigma2*rw2); -------------------------------------------------------------------------------- /matlab/stats/normdist.m: -------------------------------------------------------------------------------- 1 | function g = normdist(t, mu, sigma) 2 | 3 | g = zeros(size(t)); 4 | 5 | for i = 1:length(t) 6 | g(i) = (1/(sqrt(2*pi)*sigma)) * exp(-1*((t(i)- mu)^2)/(2*sigma)); 7 | end -------------------------------------------------------------------------------- /matlab/plotting/plotRadius3DDist.m: -------------------------------------------------------------------------------- 1 | function plotRadius3DDist(Sigma, t, lineprop) 2 | 3 | step = t(2) - t(1); 4 | t = t + (step/2)*ones(size(t)); 5 | g = radius3Ddist(t, Sigma); 6 | plot(t, g, lineprop); -------------------------------------------------------------------------------- /matlab/math/getNDIRelXfrm.m: -------------------------------------------------------------------------------- 1 | function xfrm = getNDIRelXfrm(refXfrm, toolXfrm) 2 | 3 | invRefXfrm = getInvXfrmQuat(refXfrm); 4 | %xfrm = getCombXfrm(toolXfrm, refXfrm); 5 | xfrm = getCombXfrm(invRefXfrm, toolXfrm); -------------------------------------------------------------------------------- /matlab/math/getPointtoPlane.m: -------------------------------------------------------------------------------- 1 | function [p_close,dist] = getPointtoPlane(n, orig, pt) 2 | 3 | %normalize the normal. 4 | n = 1/(sum(n.^2)^(1/2)) * n; 5 | 6 | dist = n * (pt-orig)'; 7 | 8 | p_close = pt - dist*n; 9 | -------------------------------------------------------------------------------- /matlab/stats/maxdist.m: -------------------------------------------------------------------------------- 1 | function g = maxdist(t, rms) 2 | 3 | g = zeros(size(t)); 4 | 5 | sigma = rms/sqrt(3); 6 | 7 | for i = 1:length(t) 8 | g(i) = sqrt(2/pi) * (t(i)^2/sigma^3) * exp(-(t(i)^2)/(2*sigma^2)); 9 | end -------------------------------------------------------------------------------- /matlab/stats/dchisq.m: -------------------------------------------------------------------------------- 1 | function f=dchisq(x,n) 2 | % DCHISQ(X,N) - Density function of the chi-square distribution. 3 | if nargin<2 4 | n=1; 5 | end 6 | f=zeros(size(x)); 7 | s = x>=0; 8 | f(s) = x(s).^(n/2-1).*exp(-x(s)/2)./(2^(n/2)*gamma(n/2)); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/eg6.2: -------------------------------------------------------------------------------- 1 | 2.38 -13.39 2 | 0.18 -24.45 3 | 8.17 3.40 4 | 3.43 -2.76 5 | 4.43 -3.58 6 | 0.54 0.49 7 | 4.00 -1.03 8 | -------------------------------------------------------------------------------- /matlab/target_error/misc/old_possibly_obsolete/runEstimateFRE.m: -------------------------------------------------------------------------------- 1 | % script to run estimateFRE 2 | clear all; 3 | close all; 4 | estimateFRE(1,100); 5 | estimateFRE(1,200); 6 | estimateFRE(1,500); 7 | estimateFRE(2,100); 8 | estimateFRE(2,200); 9 | estimateFRE(2,500); -------------------------------------------------------------------------------- /matlab/math/rotateCovQuat.m: -------------------------------------------------------------------------------- 1 | function [m,cov] = rotateCovQuat(mu, Sigma, q) 2 | 3 | R = (quat2rm(q)); 4 | 5 | m = (R*mu')'; 6 | cov = R*Sigma*R'; 7 | 8 | %DEBUG: 9 | % x.pos = [0 0 0]; 10 | % x.rot = q; 11 | % m_q = getXfrmPointQuat(x, mu); 12 | % 13 | % m 14 | % m_q -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/sdsvui.m: -------------------------------------------------------------------------------- 1 | function R=sdsvui(p,sx) 2 | % sdsvui.m 2-20-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | R=sx*sf3(p); 9 | -------------------------------------------------------------------------------- /matlab/math/getInvXfrmQuat.m: -------------------------------------------------------------------------------- 1 | function xfrm = getInvXfrmQuat(xfrm0) 2 | 3 | %invert the quaternion 4 | xfrm.rot = getQuatNormalized(getInvQuat(xfrm0.rot)); 5 | 6 | %rotate the coordinate frame location and change direction. 7 | xfrm.pos = (-1) * getRotPointQuat( xfrm.rot, xfrm0.pos); 8 | 9 | -------------------------------------------------------------------------------- /matlab/target_error/estimateFLE/xfrmToPAWrapper.m: -------------------------------------------------------------------------------- 1 | function A0 = xfrmToPAWrapper(x0) 2 | %#eml 3 | 4 | [U, L, V, x, A, Ainv] = xfrmToPA(x0); 5 | 6 | % stack the matrices for now. 7 | N = length(A); 8 | 9 | A0 = zeros(N*6,6); 10 | for i = 1:N 11 | A0(((i-1)*N+1):i*N,:) = A{i}; 12 | end -------------------------------------------------------------------------------- /matlab/math/getCombXfrm.m: -------------------------------------------------------------------------------- 1 | function xfrm = getCombXfrm(xfrm1, xfrm2) 2 | 3 | %% multiply the quaternions 4 | xfrm.rot = getQuatMultiply( xfrm2.rot, xfrm1.rot); 5 | 6 | %% rotate the origin of the one coordinate frame. 7 | xfrm.pos = getRotPointQuat( xfrm2.rot, xfrm1.pos ); 8 | xfrm.pos = xfrm.pos + xfrm2.pos; 9 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/n2gmrms.m: -------------------------------------------------------------------------------- 1 | function ratio=n2gmrms(n) 2 | % n2gmrms.m 3-14-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | y=psin(n/2); 9 | ratio=exp(y/2)/sqrt(n/2); 10 | -------------------------------------------------------------------------------- /matlab/stats/getSkewness.m: -------------------------------------------------------------------------------- 1 | function sk = getSkewness(data) 2 | 3 | %skewness. 4 | %m = getCentralMoments(data); 5 | %s = m(3) / m(2)^(1.5); 6 | [N, col] = size(data); 7 | m = mean(data); 8 | s = std(data); 9 | m = m(ones(N,1),:); 10 | 11 | m3= mean( (data - m).^3 ); 12 | sm2 = sqrt(mean( (data - m).^2 )); 13 | sk = m3/(sm2^3); -------------------------------------------------------------------------------- /matlab/stats/getKurtosis.m: -------------------------------------------------------------------------------- 1 | function k = getKurtosis(data) 2 | 3 | %Kutosis excess. 4 | %m = getCentralMoments(data); 5 | %k = (m(4) / (m(2)^2)) - 3; 6 | [N, col] = size(data); 7 | m = mean(data); 8 | s = std(data); 9 | m = m(ones(N,1),:); 10 | 11 | m4 = mean( (data - m).^4 ); 12 | m2 = mean( (data - m).^2 ); 13 | k = m4/(m2^2)-3; 14 | 15 | -------------------------------------------------------------------------------- /matlab/math/registration/AnisotropicWeighted/e_maker.m: -------------------------------------------------------------------------------- 1 | function e = e_maker(X,Y,W) 2 | W1 = W(:,1,:); W2 = W(:,2,:); W3 = W(:,3,:); 3 | D = Y-X; 4 | D1 = D(1,:); D2 = D(2,:); D3 = D(3,:); 5 | D = reshape([repmat(D1,3,1);repmat(D2,3,1);repmat(D3,3,1)],size(W)); 6 | D1 = D(:,1,:); D2 = D(:,2,:); D3 = D(:,3,:); 7 | e = [W1.*D1 + W2.*D2 + W3.*D3]; 8 | e = e(:); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/eg6.3: -------------------------------------------------------------------------------- 1 | 2.38 -13.39 1.21 2 | 0.18 -24.45 -0.35 3 | 8.17 3.40 2.44 4 | 3.43 -2.76 1.76 5 | 4.43 -3.58 0.58 6 | 0.54 0.49 -2.14 7 | 4.00 -1.03 3.09 8 | -2.24 1.52 2.89 9 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/myfun1.m: -------------------------------------------------------------------------------- 1 | function x=myfun1(fname) 2 | % myfun1.m 2-14-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | s1=['load ' 'c:\mfile\' fname] 9 | eval(s1) 10 | eval(['x = ', fname(1:3) ';']) 11 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/rmsx.m: -------------------------------------------------------------------------------- 1 | function r = rmsx(x) 2 | % rmsx.m 8-06-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % find sqrt{[x(1)^2+x(2)^2+ ... + x(N)^2]/N} 9 | 10 | N=length(x); 11 | r =sqrt(sum(x.^2)/N); 12 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/sf1.m: -------------------------------------------------------------------------------- 1 | function y=sf1(r) 2 | % sf1.m 3-06-94 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % define f(r)= Maxwell Probability Density Function sigma=1 9 | y=sqrt(2/pi)*r.^2 .* exp(-r.^2/2); 10 | -------------------------------------------------------------------------------- /matlab/plotting/plotSphere.m: -------------------------------------------------------------------------------- 1 | function plotSphere(center, radius, facecolor) 2 | if(nargin<3) 3 | facecolor = 'r'; 4 | end 5 | [XX, YY, ZZ] = ellipsoid(center(1),center(2),center(3),radius,radius,radius,50); 6 | s = surface(XX,YY,ZZ); 7 | set(s,'facecolor',facecolor,'edgecolor','none'); 8 | camlight left 9 | lighting gouraud 10 | alpha(0.5); 11 | axis equal; -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/x2dg.m: -------------------------------------------------------------------------------- 1 | function y=x2dg(x,n) 2 | % x2dn.m 1-2-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | if n<2 9 | fac1=1/(2^(n/2)*gamma(n/2+1)); 10 | y=fac1*x.^(n/2).*exp(-x/2); 11 | else 12 | y=0; 13 | end 14 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/ichi2.m: -------------------------------------------------------------------------------- 1 | function x=ichi2(p,N) 2 | % ichi2.m 9-29-97 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | s=['c:\mfile\p2x' num2str(N)]; 9 | s1=['load ' s]; 10 | eval(s1) 11 | x = interp1(pp,xx,p); 12 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cdfchi2.m: -------------------------------------------------------------------------------- 1 | function p=cdfchi2(x,N) 2 | % cdfchi2.m 9-29-97 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | s=['c:\mfile\p2x' num2str(N)]; 9 | s1=['load ' s]; 10 | eval(s1) 11 | p = interp1(xx,pp,x); 12 | -------------------------------------------------------------------------------- /matlab/target_error/int2charXLS.m: -------------------------------------------------------------------------------- 1 | function str = int2charXLS(n) 2 | % returns the char representing column #n in excel-file 3 | 4 | q = floor(n/26); 5 | r = mod(n,26); 6 | 7 | if r == 0 8 | r = 26; 9 | q = q - 1; 10 | end 11 | 12 | str = ''; 13 | if q ~= 0 14 | str = strcat(str,char(64+q)); 15 | end 16 | if r ~= 0 17 | str = strcat(str,char(64+r)); 18 | end -------------------------------------------------------------------------------- /matlab/math/isIsotropic.m: -------------------------------------------------------------------------------- 1 | function [ bIso ] = isIsotropic( Sigma ) 2 | %isIsotropoic Determines if the matrix is isotropic FLE. 3 | % Sigma - FLE, TRE or other covariance matrix. 4 | % bIso - boolean saying if it is isotropic. 5 | 6 | eigv = eig(Sigma); 7 | 8 | if( mean(eigv) == eigv(1) ) 9 | bIso = 1; 10 | else 11 | bIso = 0; 12 | end 13 | 14 | end 15 | 16 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/bf3.m: -------------------------------------------------------------------------------- 1 | function y=bf3(r) 2 | % bf3.m , p, mu are given parameters 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % define G(r)= F(r)-p = {integral of binormal pdf over [0,r]} - p 9 | global p mu 10 | y=quad('bf1',0,r) - p; 11 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/psi.m: -------------------------------------------------------------------------------- 1 | function y=psi(x) 2 | % psi.m 6-21-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | global n 9 | 10 | if x==1 11 | y=n; 12 | else 13 | y=(1-x.^n)./(1-x); % use 1-eps to avoid singularity when integrating 14 | end 15 | -------------------------------------------------------------------------------- /matlab/math/getRotMatrix.m: -------------------------------------------------------------------------------- 1 | function rot = getRotMatrix(euler) 2 | 3 | Rx = euler(1); 4 | Ry = euler(2); 5 | Rz = euler(3); 6 | 7 | % from Craig 1989. 8 | rotX = [ 1 0 0; 0 cos(Rx) -sin(Rx); 0 sin(Rx) cos(Rx)]; % yaw 9 | rotY = [cos(Ry) 0 sin(Ry); 0 1 0; -sin(Ry) 0 cos(Ry)]; % pitch 10 | rotZ = [cos(Rz) -sin(Rz) 0; sin(Rz) cos(Rz) 0; 0 0 1]; % roll 11 | 12 | rot = rotZ*(rotY*rotX); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/simprule.m: -------------------------------------------------------------------------------- 1 | function s=simprule(y,h) 2 | % simprule.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | n= length(y)-1 ; 9 | s0=y(1)+y(n+1); 10 | s1=sum(y(2:2:n)); 11 | s2=sum(y(3:2:n-1)); 12 | s=h*(s0+4*s1+2*s2)/3; 13 | -------------------------------------------------------------------------------- /matlab/target_error/convertPathForSimulink.m: -------------------------------------------------------------------------------- 1 | function data = convertPathForSimulink(filename) 2 | %% convert paths used in FLE testing to time stamp format for Simulink. 3 | load(filename); 4 | 5 | M = length(xfrm); 6 | data = zeros(8,M); % data rows: t, x, y, z, q0, qx, qy, qz 7 | for i = 1:M 8 | t = i/60; 9 | q = rm2quat(xfrm{i}.R); 10 | data(:,i) = [t, xfrm{i}.pos, q]'; 11 | end -------------------------------------------------------------------------------- /matlab/math/getEulerAngles.m: -------------------------------------------------------------------------------- 1 | function euler = getEulerAngles(R) 2 | % R is the rotation matrix. 3 | 4 | Roll = atan2( R(2,1), R(1,1) ); 5 | CosRoll = cos( Roll ); 6 | SinRoll = sin( Roll ); 7 | euler.roll = Roll; 8 | euler.pitch = atan2( -R(3,1), CosRoll * R(1,1) + SinRoll * R(2,1) ); 9 | euler.yaw = atan2( SinRoll * R(1,3)- CosRoll * R(2,3), ... 10 | -SinRoll * R(1,2) + CosRoll * R(2,2) ); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cf1.m: -------------------------------------------------------------------------------- 1 | function y=cf1(r) 2 | % cf1.m 6/04/97, define f(r)= circular normal pdf 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % define f(r)= circular normal pdf 9 | % Rayleigh Probability Density Function sigma=1 10 | y=r.* exp(-r.^2/2); 11 | -------------------------------------------------------------------------------- /matlab/math/registration/AnisotropicWeighted/C_maker.m: -------------------------------------------------------------------------------- 1 | function C = C_maker(X,W) 2 | W1 = W(:,1,:); W2 = W(:,2,:); W3 = W(:,3,:); 3 | X1 = X(1,:); X2 = X(2,:); X3 = X(3,:); 4 | X = reshape([repmat(X1,3,1);repmat(X2,3,1);repmat(X3,3,1)],size(W)); 5 | X1 = X(:,1,:); X2 = X(:,2,:); X3 = X(:,3,:); 6 | C = [-W2.*X3+W3.*X2, +W1.*X3-W3.*X1, -W1.*X2+W2.*X1, W1, W2, W3]; 7 | C = permute(C,[1,3,2]); 8 | C = reshape(C,[],6); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/nf6.m: -------------------------------------------------------------------------------- 1 | function y=nf6(x,mu,sig) 2 | % nf6.m 3-06-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % nf6.m define f(x)=N(mean, variance) 9 | % notice .^ is used for vector x 10 | fac1=1/(sqrt(2*pi)*sig); 11 | y=fac1*exp(-(x-mu).^2/(2*sig^2)); 12 | -------------------------------------------------------------------------------- /matlab/math/screw2rot.m: -------------------------------------------------------------------------------- 1 | function R = screw2rot(screw) 2 | 3 | kx = screw.axis(1); 4 | ky = screw.axis(2); 5 | kz = screw.axis(3); 6 | 7 | cT = cos(screw.angle); 8 | sT = sin(screw.angle); 9 | vT = 1 - cT; 10 | 11 | R = [ (kx*kx*vT + cT) (kx*ky*vT -kz*sT) (kx*kz*vT + ky*sT); ... 12 | (kx*ky *vT + kz*sT) (ky*ky*vT + cT) (ky*kz*vT -kx*sT); ... 13 | (kx*kz*vT - ky*sT) (ky*kz*vT + kx*sT) (kz*kz*vT + cT)]; -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/nf1.m: -------------------------------------------------------------------------------- 1 | function y=nf1(x) 2 | % nf1.m 3-06-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % define f(x)=N(0,1)= Standard Normal Probability Density Function 9 | % notice .^ is used for vector x, sigma=1 10 | fac1=1/sqrt(2*pi); 11 | y=fac1*exp(-x.^2/2); 12 | -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateTTE/readXfrmFile.m: -------------------------------------------------------------------------------- 1 | function xfrm = readXfrmFile(filename,row) 2 | 3 | inXfrms = csvread(filename); 4 | nComponents = size(inXfrms,2); 5 | if (nComponents ~= 7) 6 | error('File does not have 7 columns for the transormation data using quaternions.'); 7 | end 8 | 9 | if(nargin < 2) 10 | row = 1; 11 | end 12 | 13 | xfrm.pos = inXfrms(row, 1:3); 14 | xfrm.rot = inXfrms(row, 4:7); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pf3.m: -------------------------------------------------------------------------------- 1 | function pf3(fname) 2 | % pf3.m 01-25-96 3 | % print figure to an eps file 4 | % 5 | % Spatial Error Analysis ToolBoxc Version 1.0, October 5, 1997 6 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 7 | % dhsu@littongcs.com 8 | % 9 | fname=['e:\temp\hsu_test\' fname '.eps']; 10 | % edit by ADW -- March 27, 2006 for Matlab 6.5 11 | s1=['print -deps ' fname]; 12 | eval(s1) 13 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/gofu.m: -------------------------------------------------------------------------------- 1 | % gofu.m 09-26-97 2 | % g(u), Probability Density Function of the random variable u 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | function y=gofu(u) 9 | global n 10 | 11 | xm=sqrt(2*n-1); 12 | fac1=1/(2^(n-1)*gamma(n/2)); 13 | y=fac1*(u+xm).^(n-1).*exp(-(u+xm).^2/4); 14 | -------------------------------------------------------------------------------- /matlab/stats/qchisq.m: -------------------------------------------------------------------------------- 1 | function x=qchisq(P,n) 2 | % QCHISQ(P,N) - quantile of the chi-square distribution. 3 | if nargin<2 4 | n=1; 5 | end 6 | 7 | s0 = P==0; 8 | s1 = P==1; 9 | s = P>0 & P<1; 10 | x = 0.5*ones(size(P)); 11 | x(s0) = -inf; 12 | x(s1) = inf; 13 | x(~(s0|s1|s))=nan; 14 | 15 | for ii=1:14 16 | dx = -(pchisq(x(s),n)-P(s))./dchisq(x(s),n); 17 | x(s) = x(s)+dx; 18 | if all(abs(dx) < 1e-6) 19 | break; 20 | end 21 | end -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/tdis.m: -------------------------------------------------------------------------------- 1 | function y=tdis(x) 2 | % tdis.m 3-22-94 3 | % 4 | % Spatial Error Analysis ToolBoxc Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % student t - Probability Density Function 9 | % notice .* and .^ are used for vector x: 10 | global n 11 | fac1=gamma((n+1)/2)/gamma(n/2)/sqrt(pi*n) ; 12 | y=fac1*(1+x.^2/n).^(-(n+1)/2); 13 | -------------------------------------------------------------------------------- /matlab/stats/radius3Ddist.m: -------------------------------------------------------------------------------- 1 | function g = radius3Ddist(t, Sigma) 2 | % t - distance locations. 3 | % Sigma - covariance matrix. 4 | 5 | g = zeros(size(t)); 6 | %diag(Sigma) 7 | rho(1) = Sigma(1,2)/(sqrt(Sigma(1,1))*(sqrt(Sigma(2,2)))); 8 | rho(2) = Sigma(2,3)/(sqrt(Sigma(2,2))*(sqrt(Sigma(3,3)))); 9 | rho(3) = Sigma(1,3)/(sqrt(Sigma(1,1))*(sqrt(Sigma(3,3)))); 10 | 11 | for i = 1:length(t) 12 | g(i) = edsvcd(t(i), sqrt(diag(Sigma)), rho); 13 | end -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/nf2.m: -------------------------------------------------------------------------------- 1 | function p=nf2(r) 2 | % nf2.m 3-06-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % given r>=0 , find p=F(r) 9 | % F(r) = integral of N(0,1) over [-r ,r]. (using erf(x) relation) 10 | 11 | if r<0 12 | error('check input argument in nf2(r) ') 13 | end 14 | 15 | p=erf(r/sqrt(2)); 16 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/sf3a.m: -------------------------------------------------------------------------------- 1 | function y=sf3a(r) 2 | % sf3a.m 2-19-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % given p, find G(r)=0 9 | % define G(r)= F(r)-p={integral of trinormal pdf over [0,r]} - p 10 | global p 11 | % y=sf2(r) - p; 12 | fac1=sqrt(2/pi); 13 | y=fac1*(-r.*exp(-r.^2/2))+erf(r/sqrt(2)) - p ; 14 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/x2df.m: -------------------------------------------------------------------------------- 1 | function y=x2df(x) 2 | % x2df.m 12-30-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | global n 9 | 10 | if n<2 11 | fac1=1/(2^(n/2+1)*gamma(n/2+1)); 12 | y=fac1*x.^(n/2).*exp(-x/2); 13 | else 14 | fac1=1/(2^(n/2)*gamma(n/2)); 15 | y=fac1*x.^(n/2-1).*exp(-x/2); 16 | end 17 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/gmrms2n.m: -------------------------------------------------------------------------------- 1 | function n=gmrms2n(gm,rms) 2 | % gmrms2n.m 3-14-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | fid=fopen('c:\mfile\psi.dat','r'); 9 | dat=fscanf(fid,'%g %g',[2,inf]); 10 | fclose(fid); 11 | ratio=gm/rms ; 12 | 13 | n2=dat(1,:); 14 | r2=dat(2,:); 15 | n =interp1(r2,n2,ratio,'spline'); 16 | -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateTTE/runPlotTTEMonteCarlo.m: -------------------------------------------------------------------------------- 1 | function runPlotTTEMonteCarlo(name, test, axisLim) 2 | 3 | close all; 4 | cd(name); 5 | currentcasename = sprintf('%s-%04d', name, test); 6 | load(currentcasename); 7 | if(nargin > 2) 8 | plotTTEMonteCarlo(data,stats,currentcasename, axisLim); 9 | else 10 | plotTTEMonteCarlo(data,stats,currentcasename); 11 | end 12 | copyfile('*.png', '\\San-350\awiles\data\TTE6DImages'); 13 | 14 | cd .. -------------------------------------------------------------------------------- /matlab/math/getLinePlaneIntersection.m: -------------------------------------------------------------------------------- 1 | function p = getLinePlaneIntersection(p1, p2, p0, nrm) 2 | 3 | % if( (size(p1) ~= [1,3]) || (size(p2) ~= [1,3]) || (size(p0) ~= [1,3]) || (size(nrm) ~= [1,3]) ) 4 | % error('inputs are in the wrong dimension'); 5 | % end 6 | 7 | u = (nrm * (p0 - p1)')/(nrm*(p2 - p1)'); 8 | 9 | if( (u < 0) || (u > 1) ) 10 | % only return points between the segment ends. 11 | p = [nan nan nan]; 12 | else 13 | p = p1 + u * (p2 - p1); 14 | end 15 | -------------------------------------------------------------------------------- /matlab/stats/getPercentile.m: -------------------------------------------------------------------------------- 1 | function val = getPercentile(data, p) 2 | 3 | if( (p<0) || (p>1)) 4 | error('Invalid percentile value given. Must be between 0 and 1.'); 5 | end 6 | % assuming the data is a vector. 7 | data = sort(data); 8 | N = length(data); 9 | 10 | kd = p*(N+1); 11 | k = floor(kd); 12 | d = kd - k; 13 | 14 | if(k <=0) 15 | val = data(1); 16 | elseif (k >= N) 17 | val = data(N); 18 | else 19 | val = data(k) + d*(data(k+1) - data(k)); 20 | end 21 | -------------------------------------------------------------------------------- /matlab/target_error/montecarlo/getMeasNoise.m: -------------------------------------------------------------------------------- 1 | function noise = getMeasNoise(fle, N, stream) 2 | %#eml 3 | noise = zeros(N,3); 4 | 5 | if(ismatrix(fle)) 6 | %[V,D] = eig(fle) 7 | [U,D,V] = svd(fle); 8 | noise = (randn(stream, [N, 3]))*(D.^0.5)*V'; 9 | %fprintf('Identical FLE'); 10 | else 11 | for i = 1:N 12 | %[V,D] = eig(fle(:,:,i)); 13 | [U,D,V] = svd(fle(:,:,i)); 14 | noise(i,:) = (randn(stream, [1, 3]))*(D.^0.5)*V'; 15 | end 16 | end -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cln2a.m: -------------------------------------------------------------------------------- 1 | function a=cln2a(C,L,N) 2 | % cln2a.m 9-29-97 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | Ka=L/C; 9 | ta=2*N*Ka^2; 10 | % if statistics toolbox is available 11 | % a=(1-chi2cdf(ta,2*N-1))*100 ; 12 | 13 | % if statistics toolbox is not available, replace chi2cdf with cdfchi2 14 | a=(1-cdfchi2(ta,2*N-1))*100 ; 15 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/ang2p.m: -------------------------------------------------------------------------------- 1 | function p=ang2p(ang,p0) 2 | % ang2p.m 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | 9 | ro=sqrt(-2*log(1-p0)) 10 | d2r=pi/180; 11 | 12 | ang=ang*d2r; 13 | s1=csc(ang/2)/sqrt(2); 14 | s2=sec(ang/2)/sqrt(2); 15 | sx=max(s1,s2) 16 | sy=min(s1,s2); 17 | u=sy/sx 18 | mu=u; 19 | r=ro/sx 20 | p=bf2(r,mu)*100; 21 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/sf2.m: -------------------------------------------------------------------------------- 1 | function y=sf2(r) 2 | % sf2.m 3-06-94 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % find p=F(r) 9 | % where F(r)={integral of Maxwell pdf from 0 to r}, in terms of erf(x) 10 | 11 | if r<0 12 | disp( 'input error ') 13 | disp( ' r>=0 ') 14 | else 15 | fac1=sqrt(2/pi); 16 | y=fac1*(-r.*exp(-r.^2/2))+erf(r/sqrt(2)); 17 | end 18 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/abc2ac.m: -------------------------------------------------------------------------------- 1 | function [a1,c1]=abc2ac(a,b,c) 2 | % abc2ac.m 7-10-97 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % rotation of ellipse 9 | % Ax^2 + Bxy + Cy^2 = 1 ---> au^2 + cv^2 = 1. 10 | 11 | ang=atan2(b,a-c); 12 | t=ang/2; 13 | tdeg=t*180/pi; 14 | 15 | ct=cos(t); 16 | st=sin(t); 17 | 18 | a1=a*ct^2+b*ct*st+c*st^2; 19 | c1=a*st^2-b*ct*st+c*ct^2; 20 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edevud.m: -------------------------------------------------------------------------------- 1 | function p=edevud(abc,sxyz) 2 | % edevud.m 2-21-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Ellipsoidal PDF, uncorrelated, ellipsoidal volume 9 | % abc=[ a, b, c] 10 | % sxyz=[sx,sy,sz] 11 | 12 | if min(abc)<=0 | min(sxyz)<=0 13 | error('check input arguments in edevud.m') 14 | end 15 | 16 | sxyz= sxyz./abc; 17 | p=edsvud(1,sxyz); 18 | -------------------------------------------------------------------------------- /matlab/stats/pchisq.m: -------------------------------------------------------------------------------- 1 | function F=pchisq(x,n) 2 | % PCHISQ(X,N) - Probability function of the chi-square distribution. 3 | if nargin<2 4 | n=1; 5 | end 6 | F=zeros(size(x)); 7 | 8 | if rem(n,2) == 0 9 | s = x>0; 10 | k = 0; 11 | for jj = 0:n/2-1; 12 | k = k + (x(s)/2).^jj/factorial(jj); 13 | end 14 | F(s) = 1-exp(-x(s)/2).*k; 15 | else 16 | for ii=1:numel(x) 17 | if x(ii) > 0 18 | F(ii) = quadl(@dchisq,0,x(ii),1e-6,0,n); 19 | else 20 | F(ii) = 0; 21 | end 22 | end 23 | end -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/an2ka.m: -------------------------------------------------------------------------------- 1 | function Ka=an2ka(a,N) 2 | % an2ka.m 9-29-97 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % given a and N, find Ka 9 | 10 | % if statistics toolbox is available, use the command chi2inv.m 11 | % ta=chi2inv(1-a,2*N-1); 12 | 13 | % if statistics toolbox is not available, to replace chi2inv.m 14 | ta=ichi2(1-a,2*N-1); 15 | 16 | Ka=sqrt(ta/(2*N)); 17 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cf2.m: -------------------------------------------------------------------------------- 1 | function p=cf2(r) 2 | % cf2.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % for 2-dim normal distribution with sigma(x)=sigma(y) 9 | % find p=F(r) 10 | % where F(r) = integral of f(x) from 0 to r. 11 | % Rayleigh pdf used for f(x) here. 12 | % 13 | 14 | if r<0 15 | error('check input argument in cf2(r)') 16 | end 17 | 18 | p=1-exp(-r.^2/2); 19 | -------------------------------------------------------------------------------- /matlab/plotting/plotDistance3DDist.m: -------------------------------------------------------------------------------- 1 | function plotDistance3DDist(S, t, lineformat, linecolor) 2 | 3 | if(nargin < 3) 4 | lineformat = 'r-'; 5 | end 6 | 7 | sxyz = sqrt(diag(S)) 8 | rho(1) = S(1,2)/sqrt(S(1,1)*S(2,2)); 9 | rho(2) = S(2,3)/sqrt(S(2,2)*S(3,3)); 10 | rho(3) = S(1,3)/sqrt(S(1,1)*S(3,3)) 11 | 12 | step = t(2) - t(1); 13 | t = t + (step/2)*ones(size(t)); 14 | g = zeros(size(t)); 15 | for i=1:length(t) 16 | g(i) = edsvcd(t(i),sxyz, rho); 17 | end 18 | plot(t, g, lineformat,'Color', linecolor, 'LineWidth', 2); -------------------------------------------------------------------------------- /matlab/plotting/plotCumHist.m: -------------------------------------------------------------------------------- 1 | function [mu, sigma, rms, t] = plotCumHist(data, t, lineprop) 2 | 3 | N = length(data); 4 | mu = mean(data); 5 | sigma = std(data); 6 | rms = sqrt(mean(data.^2)); 7 | 8 | if( isempty(t)) 9 | minVal = min(data); 10 | maxVal = max(data); 11 | step = 0.01*(maxVal - minVal); 12 | t = minVal:step:maxVal; 13 | 14 | else 15 | step = t(2) - t(1); 16 | end 17 | 18 | d0 = histc(data, t); 19 | d0cum = cumsum(d0); 20 | t = t + (step/2)*ones(size(t)); 21 | 22 | plot(t, (1/N)*d0cum, lineprop); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/sem1.m: -------------------------------------------------------------------------------- 1 | function p=sem1(r,s1,s2,ang) 2 | % sem1.m 4-18-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % from Burt, single ellipse method #1 9 | % ang = cut angle in degrees 10 | 11 | d2r=pi/180 ; 12 | % fictitious sigma and fictitious cut angle 13 | [sf, af, fac]=fsca(s1,s2,ang); 14 | 15 | sa= sf/sqrt(2)/sin(af*d2r/2) ; 16 | sb= sf/sqrt(2)/cos(af*d2r/2) ; 17 | p=edcaud(r,sa,sb); 18 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/sdsvud.m: -------------------------------------------------------------------------------- 1 | function p=sdsvud(R,sig) 2 | % sdsvud.m 2-20-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % for 3-dim normal distribution with sigma(x)=sigma(y)=sigma(z)=sig 9 | % find p=F(r) 10 | % where F(r) = integral of f(x) from 0 to r. 11 | % Maxwell pdf used for f(x) here. 12 | % 13 | if sig<=0 | R<0 14 | error(' check signs of input argument for cdcrud.m') 15 | end 16 | 17 | r=R/sig; 18 | p=sf2(r); 19 | -------------------------------------------------------------------------------- /matlab/stats/checkChi2Test.m: -------------------------------------------------------------------------------- 1 | function result = checkChi2Test(chi2stat, dof) 2 | % assume alpha = 0.05. 3 | if( chi2stat < 0) 4 | error('Invalid chi2 statistic returned.'); 5 | end 6 | 7 | switch(dof) 8 | case 6 9 | if( chi2stat < 12.59) 10 | result = 1; 11 | else 12 | result = 0; 13 | end 14 | case 9 15 | if( chi2stat < 16.92) 16 | result = 1; 17 | else 18 | result = 0; 19 | end 20 | otherwise 21 | error('Invalid dof returned. Check the data.'); 22 | end -------------------------------------------------------------------------------- /matlab/math/getRandomXfrm.m: -------------------------------------------------------------------------------- 1 | function xfrm = getRandomXfrm(s) 2 | % returns the random transform corresponding to the fiducial tracking error 3 | % covariance matrix s. s is a 5x5 or 6x6 matrix. 4 | 5 | if(size(s,1)< 5 || size(s,1) > 6) 6 | error('Invalid size of input matrix to getRandomXfrm given.'); 7 | end 8 | 9 | [V,D] = eig(s); 10 | x = (randn(1, size(s,1)))*(D^0.5)*V'; 11 | xfrm.pos = x(1:3); 12 | if(size(s,1) == 5) 13 | xfrm.rot = [0 x(4:5) 0]; 14 | else 15 | xfrm.rot = [0 x(4:6)]; 16 | end 17 | xfrm.rot(1) = sqrt(1 - xfrm.rot(2)^2 - xfrm.rot(3)^2 - xfrm.rot(4)^2); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cdcaui.m: -------------------------------------------------------------------------------- 1 | function R=cdcaui(p,sig) 2 | % cdcaui.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % given p and sigma, find R such that p = F(R), 9 | % where F(R) = integral of f(x) from 0 to R. 10 | % Rayleigh pdf used for f(x) here. 11 | % 12 | if p>1 | p<0 | sig <=0 13 | error( 'check input arguments in cdcaui.m') 14 | end 15 | 16 | if p==1 17 | R=Inf; 18 | else 19 | R=sig*sqrt(-2*log(1-p)); 20 | end 21 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cf3.m: -------------------------------------------------------------------------------- 1 | function r=cf3(p) 2 | % cf3.m 3-06-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % given p and f(x), find r such that G(r)=F(r)-p = 0, 9 | % where F(r) = integral of f(x) from 0 to r. 10 | % Rayleigh pdf used for f(x) here. 11 | % 12 | if p>1 | p<0 13 | disp( 'input error for cf3(p)') 14 | disp( '0 <= p <= 1') 15 | else 16 | if p==1 17 | r=Inf; 18 | else 19 | r=sqrt(-2*log(1-p)); 20 | end 21 | end 22 | -------------------------------------------------------------------------------- /matlab/stats/getCentralMoments.m: -------------------------------------------------------------------------------- 1 | function m = getCentralMoments(data) 2 | 3 | % for 5 central moments. 4 | mu = mean(data); 5 | N = length(data); 6 | S = zeros(1,5); 7 | 8 | for i = N 9 | d = (data(i) - mu); 10 | S(1) = S(1) + d; 11 | S(2) = S(2) + d^2; 12 | S(3) = S(3) + d^3; 13 | S(4) = S(4) + d^4; 14 | S(5) = S(5) + d^5; 15 | end 16 | 17 | %m(1) = 0; 18 | %m(2) = (S(1)^2/N^2) + S(2)/N; 19 | %m(3) = ((2*S(1)^3)/(N^3)) - (3*S(1)*S(2)/(N^2)) + S(3)/N; 20 | %m(4) = -(3*(S(1)^4)/(N^4)) + ((6*(S(1)^2)*S(2))/(N^3)) - (4*S(1)*S(3))/(N^2) + (S(4))/(N); 21 | m = (1/N) * S; -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cdcaud.m: -------------------------------------------------------------------------------- 1 | function p=cdcaud(R,sig) 2 | % cdcaud.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBoxc Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % r=R/sig 9 | % for 2-dim normal distribution with sigma(x)=sigma(y)=sig 10 | % find p=F(r) 11 | % where F(r) = integral of f(x) from 0 to r. 12 | % Rayleigh pdf used for f(x) here. 13 | % 14 | if sig<=0 | R<0 15 | error(' check signs of input argument for cdcaud.m') 16 | end 17 | 18 | r=R/sig; 19 | p=1-exp(-r.^2/2); 20 | end 21 | -------------------------------------------------------------------------------- /matlab/math/quat2rm.m: -------------------------------------------------------------------------------- 1 | function R = quat2rm(q) 2 | % insert reference here... 3 | %#eml 4 | 5 | w = 1; x = 2; y = 3; z = 4; 6 | % compute the multiplications. 7 | D = q' * q; 8 | 9 | % build the rotation matrix. 10 | R = zeros(3,3); 11 | R(1,1) = D(w,w) + D(x,x) - D(y,y) - D(z,z); 12 | R(1,2) = 2 * (-D(w,z) + D(x,y)); 13 | R(1,3) = 2 * ( D(w,y) + D(x,z)); 14 | R(2,1) = 2 * ( D(w,z) + D(x,y)); 15 | R(2,2) = D(w,w) - D(x,x) + D(y,y) - D(z,z); 16 | R(2,3) = 2 * (-D(w,x) + D(y,z)); 17 | R(3,1) = 2 * (-D(w,y) + D(x,z)); 18 | R(3,2) = 2 * ( D(w,x) + D(y,z)); 19 | R(3,3) = D(w,w) - D(x,x) - D(y,y) + D(z,z); -------------------------------------------------------------------------------- /matlab/target_error/montecarlo/getRandOrientation.m: -------------------------------------------------------------------------------- 1 | function [R, q] = getRandOrientation() 2 | 3 | % generate random components of phi, theta 4 | phi = 2*pi*rand(1,1); 5 | theta = acos(1 - 2*rand(1,1)); 6 | 7 | % vector portion. 8 | nx = sin(theta) * cos(phi); 9 | ny = sin(theta) * sin(phi); 10 | nz = cos(theta); 11 | 12 | % generate random component about the vector. 13 | thetaprime = 2*pi*rand(1,1); 14 | 15 | q0 = cos(thetaprime/2); 16 | qx = sin(thetaprime/2)*nx; 17 | qy = sin(thetaprime/2)*ny; 18 | qz = sin(thetaprime/2)*nz; 19 | 20 | q = [q0, qx, qy, qz]; 21 | 22 | R = quat2rm(q); 23 | 24 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edsvud.m: -------------------------------------------------------------------------------- 1 | function p=edsvud(R,sxyz) 2 | % edsvud.m 2-20-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Ellipsoidal PDF, uncorrelated, spherical volume 9 | % sxyz=[sx,sy,sz] 10 | 11 | if R<0 | min(sxyz) < 0 | sxyz==[0 0 0] 12 | error('check input arguments in edsvud.m') 13 | end 14 | 15 | sx=sxyz(1); 16 | sy=sxyz(2); 17 | sz=sxyz(3); 18 | 19 | sig=sort(sxyz); 20 | r= R/sig(3); 21 | u=sig(2)/sig(3); 22 | v=sig(1)/sig(3); 23 | 24 | p=edsv(r,u,v); 25 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/gf2.m: -------------------------------------------------------------------------------- 1 | function p=gf2(r,mu) 2 | % gf2.m 01-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % mu=min(sx,sy)/max(sx,sy) 9 | % r=R/max(sx,sy) 10 | % 11 | 12 | % r>=0, mu in (0,1) 13 | if r<0 | mu<=0 | mu>=1 14 | error(' check input arguments in gf2(r,mu)') 15 | end 16 | 17 | n=2000; 18 | h=2*pi/n; 19 | fac1=1/(2*pi*mu); 20 | x=[0:n]*h; 21 | gf=(cos(x).^2+sin(x).^2/mu^2); 22 | y=(1-exp(-r^2.*gf/2))./gf; 23 | p=fac1*simprule(y,h); 24 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/pdft.m: -------------------------------------------------------------------------------- 1 | function y=pdft(td,ux,uy,sx,sy) 2 | % pdft.m 11-03-95 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % td in degrees 9 | sx2=sx^2; sy2=sy^2; 10 | d2r=pi/180; 11 | t=td*d2r; 12 | ct=cos(t); 13 | st=sin(t); 14 | ct2=ct^2; 15 | st2=st^2; 16 | A=ct2/sx2+st2/sy2; 17 | B=-2*(ux*ct/sx2+uy*st/sy2); 18 | C=(ux/sx)^2+(uy/sy)^2; 19 | w=B/sqrt(8*A); 20 | w2=w^2; 21 | y=1/(2*pi*sx*sy*A)*exp(-C/2)*(1-w*sqrt(pi)*erfc(w)*exp(w2)); 22 | -------------------------------------------------------------------------------- /matlab/target_error/montecarlo/getQuadraticFLE.m: -------------------------------------------------------------------------------- 1 | function FLE = getQuadraticFLE(pos, rate, model) 2 | 3 | nMrks = size(pos,1); 4 | FLE = zeros(3,3,nMrks); 5 | 6 | for i = 1:nMrks 7 | % check that the point is in the volume. 8 | switch(model) 9 | case 'cartesian' 10 | FLE(:,:,i) = diag([rate(1)*pos(i,1)^2 rate(2)*pos(i,2)^2 rate(3)*pos(i,3)^2]); 11 | case 'radial' 12 | r2 = pos(i,1)^2 + pos(i,2)^2 + pos(i,3)^2; 13 | FLE(:,:,i) = diag([rate(1)*r2 rate(2)*r2 rate(3)*r2]); 14 | otherwise 15 | error('Invalid model type given.'); 16 | end 17 | end -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/nf3.m: -------------------------------------------------------------------------------- 1 | function r=nf3(p) 2 | % nf3.m 3-07-94 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % given p and f(x), find r such that G(r)= F(r)-p = 0, 9 | % where F(r) = integral of f(x) from -r to r, 10 | % using inverse erf(x) relation. 11 | % f(x)= N(0,1) used here. 12 | % 13 | if p>1 | p<0 14 | disp(' input error for nf3(p)') 15 | disp(' 0 <= p <= 1') 16 | else 17 | if p==1 18 | r=Inf; 19 | else 20 | r=sqrt(2)*erfinv(p); 21 | end 22 | end 23 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/r2pc6.m: -------------------------------------------------------------------------------- 1 | function p=r2pc6(gm,rms,r) 2 | % r2pc6.m 6-22-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % when n= 0.9358 and r=14.7497 9 | % gm=5.81, rms=11.53 ---> p=0.8000. 10 | clear global n 11 | global n 12 | 13 | fid=fopen('c:\mfile\psi.dat','r'); 14 | dat=fscanf(fid,'%g %g',[2,inf]); 15 | fclose(fid); 16 | 17 | ratio=gm/rms; 18 | 19 | n2=dat(1,:); 20 | r2=dat(2,:); 21 | 22 | n =interp1(r2,n2,ratio,'spline'); 23 | ms=rms^2; 24 | x=n*r^2/ms; 25 | 26 | p=x2cdf(x) ; 27 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edcaud.m: -------------------------------------------------------------------------------- 1 | function p=edcaud(R,sx,sy) 2 | % edcaud.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Elliptical PDF, Uncorrelated, Circular region 9 | % edcaud.m, Find probability p, given R,sx,sy. 10 | % bf1b.m is the normalized version 11 | % function p=edcaud(R,sx,sy) 12 | % 1>= mu >=0 13 | 14 | if R<0 | sx< 0 | sy< 0 | (sx==0 & sy==0) 15 | error('check input arguments for edcaud(R,sx,sy) ') 16 | end 17 | 18 | mu=min(sx,sy)/max(sx,sy); 19 | r=R/max(sx,sy); 20 | p=edca(r,mu); 21 | -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateTTE/runAllTTEMonteCarloPlots.m: -------------------------------------------------------------------------------- 1 | %% do all plots. 2 | axisLim{1} = [-0.75 0.75 -0.75 0.75 -0.75 0.75]; 3 | axisLim{2} = [-0.75 0.75 -0.75 0.75 -0.75 0.75]; 4 | axisLim{3} = [-0.75 0.75 -0.75 0.75 -0.75 0.75]; 5 | axisLim{4} = [-0.75 0.75 -0.75 0.75 -0.75 0.75]; 6 | axisLim{5} = [-3 3 -3 3 -3 3]; 7 | cd E:\awiles\data\TTEMonteCarlo 8 | for i=1:3 9 | for j=1:5 10 | %casename = sprintf('TTENeedle-tar%02d-xfrm%02d', i, j); 11 | casename = sprintf('TTERadolfzell-tar%02d-xfrm%02d', i, j); 12 | runPlotTTEMonteCarlo(casename,1, axisLim{j}); %run the first test for each case. 13 | end 14 | end -------------------------------------------------------------------------------- /matlab/math/getRotPointQuat.m: -------------------------------------------------------------------------------- 1 | function pos = getRotPointQuat( q, pos0); 2 | %#eml 3 | 4 | % %UcrossV = cross( q(2:4), pos0); 5 | % UcrossV = [ q(3)*pos0(3) - q(4)*pos0(2),... 6 | % q(4)*pos0(1) - q(2)*pos0(3),... 7 | % q(2)*pos0(2) - q(3)*pos0(1) ]; 8 | % 9 | % pos = [ pos0(1) + 2.0 * (q(1)*UcrossV(1) + q(3)*UcrossV(3) - q(4)*UcrossV(2)),... 10 | % pos0(2) + 2.0 * (q(1)*UcrossV(2) + q(4)*UcrossV(1) - q(2)*UcrossV(3)),... 11 | % pos0(3) + 2.0 * (q(1)*UcrossV(3) + q(2)*UcrossV(2) - q(3)*UcrossV(1))]; 12 | 13 | r = [0 pos0]; 14 | rprime = getQuatMultiply(getQuatMultiply(q, r), getConjQuat(q)); 15 | pos = rprime(1,2:4); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/gmrms.m: -------------------------------------------------------------------------------- 1 | function [gm , rms, ratio]=gmrms(fname) 2 | % gmrms.m 6-25-94 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | xo=myfun1(fname) 9 | 10 | [m,n]=size(xo); 11 | if n==1 12 | x=xo ; 13 | end 14 | 15 | if n==2 16 | x=xo(:,1).^2 + xo(:,2).^2 ; 17 | x=sqrt(x); 18 | end 19 | 20 | if n==3 21 | x=xo(:,1).^2 + xo(:,2).^2 + xo(:,3).^2; 22 | x=sqrt(x); 23 | end 24 | 25 | tmp=prod(x); 26 | gm=tmp^(1/m); 27 | 28 | x2=x.^2; 29 | tmp2=sum(x2); 30 | rms=sqrt(tmp2/m); 31 | 32 | ratio=gm/rms; 33 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/tandn.m: -------------------------------------------------------------------------------- 1 | % tandn.m 3-23-96 plot Figure 2.3 2 | % 3 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 4 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 5 | % dhsu@littongcs.com 6 | % 7 | clear global n 8 | global n 9 | n=100; 10 | subplot(211) 11 | fplot('nf1',[-5,5]) 12 | xlabel('x') 13 | ylabel('f(x)') 14 | title('N(0,1) PDF') 15 | axis([-5 , 5, 0, 0.5]) 16 | grid 17 | 18 | n=100; 19 | subplot(212) 20 | fplot('tdis',[-5,5]) 21 | grid 22 | axis([-5 , 5, 0, 0.5]) 23 | sxlabel('\tau') 24 | sylabel('g(\tau{})') 25 | stitle('Student-t_n PDF, n=100') 26 | fixstext 27 | figure(1) 28 | pf3('f23') 29 | -------------------------------------------------------------------------------- /matlab/plotting/drawCube.m: -------------------------------------------------------------------------------- 1 | function drawCube ( origin, size ) 2 | 3 | if(length(size) == 1) 4 | sz = size*ones(1,3); 5 | elseif(length(size) == 3) 6 | sz = size; 7 | else 8 | error('Invalid dimensions for size variable. Length should be 1 (cube) or 3 (rectangular prism).'); 9 | end 10 | x=([0 1 1 0 0 0;1 1 0 0 1 1;1 1 0 0 1 1;0 1 1 0 0 0]-0.5)*sz(1)+origin(1); 11 | y=([0 0 1 1 0 0;0 1 1 0 0 0;0 1 1 0 1 1;0 0 1 1 1 1]-0.5)*sz(2)+origin(2); 12 | z=([0 0 0 0 0 1;0 0 0 0 0 1;1 1 1 1 0 1;1 1 1 1 0 1]-0.5)*sz(3)+origin(3); 13 | for i=1:6 14 | h=patch(x(:,i),y(:,i),z(:,i),'w'); 15 | set(h,'facecolor', [0 .5 .6], 'edgecolor','k', 'FaceAlpha', 0.25) 16 | end -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/bf1.m: -------------------------------------------------------------------------------- 1 | function z=bf1(r) 2 | % bf1.m r>=0, 1-15-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % define f(r)= 2-dim normal pdf, rho=0, sigx and sigy may be unequal. 9 | global mu 10 | 11 | k1= r.^2*(1+mu^2)/(4*mu^2) ; 12 | k2= r.^2*(1-mu^2)/(4*mu^2) ; 13 | z= r/mu.* exp(-k1).*besseli(0,k2) ; 14 | 15 | % 16 | % or use (bf1a.m) 17 | % function y=sigxy(x) 18 | % global mu xlam 19 | % fac1=1/(2*pi*mu)%; 20 | % gf=(cos(x).^2+sin(x).^2/mu^2)%; 21 | % y=fac1*(1-exp(-xlam^2.*gf/2))./gf %; 22 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edraud.m: -------------------------------------------------------------------------------- 1 | function p=edraud(a,b,sx,sy) 2 | % edraud.m 2-07-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % The probability of a 9 | % 2D joint elliptical normal pdf with sx, sy, rho=0 10 | % over a rectangular region: |x| < a, |y| < b. 11 | % 12 | 13 | if sx<0 | sy<0 | a<0 | b<0 | (sx==0 & sy==0) 14 | error('check signs of input argument') 15 | end 16 | 17 | if sx==0 18 | p=nf2(b/sy); 19 | end 20 | 21 | if sy==0 22 | p=nf2(a/sx); 23 | end 24 | 25 | if sx > 0 & sy > 0 26 | p=nf2(a/sx)*nf2(b/sy); 27 | end 28 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/bf2.m: -------------------------------------------------------------------------------- 1 | function y=bf2(r,mu) 2 | % bf2.m 1-15-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Find probability p 9 | % function p=bf2(r,mu) 10 | % where F(r) = integral of f(r,mu) over [0,r] 11 | clear global mu 12 | global mu 13 | 14 | if r<0 | mu>1 | mu<0 15 | disp( ' input error for bf2(r,mu)' ) 16 | disp( ' r >= 0 ' ) 17 | disp( ' 0<= mu <= 1' ) 18 | 19 | elseif mu==0 20 | y=nf2(r); 21 | 22 | elseif mu==1 23 | y=cf2(r); 24 | 25 | else 26 | y=quad('bf1',0,r); 27 | end 28 | -------------------------------------------------------------------------------- /matlab/target_error/montecarlo/getFLEMatrix.m: -------------------------------------------------------------------------------- 1 | function sigma = getFLEMatrix(parms, point) 2 | %#eml 3 | model = parms.modeltype; 4 | switch model 5 | case 1 % radial model. 6 | vec = point - parms.origin; 7 | r = sqrt(vec * vec'); 8 | rms = parms.rate * r + parms.baseline; 9 | rms2 = rms^2; 10 | sigma = weightMatrix(rms2, parms.weight); 11 | case 2 % random FLE. 12 | sigma = diag(rand(1,3) .* parms.range); 13 | case 3 14 | sigma = diag(rand(1,3) .* parms.range); 15 | R = getRandOrientation(); 16 | sigma = R * sigma * R'; 17 | otherwise 18 | error('Not a valid model given.'); 19 | end 20 | 21 | -------------------------------------------------------------------------------- /matlab/math/registration/updateRegistration.m: -------------------------------------------------------------------------------- 1 | function [xfrm, fre] = updateRegistration(refmrk, measmrk, regmethod) 2 | %% Written by Andrew D. Wiles, 2009-12-03. 3 | 4 | nMrks = size(refmrk,1); 5 | 6 | switch regmethod 7 | case 1 8 | % using SVD: 9 | xfrm = getRigidXfrmSVD( refmrk, measmrk ); 10 | xfrmmrk = (xfrm.rot * refmrk')' + repmat(xfrm.pos, nMrks, 1); 11 | case 2 12 | % using Horn: 13 | %xfrm = getRigidXfrm( refmrk, measmrk ); 14 | %xfrmmrk = (quat2rm(xfrm.rot) * refmrk')' + repmat(xfrm.pos, nMrks, 1); 15 | otherwise 16 | error('Invalid registration method specified.'); 17 | end 18 | %compute the FRE. 19 | fre = xfrmmrk - measmrk; -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/anl2b.m: -------------------------------------------------------------------------------- 1 | function b=anl2b(a,N,lamb) 2 | % anl2b.m 9-29-97 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % given a, N and lambda, find b 9 | 10 | % if statistics toolbox is available 11 | % ta=chi2inv(1-a,2*N-1); 12 | 13 | % if statistics toolbox is not available, replace chi2inv with ichi2 14 | ta=ichi2(1-a,2*N-1); 15 | 16 | Ka=sqrt(ta./(2*N)) 17 | tb=ta/lamb^2; 18 | 19 | % if statistics toolbox is available 20 | % b=chi2cdf(tb,2*N-1); 21 | 22 | % if statistics toolbox is not available, replace chi2cdf with cdfchi2 23 | b=cdfchi2(tb,2*N-1); 24 | 25 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/newton.m: -------------------------------------------------------------------------------- 1 | function x=newton(xo,fun,dfun,tol) 2 | % newton.m 1-1-97 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % xo is initial guess 9 | % fun and dfun are string names 10 | 11 | % example: 12 | % fun1 = sf3a; 13 | % dfun1 = sf1 ; 14 | % x = newton(xo,'sf3a','sf1',0.0001); 15 | 16 | global p 17 | 18 | eval(['x = xo - ', fun,'(xo)/', dfun,'(xo);']); 19 | d=abs(x-xo); 20 | N=1; 21 | 22 | while d>tol 23 | xo=x ; 24 | eval(['x = xo - ', fun,'(xo)/', dfun,'(xo);']); 25 | N=N+1; 26 | d=abs(x-xo); 27 | end 28 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edbvud.m: -------------------------------------------------------------------------------- 1 | function p=edbvud(abc,sxyz) 2 | % edbvud.m 2-20-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % The probability of a 9 | % 3D joint ellipsoidal normal pdf with (sx, sy, sz), rho=0 10 | % over a box volume: |x| < a, |y| < b, |z| < c. 11 | % abc=[ a, b, c] 12 | % sxyz=[sx,sy,sz] 13 | % 14 | 15 | a=abc(1); 16 | b=abc(2); 17 | c=abc(3); 18 | 19 | sx=sxyz(1); 20 | sy=sxyz(2); 21 | sz=sxyz(3); 22 | 23 | if sx<=0 | sy<=0 | sz<=0 | a<0 | b<0 | c <0 24 | error('check signs of input argument') 25 | end 26 | 27 | p=nf2(a/sx)*nf2(b/sy)*nf2(c/sz); 28 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edcaci.m: -------------------------------------------------------------------------------- 1 | function R=edcaci(p,sx,sy,rho) 2 | % edcaci.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Elliptical PDF, correlated, Circular region 9 | 10 | if p<0 | p>1 | sx <0 | sy<0 | abs(rho)>1 | (sx==0 & sy==0) 11 | error('check input arguments in edcacd.m') 12 | end 13 | 14 | 15 | R=1; 16 | p0=edcacd(R,sx,sy,rho) 17 | %%%%%%%%%%%%%%%%%%%%%%% 18 | for j=1:2:5 19 | while (p0 > p) 20 | R=R-(0.1)^j; 21 | p0=edcacd(R,sx,sy,rho) 22 | end 23 | 24 | while (p0 < p) 25 | R=R+(0.1)^j; 26 | p0=edcacd(R,sx,sy,rho) 27 | end 28 | end 29 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edcaui.m: -------------------------------------------------------------------------------- 1 | function R=edcaui(p,a,b) 2 | % edcaui.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Elliptical PDF, Uncorrelated, Circular region 9 | % function R=edcaui(p,a,b) 10 | 11 | if p<0 | p>1 | a< 0 | b< 0 | (a==0 & b==0) 12 | error('check signs of inputs for edcaui(p,a,b) ') 13 | end 14 | 15 | R=1; 16 | p0=edcaud(R,a,b); 17 | %%%%%%%%%%%%%%%%%%%%%% 18 | for j=1:2:5 19 | while (p0 > p) 20 | R=R-(0.1)^j; 21 | p0=edcaud(R,a,b); 22 | end 23 | 24 | while (p0 < p) 25 | R=R+(0.1)^j; 26 | p0=edcaud(R,a,b); 27 | end 28 | end 29 | -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateTTE/readFTECovariance.m: -------------------------------------------------------------------------------- 1 | function fte = readFTECovariance(filename,row) 2 | 3 | inCov = csvread(filename); 4 | nComponents = size(inCov,2); 5 | if (nComponents == 21) 6 | %6D 7 | dim = 6; 8 | fte = zeros(6); 9 | elseif( size(inCov,2) == 15 ) 10 | %5D 11 | dim = 5; 12 | fte = zeros(5); 13 | else 14 | error('The width of the covariance file %s is not correct.', filename); 15 | end 16 | 17 | if(nargin < 2) 18 | row = 1; 19 | end 20 | 21 | nCnt = 1; 22 | for i = 1:dim 23 | j = 1; 24 | while(j <= i) 25 | fte(i,j) = inCov(row,nCnt); 26 | fte(j,i) = fte(i,j); 27 | nCnt = nCnt+1; 28 | j= j+1; 29 | end 30 | end 31 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/xandn.m: -------------------------------------------------------------------------------- 1 | % xandn.m 9-26-97 plot Figure 2.6 2 | % 3 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 4 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 5 | % dhsu@littongcs.com 6 | % 7 | clear global n 8 | global n 9 | clf 10 | 11 | subplot(211) 12 | fplot('nf1',[-5,5]) 13 | sylabel('f(x)') 14 | sxlabel('x') 15 | stitle(['f(x)=N(0,1) is defined for all real u']) 16 | axis([-5 , 5, 0, 0.5]) 17 | grid 18 | 19 | n=100; 20 | umin = -sqrt(2*n-1) 21 | subplot(212) 22 | fplot('gofu',[-5,5]) 23 | sylabel('g(u)') 24 | sxlabel('u') 25 | stitle(['g(u) is defined for u \geq ' num2str(umin), ', when n=100']) 26 | axis([-5,5, 0, 0.5]) 27 | grid 28 | figure(1) 29 | pf3('f26') 30 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cln2b.m: -------------------------------------------------------------------------------- 1 | function b=cln2b(C,L,N,lamb) 2 | % cln2b.m 10-03-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | Ka=L/C; 9 | ta=2*N*Ka^2; 10 | % if statistics toolbox is available 11 | % a=(1-chi2cdf(ta,2*N-1))*100; 12 | 13 | % if statistics toolbox is not available, replace chi2cdf with cdfchi2 14 | a=(1-cdfchi2(ta,2*N-1))*100; 15 | 16 | tb=2*N*(Ka./lamb).^2; 17 | % if statistics toolbox is available 18 | % b=chi2cdf(tb,2*N-1)*100 ; 19 | 20 | % if statistics toolbox is not available, replace chi2cdf with cdfchi2 21 | b=cdfchi2(tb,2*N-1)*100 ; 22 | 23 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/fsca.m: -------------------------------------------------------------------------------- 1 | function [sf, af, fac]=fsca(sa,sb,ang) 2 | % fsca.m 3-11-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % p. 1219 Bowditch, Fig. 6qi 9 | % Find fictitious sigma, sigma factor, and fictitious cut angle 10 | % ang, af in degrees 11 | 12 | d2r=pi/180; 13 | u=min(sa,sb)/max(sa,sb); 14 | 15 | b=atan(u); 16 | sf=sin(2*b).*sqrt(sa^2+sb^2)/sqrt(2); 17 | af=asin(sin(2*b)*sin(ang*d2r))/d2r; 18 | fac = sf/max(sa,sb); 19 | 20 | disp([' fictitious sigma = ' num2str( sf )]) 21 | disp([' fictitious cut angle = ' num2str(af) ' degrees'] ) 22 | disp([' sigma factor = ' num2str(fac)] ) 23 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/bf4.m: -------------------------------------------------------------------------------- 1 | function r=bf4(p,mu) 2 | % bf4.m 2-14-94 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % find r for G(r)=F(r)-p=0 9 | 10 | %clear global p mu 11 | %global p mu 12 | 13 | if p>1 | p<0 | mu>1 | mu < 0 14 | disp( ' input error for bf4(p,mu) ') 15 | disp( ' 0 <= p <= 1 ') 16 | disp( ' 0 <= mu <= 1 ') 17 | 18 | elseif mu==1 19 | r=cf3(p); 20 | 21 | elseif mu==0 22 | r=nf3(p); 23 | 24 | else 25 | if p==1 26 | r=Inf; 27 | else 28 | t1=clock; 29 | % r=fsolve('bf3',1); 30 | r=newton(1,'bf3','bf1',0.0001); 31 | time_used=etime(clock,t1) 32 | end 33 | end 34 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/psi.dat: -------------------------------------------------------------------------------- 1 | 0.1000 0.0002 2 | 0.1250 0.0011 3 | 0.1500 0.0037 4 | 0.1750 0.0089 5 | 0.2000 0.0172 6 | 0.3000 0.0772 7 | 0.4000 0.1588 8 | 0.5000 0.2416 9 | 0.6000 0.3169 10 | 0.7000 0.3827 11 | 0.8000 0.4393 12 | 0.9000 0.4880 13 | 1.0000 0.5298 14 | 2.0000 0.7493 15 | 3.0000 0.8315 16 | 4.0000 0.8736 17 | 5.0000 0.8989 18 | 6.0000 0.9158 19 | 7.0000 0.9279 20 | 8.0000 0.9370 21 | 9.0000 0.9440 22 | 10.0000 0.9497 23 | 20.0000 0.9749 24 | 30.0000 0.9833 25 | 40.0000 0.9875 26 | 50.0000 0.9900 27 | 60.0000 0.9917 28 | 70.0000 0.9928 29 | 80.0000 0.9937 30 | 90.0000 0.9944 31 | 100.0000 0.9950 32 | -------------------------------------------------------------------------------- /matlab/math/getPA.m: -------------------------------------------------------------------------------- 1 | function [V,D] = getPA(x0) 2 | 3 | N = size(x0,1); 4 | %%demean the markers. 5 | x = x0 - repmat(mean(x0),N,1); 6 | 7 | %get the moment of inertia tensor. 8 | I0 = getmoi(x); % see function below 9 | 10 | % transform the markers into principal axes. 11 | [V D] = eig(I0); 12 | %x = (V'*x')'; 13 | 14 | function I0 = getmoi(x) 15 | 16 | %find the inertia tensor. 17 | I0 = zeros(3,3); 18 | 19 | for i = 1:3 20 | for j=1:3 21 | if (i==j) 22 | for k=1:size(x,1) 23 | I0(i,j) = I0(i,j) + (sum(x(k,:).^2) - x(k,i)^2); 24 | end 25 | else 26 | for k=1:size(x,1) 27 | I0(i,j) = I0(i,j) - x(k,i)*x(k,j); 28 | end 29 | end 30 | end 31 | end -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/tf3.m: -------------------------------------------------------------------------------- 1 | function r=tf3(p,u,v) 2 | % tf3.m, 2-20-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % solve for r from F(r,u,v) - p=0, 9 | % sig(x)~=sig(y)~=sig(z) 10 | 11 | if p>1 | p<0 | u<0 | v<0 | u>1 | v>1 | v>u 12 | error( 'input error for tf3(p)') 13 | end 14 | 15 | if p==1 16 | r=Inf; 17 | else 18 | r=1; 19 | p0=tf2(r,u,v); 20 | %%%%%%%%% searching for a, b %%%%%%%%%%%%%%%% 21 | for j=1:2:5 22 | while (p0 > p) 23 | r=r-(0.1)^j; 24 | p0=tf2(r,u,v); 25 | end 26 | 27 | while (p0 < p) 28 | r=r+(0.1)^j; 29 | p0=tf2(r,u,v); 30 | end 31 | end 32 | end 33 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/sf4.m: -------------------------------------------------------------------------------- 1 | function r=sf4(p) 2 | % sf4.m 2-19-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % solve for r in G(r)=0 9 | % where G(r)= F(r)-p = {integral of trinormal pdf over [0,r]} - p 10 | clear global p 11 | global p 12 | if p>1 | p<0 13 | disp( ' error in input ') 14 | disp( ' 0 <= p <= 1 ') 15 | else 16 | if p==1 17 | r=Inf; 18 | elseif p==0 19 | r=0; 20 | % elseif p==0.025 | p==0.05 21 | elseif p<=0.05 & p>0 22 | % r=fsolve('sf3a',0.5); 23 | r=newton(0.5,'sf3a','sf1',0.0001); 24 | else 25 | % r=fsolve('sf3a',1); 26 | r=newton(1,'sf3a','sf1',0.0001); 27 | end 28 | end 29 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edsvui.m: -------------------------------------------------------------------------------- 1 | function R=edsvui(p,sxyz) 2 | % edsvui.m 2-20-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Ellipsoidal PDF, uncorrelated, spherical volume 9 | % sxyz=[sx,sy,sz] 10 | 11 | disp(' Executing ... Patience ') 12 | if p<0 | p>1 | min(sxyz) < 0 | sxyz==[0 0 0] 13 | error('check input arguments in edsvui.m') 14 | end 15 | 16 | R=mean(sxyz)*sf3(p); 17 | p0 = edsvud(R,sxyz); 18 | %%%%%%%%% searching for a, b %%%%%%%%%%%%%%%% 19 | for j=1:1:5 20 | while (p0 > p) 21 | R=R-(0.1)^j ; 22 | p0 = edsvud(R,sxyz); 23 | end 24 | 25 | while (p0 < p) 26 | R=R+(0.1)^j ; 27 | p0 = edsvud(R,sxyz); 28 | end 29 | end 30 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edeaud.m: -------------------------------------------------------------------------------- 1 | function p=edeaud(a,b,sx,sy) 2 | % edeaud.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % edea= elliptical normal pdf over elliptical area, 9 | % elliptical normal pdf, x and y are uncorrelated. 10 | % 1/(2*pi*sx*sy) exp[(-x^2/sx^2 - y^2/sy^2)/2] 11 | % elliptical area of integration 12 | % x^2/a^2 + y^2/b^2 = 1 13 | % 14 | % example 15 | % a=0.6, b=1.2, sx=.4, sy=1.0. 16 | % p=edeaud(0.6, 1.2, 0.4, 1.0) 17 | % 18 | 19 | if sx< 0 | sy< 0 | a<=0 | b<=0 | (sx==0 & sy==0) 20 | error('check input arguments in edeaud.m') 21 | end 22 | 23 | s1=sx/a; 24 | s2=sy/b; 25 | mu=min(s1,s2)/max(s1,s2) ; 26 | p=edcaud(1,s1,s2); 27 | -------------------------------------------------------------------------------- /matlab/math/TransformRigidBody.m: -------------------------------------------------------------------------------- 1 | function [xfrm, xfrmPos, rmsError] = TransformRigidBody(refPos, measPos); 2 | 3 | nRefMarkers = size(refPos, 1); 4 | nMeasMarkers = size(measPos, 1); 5 | if( nRefMarkers ~= nMeasMarkers) 6 | error('getRigidXfrm: Number of reference markers is not equal to the number of measured markers.\n'); 7 | end 8 | 9 | %% compute the transform first. 10 | xfrm = getRigidXfrm( refPos, measPos ); 11 | 12 | %% transform each of the reference positions into measured space and 13 | %% compute the errors. 14 | rmsError = 0; 15 | nMarkers = 0; 16 | for i = 1:size(refPos,1) 17 | xfrmPos(i,:) = getXfrmPointQuat(xfrm, refPos(i,:)); 18 | rmsError = rmsError + sum((xfrmPos(i,:) - measPos(i,:)).^2); 19 | nMarkers = nMarkers + 1; 20 | end 21 | 22 | rmsError = sqrt(rmsError/nMarkers); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/sf3.m: -------------------------------------------------------------------------------- 1 | function r=sf3(p) 2 | % sf3.m 2-19-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % given p and f(x), find r such that F(r)-p = 0, 9 | % where F(r) = integral of f(x) from 0 to r. 10 | % Maxwell pdf used for f(x) here. 11 | % 12 | if p>1 | p<0 13 | disp( '0 <= p <= 1') 14 | error( 'input error for sf3(p)') 15 | end 16 | 17 | if p==1 18 | r=Inf; 19 | else 20 | r=1; 21 | p0=sf2(r); 22 | %%%%%%%%% searching for a, b %%%%%%%%%%%%%%%% 23 | for j=1:2:5 24 | while (p0 > p) 25 | r=r-(0.1)^j ; 26 | p0=sf2(r); 27 | end 28 | 29 | while (p0 < p) 30 | r=r+(0.1)^j ; 31 | p0=sf2(r); 32 | end 33 | end 34 | end 35 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edcacd.m: -------------------------------------------------------------------------------- 1 | function p=edcacd(R,sx,sy,rho) 2 | % edcacd.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Elliptical PDF, correlated, Circular region 9 | 10 | if R<0 | sx <0 | sy<0 | abs(rho)>1 | (sx==0 & sy==0) 11 | error('check input arguments in edcacd.m') 12 | end 13 | 14 | if abs(rho)==1 15 | st=sqrt(sx^2+sy^2); 16 | p=nf2(R/st); 17 | end 18 | 19 | if abs(rho)==0 20 | p=edcaud(R,sx,sy); 21 | end 22 | 23 | if abs(rho)<1 & abs(rho) >0 24 | A= 1/(1-rho^2)/sx^2 ; 25 | B= -2*rho/(1-rho^2)/(sx*sy) ; 26 | C= 1/(1-rho^2)/sy^2 ; 27 | [a1, c1]=abc2ac(A,B,C); 28 | s1=1/sqrt(a1); 29 | s2=1/sqrt(c1); 30 | p=edcaud(R,s1,s2); 31 | end 32 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edraui.m: -------------------------------------------------------------------------------- 1 | function [a,b]=edraui(p,v,sx,sy) 2 | % edraui.m 2-07-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Given the probability sx, sy, v=a/b, p 9 | % Find a, b 10 | % 11 | % 2D joint elliptical normal pdf with sx, sy, rho=0 12 | % over a rectangular region: -a < x < a , -b < y < b. 13 | % 14 | 15 | b=max(sx,sy); 16 | a=v*b; 17 | p0=edraud(a,b,sx,sy); 18 | 19 | %%%%%%%%% searching for a, b %%%%%%%%%%%%%%%% 20 | for j=1:2:5 21 | while (p0 > p) 22 | b=b-(0.1)^j ; 23 | a=v*b ; 24 | p0=edraud(a,b,sx,sy); 25 | end 26 | 27 | while (p0 < p) 28 | b=b+(0.1)^j ; 29 | a=v*b ; 30 | p0=edraud(a,b,sx,sy); 31 | end 32 | end 33 | -------------------------------------------------------------------------------- /matlab/math/getRelativeMeasPoints.m: -------------------------------------------------------------------------------- 1 | function measPos = getRelativeMeasPoints(data, rotId, refId, toolId) 2 | % returns the measured points in the frame of reference given by refId. 3 | 4 | measPos = []; 5 | for i = 1:size(data,1) 6 | j = rotId; 7 | for k = 1:size(data,3) 8 | pos = []; 9 | for m = 1:size(data{i,j,k,refId}.rawxfrm.pos,1) 10 | refXfrm.pos = data{i,j,k,refId}.rawxfrm.pos(m,:); 11 | refXfrm.rot = data{i,j,k,refId}.rawxfrm.quat(m,:); 12 | toolXfrm.pos = data{i,j,k,toolId}.rawxfrm.pos(m,:); 13 | toolXfrm.rot = data{i,j,k,toolId}.rawxfrm.quat(m,:); 14 | refXfrm = getInvXfrmQuat(refXfrm); 15 | xfrm = getCombXfrm(toolXfrm, refXfrm); 16 | pos = [pos; xfrm.pos]; 17 | end 18 | measPos = [measPos; mean(pos)]; 19 | end 20 | end -------------------------------------------------------------------------------- /matlab/math/registration/getFRE.m: -------------------------------------------------------------------------------- 1 | function [rms, freVect] = getFRE(X,Y,xfrm, varargin) 2 | 3 | if (size(X) ~= size(Y)) 4 | error('X and Y must be the same size\n'); 5 | end 6 | 7 | verbose = 0; 8 | 9 | if( nargin > 3 ) 10 | nVarArgs = length(varargin); 11 | i = 1; 12 | while( i <= nVarArgs ) 13 | if( strcmp(varargin{i}, 'verbose') || strcmp(varargin{i}, 'Verbose')) 14 | verbose = 1; 15 | else 16 | warning('Unknown parameter: %s -- Ignoring it.', varargin{i}) 17 | end 18 | i=i+1; 19 | end 20 | end 21 | 22 | Xprime = zeros(size(X)); 23 | 24 | for i = 1:length(X) 25 | Xprime(i,:) = getXfrmPointQuat(xfrm, X(i,:)); 26 | end 27 | 28 | freVect = Y - Xprime; 29 | rms = sqrt(mean(sum(freVect.^2,2))); 30 | 31 | if(verbose) 32 | fprintf('Registration FRE: %f\n', rms); 33 | end -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edsvci.m: -------------------------------------------------------------------------------- 1 | function R=edsvci(p,sxyz,rho) 2 | % edsvci.m 2-27-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Ellipsoidal PDF, correlated, spherical volume 9 | % sxyz=[sx,sy,sz] 10 | % rho=[rxy,ryz,rxz] 11 | 12 | if p<0 | p>1 | min(sxyz) < 0 | sxyz==[0 0 0] | min(rho)<-1 | max(rho)>1 13 | error('check input arguments in edsvci.m') 14 | end 15 | 16 | disp('Executing ... Patience') 17 | 18 | R=mean(sxyz)*sf3(p); 19 | p0 = edsvcd(R,sxyz,rho) ; 20 | %%%%%%%%% searching for a, b %%%%%%%%%%%%%%%% 21 | for j=1:1:5 22 | while (p0 > p) 23 | R=R-(0.1)^j ; 24 | p0 = edsvcd(R,sxyz,rho) ; 25 | end 26 | 27 | while (p0 < p) 28 | R=R+(0.1)^j ; 29 | p0 = edsvcd(R,sxyz,rho) ; 30 | end 31 | end 32 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/psi1.m: -------------------------------------------------------------------------------- 1 | % psi1.m 3-12-97 2 | % 3 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 4 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 5 | % dhsu@littongcs.com 6 | % 7 | % GM/RMS vs N 8 | % part I 9 | clf 10 | eu=.5772156649; 11 | 12 | x=[(.1:0.025:.2) (0.3:.1:.9) (1:9) (10:10:100)] ; 13 | Nx = length(x); 14 | 15 | for j=1:Nx 16 | n2=x(j); m=n2/2; 17 | y=psinh(n2); 18 | % y=psin(m); 19 | z(j)=exp(y/2)/sqrt(m); 20 | end 21 | 22 | DAT=[x' z'] 23 | save c:\mfile\psi.mat x z 24 | 25 | fname='c:\mfile\psi.dat' 26 | fid=fopen(fname,'wt') 27 | fprintf(fid,'%8.4f %8.4f\n',DAT') 28 | fclose(fid) 29 | %%%%%%%%%%%%%%%%%%%%%%%%%% 30 | % part II 31 | load c:\mfile\psi.mat 32 | 33 | semilogx(x,z, x,z,'o') 34 | axis('square') 35 | xlabel('n') 36 | ylabel('GM/RMS') 37 | % title(' GM/RMS vs. n ') 38 | grid 39 | figure(1) 40 | pf3('f61') 41 | -------------------------------------------------------------------------------- /matlab/target_error/montecarlo/computeStats.m: -------------------------------------------------------------------------------- 1 | function testcase = computeStats(testcase) 2 | 3 | %get summary statistics. 4 | testcase.Meas.stats.mu = mean(testcase.Meas.error); 5 | testcase.Meas.stats.Sigma = cov(testcase.Meas.error); 6 | testcase.Meas.stats.RMS = sqrt(mean(sum(testcase.Meas.error.^2,2))); 7 | testcase.Meas.stats.max = max(testcase.Meas.error); 8 | testcase.Meas.stats.min = min(testcase.Meas.error); 9 | testcase.Meas.stats.absmax = max(abs(testcase.Meas.error)); 10 | testcase.Meas.stats.skewness.x = getSkewness(testcase.Meas.tip(:,1)); 11 | testcase.Meas.stats.skewness.y = getSkewness(testcase.Meas.tip(:,2)); 12 | testcase.Meas.stats.skewness.z = getSkewness(testcase.Meas.tip(:,3)); 13 | testcase.Meas.stats.kurtosis.x = getKurtosis(testcase.Meas.tip(:,1)); 14 | testcase.Meas.stats.kurtosis.y = getKurtosis(testcase.Meas.tip(:,2)); 15 | testcase.Meas.stats.kurtosis.z = getKurtosis(testcase.Meas.tip(:,3)); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edeaui.m: -------------------------------------------------------------------------------- 1 | function [a,b]=edeaui(p,v,sx,sy) 2 | % edeaui.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % elliptical normal pdf, x and y are uncorrelated 9 | % 1/(2*pi*sx*sy) exp[(-x^2/sx^2 - y^2/sy^2)/2] 10 | % elliptical area of integration 11 | % x^2/a^2 + y^2/b^2 = 1 12 | % 13 | % v=a/b 14 | % 15 | 16 | if sx< 0 | sy< 0 | v <0 | (sx==0 & sy==0) 17 | error('check input arguments in edeaui.m') 18 | end 19 | 20 | b=1; 21 | a=v*b; 22 | p0=edeaud(a,b,sx,sy); 23 | 24 | %%%%%%%%% searching for a, b %%%%%%%%%%%%%%%% 25 | for j=1:2:5 26 | while (p0 > p) 27 | b=b-(0.1)^j; 28 | a=v*b; 29 | p0=edeaud(a,b,sx,sy); 30 | end 31 | 32 | while (p0 < p) 33 | b=b+(0.1)^j; 34 | a=v*b; 35 | p0=edeaud(a,b,sx,sy); 36 | end 37 | end 38 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edsv.m: -------------------------------------------------------------------------------- 1 | function p=edsv(r,u,v) 2 | % edsv.m 2-20-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Ellipsoidal PDF, uncorrelated, spherical volume 9 | % u >= v 10 | 11 | if r<0 | u < 0 | v > 1 | u < v 12 | error('check input arguments in edsv.m') 13 | end 14 | 15 | mlim =0.01; 16 | 17 | if u0 % n2 is less than 1 19 | y=quad8('psi',0,1-eps)-eu-1/n; % 1-eps to avoid singularity 20 | end 21 | % odd n2: 1,3,5,...,99 22 | if n2==m*2+1 % n2 is odd 23 | k=1:m ; s=1./(2*k-1); 24 | y=-eu - 2*log(2) + 2*sum(s) ; 25 | end 26 | % even n2: 2,4,6,...,100 27 | if n2==m*2 % n2 is odd 28 | k=1:m-1; s=1./k; % n2 is even 29 | y=-eu + sum(s) ; 30 | end 31 | clear global n 32 | % z=GM/RMS 33 | % z=exp(y/2)/sqrt(n); 34 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edbvui.m: -------------------------------------------------------------------------------- 1 | function [a,b,c]=edbvui(p,k1,k2,sxyz) 2 | % edbvui.m 2-28-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Given the probability sx, sy, sz, k1=a/b, k2=b/c, p 9 | % Find a, b 10 | % 11 | % 3D joint elliptical normal pdf with sx, sy, sz, rho=0 12 | % over a rectangular region: -a < x < a , -b < y < b, -c < z < c. 13 | % 14 | 15 | b=max(sxyz); 16 | a=k1*b; 17 | c=b/k2; 18 | abc=[a b c]; 19 | p0=edbvud(abc,sxyz); 20 | 21 | %%%%%%%%% searching for a, b ,c %%%%%%%%%%%%%%%% 22 | for j=1:1:5 23 | while (p0 > p) 24 | b=b-(0.1)^j ; 25 | a=k1*b ; 26 | c=b/k2 ; 27 | abc=[a b c]; 28 | p0=edbvud(abc,sxyz); 29 | end 30 | 31 | while (p0 < p) 32 | b=b+(0.1)^j ; 33 | a=k1*b ; 34 | c=b/k2 ; 35 | abc=[a b c]; 36 | p0=edbvud(abc,sxyz); 37 | end 38 | end 39 | -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateFLERegisterRealData/readFromXLS.m: -------------------------------------------------------------------------------- 1 | function data = readFromXLS(filename) 2 | 3 | % get number of lines in the file 4 | fid = fopen(filename,'rt'); 5 | nLines = 0; 6 | while (fgets(fid) ~= -1), 7 | nLines = nLines + 1; 8 | end 9 | fclose(fid); 10 | 11 | % get number of tools 12 | nTools = xlsread(filename,'A2:A2'); 13 | 14 | % initialization 15 | data = cell(1,nTools); % data contains cells, each cell represents a tool 16 | curPos = 1; 17 | 18 | % parsing the data 19 | for i = 1:nTools 20 | curPos = curPos + 5; 21 | range = strcat(int2charXLS(curPos),'2:',int2charXLS(curPos),'2'); 22 | nMarkers = xlsread(filename,range); 23 | data{i} = cell(1,nMarkers); 24 | for j = 1:nMarkers 25 | curPos = curPos + 2; 26 | range = strcat(int2charXLS(curPos),'2:',int2charXLS(curPos+2),int2str(nLines)); 27 | data{i}{j} = xlsread(filename,range); 28 | curPos = curPos + 2; 29 | end 30 | end 31 | -------------------------------------------------------------------------------- /matlab/target_error/misc/TREWest.m: -------------------------------------------------------------------------------- 1 | % TREWest.m 2 | 3 | % polaris active case 3 on pg. 541(9) of West. 4 | A = 70; 5 | B = 40; 6 | rho = 170; 7 | r = 63.6; 8 | d = 400; 9 | 10 | % probe marker and tip positions 11 | xp = [-A/2 -B/2 0; -A/2 B/2 0; A/2 B/2 0; A/2 -B/2 0]; 12 | tp = [rho 0 0]; 13 | 14 | % coordinate reference frame 15 | ang = deg2rad(45); 16 | xc = [r*cos(ang) r*sin(ang) 0; -r*cos(ang) r*sin(ang) 0;... 17 | -r*cos(ang) -r*sin(ang) 0; r*cos(ang) -r*sin(ang) 0]; 18 | tc = [d 0 0]; 19 | 20 | % create a rotation matrix. 21 | R = getRotMatrix(0, 55, 0); 22 | 23 | xp = (R*xp')'; 24 | tp = (R*tp')'; 25 | 26 | FLE = 0.33; 27 | %FLE = [0.0099 0.0099 0.0891]; %variances on the diagonal. 28 | %stdev = sqrt(FLE^2/3); 29 | 30 | TREp = calcTRE_NDI(FLE, [xp;tp],1); 31 | fprintf('Probe TRE = %4.2f\n', TREp); 32 | 33 | TREc = calcTRE_NDI(FLE, [xc;tc],2); 34 | fprintf('CRF TRE = %4.2f\n', TREc); 35 | 36 | TREt = sqrt(TREp^2 + TREc^2); 37 | fprintf('Combined TRE = %4.2f\n', TREt); 38 | 39 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/sem2.m: -------------------------------------------------------------------------------- 1 | function p=sem2(r,sa,sb,ang) 2 | % sem2.m 4-18-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Example from Burt method 2 9 | % given r, s1, s2 and cut angle find probability 10 | % p=P(r,s1,s2,cut_angle) 11 | 12 | d2r=pi/180; 13 | sa2=sa^2; 14 | sb2=sb^2; 15 | f=sin(ang*d2r)^2; 16 | 17 | sx2=(sa2+sb2+sqrt((sa2+sb2)^2-4*f*sa2*sb2))/(2*f); 18 | sy2=(sa2+sb2-sqrt((sa2+sb2)^2-4*f*sa2*sb2))/(2*f); 19 | 20 | sx=sqrt(sx2) ; 21 | sy=sqrt(sy2) ; 22 | mu=min(sx,sy)/max(sx,sy) ; 23 | k=r/max(sx,sy); 24 | p=bf2(k,mu) ; 25 | clc 26 | disp(['P(R)= ' num2str(p*100) '%, for R= ' num2str(r)]) 27 | disp(['R=r*sigma(x)= ' num2str(k) ' times sigma(x)' ]) 28 | disp(['sigma(x)= ' num2str(max(sx,sy)) ]) 29 | disp(['sigma(y)= ' num2str(min(sx,sy)) ]) 30 | disp(['u= ' num2str(mu) ]) 31 | -------------------------------------------------------------------------------- /matlab/math/solveellipse.m: -------------------------------------------------------------------------------- 1 | % 2 | % Given an ellipse in the form: 3 | % a(1)x^2 + a(2)xy + a(3)y^2 + a(4)x + a(5)y + a(6) = 0 4 | % finds the standard form: 5 | % ((x-cx)/r1)^2 + ((y-cy)/r2)^2 = 1 6 | % returning [r1 r2 cx cy theta] 7 | % 8 | function v = solveellipse(a) 9 | 10 | % get ellipse orientation 11 | theta = atan2(a(2),a(1)-a(3))/2; 12 | 13 | % get scaled major/minor axes 14 | ct = cos(theta); 15 | st = sin(theta); 16 | ap = a(1)*ct*ct + a(2)*ct*st + a(3)*st*st; 17 | cp = a(1)*st*st - a(2)*ct*st + a(3)*ct*ct; 18 | 19 | % get translations 20 | T = [[a(1) a(2)/2]' [a(2)/2 a(3)]']; 21 | t = -inv(2*T)*[a(4) a(5)]'; 22 | cx = t(1); 23 | cy = t(2); 24 | 25 | % get scale factor 26 | val = t'*T*t; 27 | scale = 1 / (val- a(6)); 28 | 29 | % get major/minor axis radii 30 | r1 = 1/sqrt(scale*ap); 31 | r2 = 1/sqrt(scale*cp); 32 | v = [r1 r2 cx cy theta]'; -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/nf7.m: -------------------------------------------------------------------------------- 1 | %nf7.m 2 | % nf7.m 3-06-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | function nf7(p) 9 | clf 10 | 11 | load c:\mfile\eg3p2.mat 12 | 13 | mu=mean(z) 14 | sig=std(z) 15 | 16 | xx1=floor(mu-4*sig); 17 | xx2=ceil(mu+4*sig); 18 | d=(xx2-xx1)/100; 19 | x=[xx1:d:xx2]; 20 | y=nf6(x,mu,sig); 21 | plot(x,y) 22 | hold 23 | 24 | r=nf3(p) 25 | a=sig*(-r)+mu; 26 | b=sig*(r)+mu; 27 | 28 | m=10; 29 | dx=(b-a)/m; 30 | 31 | for k=1:m-1 32 | x=a+k*dx; 33 | y=nf6(x,mu,sig); 34 | plot([x,x],[0,y],':') 35 | end 36 | ya=nf6(a,mu,sig); 37 | yb=nf6(b,mu,sig); 38 | plot([a,a],[0,ya],'--') 39 | plot([b,b],[0,yb],'--') 40 | 41 | 42 | xlabel(' z ') 43 | ylabel(' g(z) ') 44 | t=['Area under Normal PDF g(z), for z in [' num2str(a) ',' num2str(b) '] is ' num2str(p) ]; 45 | title(t) 46 | figure(1) 47 | pf3('f34') 48 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/nf2a.m: -------------------------------------------------------------------------------- 1 | function p=nf2a(r) 2 | % nf2a.m 5-4-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % given r and f(x), find p 9 | % p = F(r) = integral of f(x) from -r to r 10 | % f(x) is 1-dim normal distribution N(0,1) with graph 11 | 12 | clf 13 | 14 | disp([' area under pdf between ' num2str(-r) ' and ' num2str(r)]) 15 | 16 | p=nf2(r); 17 | 18 | b=abs(r) ; a=-abs(r); 19 | 20 | fplot('nf1',[-4,4]) 21 | hold 22 | 23 | m=10; 24 | dx=(b-a)/m; 25 | 26 | for k=1:m-1 27 | x=a+k*dx; 28 | y=nf1(x); 29 | plot([x,x],[0,y],':') 30 | end 31 | ya=nf1(a); 32 | yb=nf1(b); 33 | plot([a,a],[0,ya],'--') 34 | plot([b,b],[0,yb],'--') 35 | 36 | xlabel(' x') 37 | ylabel(' f(x) ') 38 | t=['Area under Normal PDF f(x), for x in [' num2str(-r) ',',... 39 | num2str(r) '] is ' num2str(p,4) ]; 40 | title(t) 41 | figure(1) 42 | pf3('f11') % or pf3('f33') 43 | -------------------------------------------------------------------------------- /matlab/target_error/montecarlo/generateRandProbeXfrm.m: -------------------------------------------------------------------------------- 1 | function probe = generateRandProbeXfrm(probe, maxAngle) 2 | 3 | bValid = 0; 4 | 5 | while(~bValid) 6 | % assume we get a valid orientation. 7 | bValid = 1; 8 | 9 | % define the transformation with a random orientation. 10 | xfrm.pos = [0 0 -1000]; 11 | [xfrm.R, q] = getRandOrientation(); 12 | 13 | 14 | % transform rigid body into test space. 15 | probe.Actual.mrk = (xfrm.R * probe.Rigid.mrk')' + repmat(xfrm.pos, 4, 1); 16 | probe.Actual.normals = (xfrm.R * probe.Rigid.normals')'; 17 | probe.Actual.tip = (xfrm.R * probe.Rigid.tip')' + xfrm.pos; 18 | 19 | % check normals. 20 | for i = 1:size(probe.Actual.normals,1) 21 | ang = acosd(probe.Actual.normals(i,:)* [0 0 1]'); 22 | if( (ang > maxAngle) || (ang < 0)) 23 | %testResult = sprintf('Marker Normals Out of Range, theta = %3.2f',ang); 24 | bValid = 0; 25 | break; 26 | end 27 | end 28 | end 29 | 30 | probe.xfrm = xfrm; 31 | 32 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/psin.m: -------------------------------------------------------------------------------- 1 | function y=psin(x) 2 | % psin.m 3-14-97 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % evaluate psi(x) 9 | % psin(x)=psinh(2x) 10 | % for x=integers, integer+0.5, and 0=1 24 | k=1:n-1 ; s=1./k; 25 | y= -eu + sum(s) ; 26 | % n: integer+0.5 % n is integer+0.5 27 | elseif n-floor(n)==1/2 & n>1 28 | m=(2*n-1)/2; 29 | k=1:m ; s=1./(2*k-1); 30 | y= -eu -2*log(2) + 2*sum(s) ; 31 | else 32 | x=n-1; 33 | y=psin(x)+ 1/x; 34 | end 35 | end 36 | 37 | clear global n 38 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/chi2.m: -------------------------------------------------------------------------------- 1 | % chi2.m 1-16-96 2 | % 3 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 4 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 5 | % dhsu@littongcs.com 6 | % 7 | % R(p)/RMS=r vs GM/RMS=z 8 | 9 | clear; clf; 10 | 11 | % result from psi1.m 12 | load c:\mfile\psi.dat 13 | z=psi(:,2); 14 | 15 | % result from chi1.m 16 | load c:\mfile\chi.dat 17 | r=chi(:,2:5); 18 | 19 | DAT=[z r] 20 | save c:\mfile\psichi.mat z r 21 | 22 | 23 | fname='c:\mfile\psichi.dat' 24 | fid=fopen(fname,'wt') 25 | fprintf(fid,'%8.4f%8.4f%8.4f%8.4f%8.4f\n',DAT') 26 | fclose(fid) 27 | 28 | plot(z,r) 29 | grid 30 | 31 | adj = [0 0 0.06 0.02]; 32 | for kk=1:4 33 | if kk==1 p=0.5 ; 34 | elseif kk==2 p=0.8 ; 35 | elseif kk==3 p=0.9 ; 36 | elseif kk==4 p=0.95; 37 | end 38 | 39 | text(0.2,r(7,kk)+adj(kk),['p=' num2str(100*p) '%']) 40 | end 41 | 42 | xlabel(' x=GM/RMS ') 43 | ylabel(' y=R(p)/RMS ') 44 | % title(' R(p)/RMS vs z=GM/RMS, for p=0.5, 0.8, 0.9, 0.95') 45 | axis('square') 46 | figure(1) 47 | pause 48 | pf3('f63') 49 | -------------------------------------------------------------------------------- /matlab/target_error/misc/stochTest.m: -------------------------------------------------------------------------------- 1 | % script to transform random data... eventually to be turned into a 2 | % function. 3 | % Given, mean and covariance matrix. 4 | 5 | N = 1000; 6 | 7 | mu = [0 0 0]'; 8 | Sigma = [0.1444 0 -0.1419; 9 | 0 0.0911 0; 10 | -0.1419 0 0.1642]; 11 | 12 | [V, D] = eig(Sigma); 13 | 14 | unitPoints = randn(3,N); 15 | circPoints = getCirclePoints( 2, N ); 16 | 17 | %transform into new space. 18 | randPoints = V * (D^.5) * unitPoints + repmat(mu, 1, N); 19 | 20 | figure(1); 21 | plotNDI(unitPoints', 'scatter'); 22 | 23 | subplot(2,2,1); 24 | hold on; 25 | plot( circPoints(1,:), circPoints(2,:), 'k'); 26 | axis equal; 27 | hold off; 28 | 29 | subplot(2,2,3); 30 | hold on; 31 | plot( circPoints(1,:), circPoints(2,:), 'k'); 32 | axis equal; 33 | hold off; 34 | 35 | subplot(2,2,1); 36 | hold on; 37 | plot( circPoints(1,:), circPoints(2,:), 'k'); 38 | axis equal; 39 | hold off; 40 | 41 | 42 | plotStochastic( randPoints, mu, Sigma ); 43 | return; 44 | figure(2); 45 | plotNDI(randPoints', 'scatter'); 46 | 47 | -------------------------------------------------------------------------------- /matlab/plotting/getOrderedPlotColor.m: -------------------------------------------------------------------------------- 1 | function [ color ] = getOrderedPlotColor( num ) 2 | %getOrderedPlotColor Return a unique color for each item. 3 | % For each number we return an appropriate color for the plots. 4 | % 1 - 'Blue' 5 | % 2 - 'Green' 6 | % 3 - 'Red' 7 | % 4 - 'Orange' 8 | % 5 - 'Purple' 9 | % 6 - 'Gray' 10 | % 7 - 'DeepSkyBlue' 11 | % 8 - 'Black' 12 | % 9 - 'DeepPink' 13 | % 10 - 'Gold' 14 | 15 | switch(num) 16 | case 1 17 | color = rgb('Blue'); 18 | case 2 19 | color = rgb('Green'); 20 | case 3 21 | color = rgb('Red'); 22 | case 4 23 | color = rgb('Orange'); 24 | case 5 25 | color = rgb('Purple'); 26 | case 6 27 | color = rgb('Gray'); 28 | case 7 29 | color = rgb('DeepSkyBlue'); 30 | case 8 31 | color = rgb('Black'); 32 | case 9 33 | color = rgb('DeepPink'); 34 | case 10 35 | color = rgb('Gold'); 36 | otherwise 37 | warning('Number exceeded colors, returning blue'); 38 | color = rgb('Blue'); 39 | end 40 | 41 | 42 | end 43 | 44 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2r1d.m: -------------------------------------------------------------------------------- 1 | function p2r1d(p) 2 | % p2r1d.m 5-04-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % 1-dim normal distribution 9 | % given p and f(x), find r such that F(r)-p = 0, 10 | % express r in terms of SIGMA, MAE, LEP 11 | % where F(r) = integral of f(x) from -r to r. 12 | % standard noraml distribution N(0,1) used for f(x) here. 13 | % 14 | r=nf3(p); 15 | 16 | disp(' ') 17 | disp(' ') 18 | disp([' For p=', num2str(p) ]) 19 | ic=menu('options:', 'k*SIGMA', 'k*MAE', 'k*LEP', 'all the above'); 20 | 21 | if ic==1 22 | disp(['r=', num2str(r), ' * SIGMA']) 23 | end 24 | 25 | if ic==2 26 | r=r*sqrt(pi/2); 27 | disp(['r=', num2str(r), ' * MAE']) 28 | end 29 | 30 | if ic==3 31 | r=r/nf3(0.5); 32 | disp(['r=', num2str(r), ' * LEP']) 33 | end 34 | 35 | if ic==4 36 | disp(['r=', num2str(r), ' * SIGMA']) 37 | 38 | r=r*sqrt(pi/2); 39 | disp(['r=', num2str(r), ' * MAE']) 40 | 41 | r=nf3(p)/nf3(0.5); 42 | disp(['r=', num2str(r), ' * LEP']) 43 | end 44 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edsvcd.m: -------------------------------------------------------------------------------- 1 | function p=edsvcd(R,sxyz,rho) 2 | % edsvcd.m 12-26-98 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Ellipsoidal PDF, correlated, spherical volume 9 | % sxyz=[sx, sy, sz] 10 | % rho =[r12, r23, r13] 11 | 12 | if R<0 | min(sxyz)<=0 | abs(max(rho)) > 1 13 | error('check input arguments in edsvcd.m') 14 | end 15 | 16 | sx=sxyz(1); 17 | sy=sxyz(2); 18 | sz=sxyz(3); 19 | 20 | r12=rho(1); 21 | r23=rho(2); 22 | r13=rho(3); 23 | 24 | f1=1/(1+2*r12*r13*r23-r12^2-r13^2-r23^2); 25 | 26 | if rho==[0 0 0] 27 | p=edsvud(R,[sx,sy,sz]); 28 | else 29 | A= f1*(1-r23^2)/sx^2 ; 30 | B= f1*(1-r13^2)/sy^2 ; 31 | C= f1*(1-r12^2)/sz^2 ; 32 | 33 | q12=-(r12-r13*r23)*f1/(sx*sy); 34 | q13=-(r13-r23*r12)*f1/(sx*sz); 35 | q23=-(r23-r12*r13)*f1/(sy*sz); 36 | 37 | Q=[A q12 q13; q12 B q23; q13 q23 C]; 38 | 39 | [V,D]=eig(Q); 40 | s1=1/sqrt(D(1,1)); 41 | s2=1/sqrt(D(2,2)); 42 | s3=1/sqrt(D(3,3)); 43 | p=edsvud(R,[s1,s2,s3]); 44 | end 45 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edevci.m: -------------------------------------------------------------------------------- 1 | function [a,b,c]=edevci(p,k1,k2,sxyz,rho) 2 | %edevci.m 2-20-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved 6 | % dhsu@littongcs.com 7 | % 8 | % ellipsoidal normal pdf, x,y,z are correlated 9 | % ellipsoidal volume of integration 10 | % x^2/a^2 + y^2/b^2 + z^2/c^2 = 1 11 | % 12 | % k1=a/b, k2=b/c 13 | % 14 | sx=sxyz(1); 15 | sy=sxyz(2); 16 | sz=sxyz(3); 17 | z3=[0 0 0] 18 | 19 | if sx< 0|sy< 0|sz <0|k1 <0|k2 <0|sxyz==z3|min(rho)<-1|max(rho)>1 20 | error('check input arguments in edevci.m') 21 | end 22 | 23 | disp('Executing ... Patience') 24 | 25 | b=sf3(p); 26 | a=k1*b; 27 | c=b/k2; 28 | abc=[a b c]; 29 | p0=edevcd(abc,sxyz,rho) 30 | 31 | %%%%%%%%% searching for a, b %%%%%%%%%%%%%%%% 32 | for j=1:1:5 33 | while (p0 > p) 34 | b=b-(0.1)^j; 35 | a=k1*b; 36 | c=b/k2 ; 37 | abc=[a b c]; 38 | p0=edevcd(abc,sxyz,rho) 39 | end 40 | 41 | while (p0 < p) 42 | b=b+(0.1)^j; 43 | a=k1*b ; 44 | c=b/k2; 45 | abc=[a b c]; 46 | p0=edevcd(abc,sxyz,rho) 47 | end 48 | end 49 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edevui.m: -------------------------------------------------------------------------------- 1 | function [a,b,c]=edevui(p,k1,k2,sxyz) 2 | %edevui.m 2-20-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % ellipsoidal normal pdf, x,y,z are uncorrelated 9 | % ellipsoidal volume of integration 10 | % x^2/a^2 + y^2/b^2 + z^2/c^2 = 1 11 | % 12 | % k1=a/b, k2=b/c 13 | % 14 | sx=sxyz(1); 15 | sy=sxyz(2); 16 | sz=sxyz(3); 17 | 18 | if sx< 0 | sy< 0 | sz <0 | k1 <0 | k2 <0 |sxyz==[0 0 0] 19 | error('check input arguments in edevui.m') 20 | end 21 | 22 | disp('Executing ... Patience') 23 | 24 | b=sf3(p)*mean(sxyz); 25 | a=k1*b; 26 | c=b/k2; 27 | abc=[a b c] ; 28 | p0=edevud(abc,sxyz) ; 29 | 30 | %%%%%%%%% searching for a, b %%%%%%%%%%%%%%%% 31 | for j=1:1:5 32 | while (p0 > p) 33 | b=b-(0.1)^j; 34 | a=k1*b; 35 | c=b/k2 ; 36 | abc=[a b c] ; 37 | p0=edevud(abc,sxyz) ; 38 | end 39 | 40 | while (p0 < p) 41 | b=b+(0.1)^j; 42 | a=k1*b ; 43 | c=b/k2; 44 | abc=[a b c] ; 45 | p0=edevud(abc,sxyz) ; 46 | end 47 | end 48 | -------------------------------------------------------------------------------- /matlab/math/getSphereFit.m: -------------------------------------------------------------------------------- 1 | function [p,r] = getSphereFit(X) 2 | % this will fit a sphere to a point cloud. 3 | 4 | [p1, r1] = getApproxSphereFit(X); 5 | 6 | n = 0; 7 | J = zeros(size(X,1), 4); 8 | while(n < 100) 9 | for i=1:size(X,1) 10 | rmeas(i) = sqrt(sum((X(i,:) - p1).^2)); 11 | d(i) = rmeas(i) - r1; 12 | %compute the Jacobian 13 | J(i,1) = -(X(i,1) - p1(1,1))/rmeas(i); 14 | J(i,2) = -(X(i,2) - p1(1,2))/rmeas(i); 15 | J(i,3) = -(X(i,3) - p1(1,3))/rmeas(i); 16 | J(i,4) = -1; 17 | end 18 | 19 | %solve for the system paramters. 20 | u = inv(J'*J)*J' * -d'; 21 | p1 = (p1' + u(1:3,1))'; 22 | r1 = r1 + u(4,1); 23 | n=n+1; 24 | end 25 | 26 | p = p1; 27 | r = r1; 28 | 29 | function [p,r] = getApproxSphereFit(X) 30 | 31 | A = zeros(size(X,1), 4); 32 | b = zeros(size(X,1), 1); 33 | 34 | for i = 1:size(X,1) 35 | A(i,1) = 2*X(i,1); 36 | A(i,2) = 2*X(i,2); 37 | A(i,3) = 2*X(i,3); 38 | A(i,4) = -1; 39 | b(i,1) = X(i,1)^2 + X(i,2)^2 + X(i,3)^2; 40 | end 41 | 42 | u = inv(A'*A)*A'*b; 43 | p = u(1:3,1)'; 44 | r = sqrt( u(1,1)^2 + u(2,1)^2 + u(3,1)^2 -u(4,1)); 45 | 46 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/noval.m: -------------------------------------------------------------------------------- 1 | function p=noval(r) 2 | % noval.m 3-11-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % multiple ellipses all axes parallel 9 | % r=input ('r= (20 in p.32 Burt) ') ; 10 | load c:\mfile\nsd.dat 11 | 12 | x=nsd(:,1) 13 | y=nsd(:,2) 14 | 15 | sa=sqrt(sum(x.*x)) 16 | sb=sqrt(sum(y.*y)) 17 | 18 | ang=90; 19 | % ang=input ('cut angle= (90 in p.32 Burt) ') ; 20 | 21 | disp(' ') 22 | d2r=pi/180; 23 | sa2=sa^2; 24 | sb2=sb^2; 25 | f=sin(ang*d2r)^2; 26 | 27 | sx2=(sa2+sb2+sqrt((sa2+sb2)^2-4*f*sa2*sb2))/(2*f); 28 | sy2=(sa2+sb2-sqrt((sa2+sb2)^2-4*f*sa2*sb2))/(2*f); 29 | 30 | sx=sqrt(sx2) ; 31 | sy=sqrt(sy2) ; 32 | mu=min(sx,sy)/max(sx,sy) ; 33 | k=r/max(sx,sy); 34 | p=bf2(k,mu) ; 35 | clc 36 | disp(['p(r)= ' num2str(p*100) '%, for r= ' num2str(r)]) 37 | disp(['r= ' num2str(k) ' times sigma(x)' ]) 38 | disp(['sigma(x)= ' num2str(max(sx,sy)) ]) 39 | disp(['sigma(y)= ' num2str(min(sx,sy)) ]) 40 | disp(['u= ' num2str(mu) ]) 41 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2rc6.m: -------------------------------------------------------------------------------- 1 | function R=p2rc6(gm,rms,p) 2 | % p2rc6.m 1-2-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % when n= 0.9358 9 | % gm=5.8054, rms=11.5296, p=0.5 ---> r=7.542 10 | clear global n 11 | global n 12 | 13 | fid=fopen('c:\mfile\psi.dat','r'); 14 | dat=fscanf(fid,'%g %g',[2,inf]); 15 | fclose(fid); 16 | 17 | ratio=gm/rms; 18 | n2=dat(1,:); 19 | r2=dat(2,:); 20 | n =interp1(r2,n2,ratio,'spline'); 21 | 22 | %%%%%% use fsolve.m (from optimization toolbox) %%%%%%% 23 | % global p 24 | % x=fsolve('x2disc2',1) ; 25 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 26 | 27 | %%%%%% iteration %%%%%%%%%%%% 28 | if p==1 29 | x=Inf; 30 | else 31 | x=1; % initial guess 32 | p0= x2cdf(x); 33 | for j=1:2:5 34 | while (p0 > p) 35 | x=x-(0.1)^j; 36 | p0= x2cdf(x); 37 | end 38 | 39 | while (p0 < p) 40 | x=x+(0.1)^j; 41 | p0= x2cdf(x); 42 | end 43 | end 44 | end 45 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%% 46 | 47 | disp([' n = ', num2str(n)]) 48 | R=rms*sqrt(x/n); 49 | -------------------------------------------------------------------------------- /matlab/math/getQuatMultiply.m: -------------------------------------------------------------------------------- 1 | function qq = getQuatMultiply( qL, qR ) 2 | % multiply two quaternions together (see Shoemake). 3 | %% TODO: 4 | % note NDI uses a different version which I think reduces the number of 5 | % multiplcations (12 vs. 16). 6 | %#eml 7 | 8 | w = 1; x = 2; y = 3; z = 4; 9 | 10 | %% multiply the quats. 11 | qq = zeros(1,4); 12 | qq(w) = qL(w)*qR(w) - qL(x)*qR(x) - qL(y)*qR(y) - qL(z)*qR(z); 13 | qq(x) = qL(w)*qR(x) + qL(x)*qR(w) + qL(y)*qR(z) - qL(z)*qR(y); 14 | qq(y) = qL(w)*qR(y) + qL(y)*qR(w) + qL(z)*qR(x) - qL(x)*qR(z); 15 | qq(z) = qL(w)*qR(z) + qL(z)*qR(w) + qL(x)*qR(y) - qL(y)*qR(x); 16 | 17 | %% second method 18 | 19 | fA = (qL(w) + qL(x)) * (qR(w) + qR(x)); 20 | fB = (qL(z) - qL(y)) * (qR(y) - qR(z)); 21 | fC = (qL(x) - qL(w)) * (qR(y) + qR(z)); 22 | fD = (qL(y) + qL(z)) * (qR(x) - qR(w)); 23 | fE = (qL(x) + qL(z)) * (qR(x) + qR(y)); 24 | fF = (qL(x) - qL(z)) * (qR(x) - qR(y)); 25 | fG = (qL(w) + qL(y)) * (qR(w) - qR(z)); 26 | fH = (qL(w) - qL(y)) * (qR(w) + qR(z)); 27 | 28 | qq(w) = fB + (-fE - fF + fG + fH) / 2.0; 29 | qq(x) = fA - ( fE + fF + fG + fH) / 2.0; 30 | qq(y) = -fC + ( fE - fF + fG - fH) / 2.0; 31 | qq(z) = -fD + ( fE - fF - fG + fH) / 2.0; -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edeaci.m: -------------------------------------------------------------------------------- 1 | function [a,b]=edeaci(p,v,sx,sy,rho) 2 | % edeaci.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % function [a,b]=edeaci(p,v,sx,sy,rho) 9 | % edeac= elliptical normal pdf over elliptical area, x,y are correlated 10 | % elliptical normal pdf 11 | % 1/[2*pi*sx*sy*sqrt(1-rho^2)] 12 | % exp{-[x^2/sx^2-2*rho*x*y/(sx*sy)+y^2/sy^2]/(2(1-rho^2)} 13 | % elliptical area of integration 14 | % x^2/a^2 + y^2/b^2 = 1 15 | % 16 | % v=a/b 17 | % 18 | % example 19 | % p=0.7; v=0.5; sx=.4; sy=1; rho=0.3 20 | % [a,b]=edeaci(0.7, 0.5, 0.4, 1, 0.3) 21 | % 22 | 23 | if abs(rho)>1 | v<=0 | sx<=0 | sy<=0 24 | error('check input arguments in edeacd.m ') 25 | end 26 | 27 | 28 | b=1; 29 | a=v*b; 30 | p0=edeacd(a,b,sx,sy,rho); 31 | 32 | %%%%%%%%% searching for a, b %%%%%%%%%%%%%%%% 33 | for j=1:2:5 34 | while (p0 > p) 35 | b=b-(0.1)^j; 36 | a=v*b; 37 | p0=edeacd(a,b,sx,sy,rho); 38 | end 39 | 40 | while (p0 < p) 41 | b=b+(0.1)^j; 42 | a=v*b; 43 | p0=edeacd(a,b,sx,sy,rho); 44 | end 45 | end 46 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edevcd.m: -------------------------------------------------------------------------------- 1 | function p=edevcd(abc,sxyz,rho) 2 | % edevcd.m 12-26-98 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % Ellipsoidal PDF, correlated, ellipsoidal volume 9 | % abc =[a, b, c] 10 | % sxyz=[sx, sy, sz] 11 | % rho =[r12, r23, r13] 12 | 13 | if min(abc)<=0 | min(sxyz)<=0 | max(abs(rho)) > 1 14 | error('check input arguments in edevcd.m') 15 | end 16 | 17 | if rho== [0 0 0] 18 | p=edevud(abc,sxyz); 19 | else 20 | 21 | a=abc(1); 22 | b=abc(2); 23 | c=abc(3); 24 | 25 | sx=sxyz(1)/a; 26 | sy=sxyz(2)/b; 27 | sz=sxyz(3)/c; 28 | 29 | r12=rho(1); 30 | r23=rho(2); 31 | r13=rho(3); 32 | 33 | f1=1/(1+2*r12*r13*r23-r12^2-r13^2-r23^2); 34 | 35 | A= f1*(1-r23^2)/sx^2 ; 36 | B= f1*(1-r13^2)/sy^2 ; 37 | C= f1*(1-r12^2)/sz^2 ; 38 | 39 | q12=-(r12-r13*r23)*f1/(sx*sy); 40 | q13=-(r13-r23*r12)*f1/(sx*sz); 41 | q23=-(r23-r12*r13)*f1/(sy*sz); 42 | 43 | Q=[A q12 q13; q12 B q23; q13 q23 C]; 44 | 45 | [V,D]=eig(Q); 46 | s1=1/sqrt(D(1,1)); 47 | s2=1/sqrt(D(2,2)); 48 | s3=1/sqrt(D(3,3)); 49 | p=edsvud(1,[s1,s2,s3]); 50 | 51 | end 52 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2r3d.m: -------------------------------------------------------------------------------- 1 | function p2r3d(p) 2 | % p2r3d.m 2-26-94 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % r=r3ng(p) 9 | % given p and f(x), find r such that F(r)-p = 0, 10 | % express r in terms of SIGMA, MSRE, SEP, RMSR 11 | % where F(r) = integral of f(x) from 0 to r. 12 | % Maxwell pdf used for f(x) here. 13 | % 14 | % 15 | 16 | disp(' ') 17 | disp(' ') 18 | disp([' For p=', num2str(p) ]) 19 | ic=menu('options:', 'k*SIGMA', 'k*MRE', 'k*RMSR','k*SEP','all 4 above'); 20 | r=sf4(p); 21 | 22 | if ic==1 23 | disp(['R=', num2str(r), ' * SIGMA']) 24 | end 25 | 26 | if ic==2 27 | r=r/sqrt(8/pi); 28 | disp(['R=', num2str(r), ' * MRE']) 29 | end 30 | 31 | if ic==3 32 | r=r/sqrt(3); 33 | disp(['R=', num2str(r), ' * RMSR']) 34 | end 35 | 36 | if ic==4 37 | r=r/sf4(0.5); 38 | disp(['R=', num2str(r), ' * SEP']) 39 | end 40 | 41 | if ic==5 42 | disp(['R=', num2str(r), ' * SIGMA']) 43 | 44 | r1=r/sqrt(8/pi); 45 | disp(['R=', num2str(r1), ' * MSRE']) 46 | 47 | r2=r/sqrt(3); 48 | disp(['R=', num2str(r2), ' * RMSR']) 49 | 50 | r3=r/sf4(0.5); 51 | disp(['R=', num2str(r3), ' * SEP']) 52 | end 53 | -------------------------------------------------------------------------------- /matlab/math/registration/getRigidXfrmSVD.m: -------------------------------------------------------------------------------- 1 | function xfrm = getRigidXfrmSVD( refPos, measPos ) 2 | % computes the rigid body transform for the meas positions. 3 | % based on the Schonemann's algorithm: Schonemann PH. "A generalized 4 | % solution of the orthogonal Procrustes problem". Psychometrika, vol. 31, 5 | % pp 1-10, 1966. (Summarized in Fitzpatrick et al. TMI 17(5): 694-702, 1998). 6 | %#eml 7 | nRefMarkers = size(refPos, 1); 8 | nMeasMarkers = size(measPos, 1); 9 | if( nRefMarkers ~= nMeasMarkers) 10 | error('getRigidXfrm: Number of reference markers is not equal to the number of measured markers.\n'); 11 | end 12 | 13 | %% find the mean positions for both the reference and the measured. 14 | avgRefPos = mean(refPos); 15 | avgMeasPos = mean(measPos); 16 | 17 | %% "demean" the collection of points. 18 | dmRefPos = refPos - repmat(avgRefPos, size(refPos,1), 1); 19 | dmMeasPos = measPos - repmat(avgMeasPos, size(measPos,1), 1); 20 | 21 | %% the SVD method. 22 | % compute matrix YTX 23 | YTX = dmMeasPos' * dmRefPos; 24 | % get the SVD. 25 | [A,D,B] = svd(YTX); 26 | % compute rotation matrix. 27 | R = B * diag([1,1,det(B*A)])* A'; 28 | % get the translation. 29 | t = avgMeasPos - avgRefPos*R; 30 | % fill structure and return. 31 | xfrm.rot = R'; 32 | xfrm.pos = t; -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/p2r2d.m: -------------------------------------------------------------------------------- 1 | function p2r2d(p) 2 | % p2r2d.m 6-07-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % given p and f(x), find r such that F(r)-p = 0, 9 | % express r in terms of sigma, CEP, MRE, DRMS(RMSR) 10 | % where F(r) = integral of f(x) from 0 to r. 11 | % Rayleigh pdf used for f(x) here. 12 | % 13 | 14 | disp(' ') 15 | disp(' ') 16 | disp([' For p=', num2str(p) ]) 17 | ic=menu('options:', 'k*sigma', 'k*MRE',... 18 | 'k*CEP', 'k*DRMS(RMSR)', 'all 4 above'); 19 | r0=cf3(p) ; 20 | % 21 | 22 | if ic==1 23 | disp(['r=', num2str(r0), ' * SIGMA']) 24 | end 25 | 26 | if ic==2 27 | r1=r0/sqrt(pi/2); 28 | disp(['r=', num2str(r1), ' * MRE']) 29 | end 30 | 31 | if ic==3 32 | r2=r0/cf3(.5); 33 | disp(['r=', num2str(r2), ' * CEP']) 34 | end 35 | 36 | if ic==4 37 | r3=r0/sqrt(2); 38 | disp(['r=', num2str(r3), ' * DRMS(RMSR)']) 39 | end 40 | 41 | if ic==5 42 | disp(['r=', num2str(r0), ' * SIGMA']) 43 | 44 | r1=r0/sqrt(pi/2); 45 | disp(['r=', num2str(r1), ' * MRE']) 46 | 47 | r2=r0/sqrt(2); 48 | disp(['r=', num2str(r2), ' * DRMS(RMSR)']) 49 | 50 | r3=r0/cf3(0.5); 51 | disp(['r=', num2str(r3), ' * CEP']) 52 | end 53 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cnf.m: -------------------------------------------------------------------------------- 1 | function p=cnf(ep,n) 2 | % cnf.m 7-01-94 3 | % determine confidence coeff about s, the sample estmate of population 4 | % sigma for a given sample size and variation epsilon (in % of s) 5 | % 6 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 7 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 8 | % dhsu@littongcs.com 9 | % 10 | 11 | % n _ 12 | % s^2 =SUM(xi - x)^2/(n-1) is the sample estimate for sigma^2 13 | % i=1 14 | 15 | %without the next line, n will stay the same after the first use. 16 | clear global n 17 | 18 | % n is used in the definition of chi-square CDF: x2cdf below. 19 | global n 20 | 21 | n=n-1; 22 | 23 | if n<0 | ep<0 24 | disp(' *** error in input *** ') 25 | else 26 | 27 | if n<101 & n>=0 % small sample 28 | if n==0 29 | p=0; 30 | else 31 | % s(1-epsilon) < sigma < s(1+epsilon) 32 | a=n/(1+ep)^2 ; 33 | b=n/(1-ep)^2 ; 34 | p=x2cdf(b)-x2cdf(a); 35 | end 36 | else % large sample 37 | a=sqrt(2*n)/(1+ep)-sqrt(2*n-1); 38 | b=sqrt(2*n)/(1-ep)-sqrt(2*n-1); 39 | p=quad8('nf1',a,b); 40 | end 41 | 42 | end 43 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edca.m: -------------------------------------------------------------------------------- 1 | function p=edca(r,mu) 2 | % edca.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % p = probability of ED (elliptical normal pdf) over CA(circular area) 9 | % mu = min(sx,sy)/max(sx,sy) 10 | % r = R/max(sx,sy) 11 | % x = theta angle 12 | % Normalized version of edcaud.m { p=edcaud(R,sx,sy) } 13 | % 1-23-96 from bf1b.m 14 | 15 | if mu>1 | mu<0 | r < 0 16 | error('check input arguments in edca(r,mu) ') 17 | end 18 | 19 | mlim=0.01; 20 | if mu=mlim & mu<1 26 | p=gf2(r,mu); % gf2.m replaces the next 7 lines 27 | end 28 | 29 | if mu==1 30 | p=cf2(r); 31 | end 32 | 33 | % method 1 34 | % this part does not work for mu=.5, and r=4 35 | % clear global 36 | % global mu r 37 | % p=quad8('bf1a',0,2*pi); 38 | % insure 39 | % this part does not work for mu=.5, and r=4 40 | % 41 | % method 2 42 | % this part works nicely, and faster 43 | % p=gf2(r,mu) % gf2.m replaces the next 7 lines 44 | % n=2000; 45 | % h=2*pi/n; 46 | % fac1=1/(2*pi*mu); 47 | % x=[0:n]*h; 48 | % gf=(cos(x).^2+sin(x).^2/mu^2); 49 | % y=(1-exp(-r^2.*gf/2))./gf; 50 | % p=fac1*simprule(y,h); 51 | % this part works nicely 52 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/edeacd.m: -------------------------------------------------------------------------------- 1 | function p=edeacd(a,b,sx,sy,rho) 2 | % edeacd.m 1-23-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1996 5 | % Copyright 1996-1997 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % function p=edeacd(a,b,sx,sy,rho) 9 | % edeac= elliptical normal pdf over elliptical area, x,y are correlated 10 | % elliptical normal pdf 11 | % 1/[2*pi*sx*sy*sqrt(1-rho^2)]* 12 | % exp{-[x^2/sx^2-2*rho*x*y/(sx*sy)+y^2/sy^2]/(2(1-rho^2)} 13 | % elliptical area of integration 14 | % x^2/a^2 + y^2/b^2 = 1 15 | % 16 | % example 17 | % a=0.6; b=1.2; sx=.4; sy=1; rho=0.3 18 | % p=edeacd(0.6, 1.2, 0.4, 1, 0.3) 19 | % 20 | 21 | if abs(rho)>1 | sx<=0 | sy<=0 | a<=0 | b<=0 22 | error('check input arguments in edeacd(a,b,sx,sy,rho)') 23 | end 24 | 25 | if abs(rho)==1 26 | u=sy/sx; 27 | st=sqrt((1+u^2))*sx; 28 | t0=sqrt(1+u^2)*a*b/sqrt(a^2*u^2+b^2); 29 | p=nf2(t0/st); 30 | end 31 | 32 | if rho==0 33 | p=edeaud(a,b,sx,sy) ; 34 | end 35 | 36 | if abs(rho)<1 & abs(rho) >0 37 | sig1=sx/a; 38 | sig2=sy/b; 39 | 40 | A=1/sig1^2; 41 | B=-2*rho/(sig1*sig2); 42 | C=1/sig2^2; 43 | 44 | [a1, c1]=abc2ac(A,B,C); 45 | 46 | s1=1/sqrt(a1); 47 | s2=1/sqrt(c1); 48 | 49 | f0=sqrt(1-rho^2); 50 | 51 | t1=s1*f0; 52 | t2=s2*f0; 53 | 54 | p=edcaud(1,t1,t2) ; 55 | end 56 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/psichi.dat: -------------------------------------------------------------------------------- 1 | 0.0002 0.0033 0.3683 1.2355 2.3063 2 | 0.0011 0.0120 0.5196 1.3990 2.3775 3 | 0.0037 0.0277 0.6461 1.5072 2.4067 4 | 0.0089 0.0499 0.7492 1.5806 2.4140 5 | 0.0172 0.0770 0.8330 1.6315 2.4093 6 | 0.0772 0.2039 1.0433 1.7220 2.3461 7 | 0.1588 0.3221 1.1479 1.7391 2.2700 8 | 0.2416 0.4180 1.2052 1.7325 2.2001 9 | 0.3169 0.4937 1.2384 1.7174 2.1388 10 | 0.3827 0.5539 1.2584 1.6993 2.0853 11 | 0.4393 0.6023 1.2704 1.6807 2.0384 12 | 0.4880 0.6418 1.2776 1.6624 1.9969 13 | 0.5298 0.6745 1.2816 1.6449 1.9600 14 | 0.7493 0.8326 1.2686 1.5174 1.7308 15 | 0.8315 0.8881 1.2439 1.4435 1.6140 16 | 0.8736 0.9161 1.2236 1.3946 1.5401 17 | 0.8989 0.9329 1.2074 1.3591 1.4880 18 | 0.9158 0.9441 1.1943 1.3320 1.4487 19 | 0.9279 0.9521 1.1834 1.3102 1.4176 20 | 0.9370 0.9581 1.1742 1.2924 1.3923 21 | 0.9440 0.9628 1.1663 1.2773 1.3711 22 | 0.9497 0.9665 1.1594 1.2644 1.3530 23 | 0.9749 0.9833 1.1189 1.1919 1.2532 24 | 0.9833 0.9889 1.0992 1.1584 1.2079 25 | 0.9875 0.9917 1.0871 1.1380 1.1807 26 | 0.9900 0.9933 1.0786 1.1240 1.1619 27 | 0.9917 0.9944 1.0722 1.1135 1.1481 28 | 0.9928 0.9952 1.0671 1.1054 1.1372 29 | 0.9937 0.9958 1.0630 1.0987 1.1285 30 | 0.9944 0.9963 1.0596 1.0932 1.1212 31 | 0.9950 0.9967 1.0567 1.0886 1.1151 32 | -------------------------------------------------------------------------------- /matlab/plotting/shared/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009, Kristjan Jonasson 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the distribution 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/noval1.m: -------------------------------------------------------------------------------- 1 | function R=noval1(p) 2 | % noval1.m 3-11-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % multiple ellipses with random axes orientations 9 | % p=input( 'p= (.5 in p.40 Burt) '); 10 | 11 | d2r=pi/180; 12 | 13 | load c:\mfile\nsd1.dat 14 | 15 | nsd1 16 | x=nsd1(:,1) ; 17 | y=nsd1(:,2) ; 18 | th=nsd1(:,3)*d2r ; 19 | 20 | a=cos(th).^2./x.^2 + sin(th).^2./y.^2 21 | b=cos(th).^2./y.^2 + sin(th).^2./x.^2 22 | c=cos(th).*sin(th).*(1./y.^2 - 1./x.^2) 23 | 24 | rho=c./sqrt(a.*b) 25 | w2=1./((1-rho.^2).*a) 26 | z2=1./((1-rho.^2).*b) 27 | 28 | sw2f=sum(w2) 29 | sz2f=sum(z2) 30 | rhof=sum(rho .*sqrt(w2) .*sqrt(z2))/(sqrt(sw2f)*sqrt(sz2f)) 31 | 32 | sx2f=0.5*(sw2f+sz2f+sqrt((sw2f+sz2f)^2-4*sw2f*sz2f*(1-rhof^2))) 33 | sy2f=0.5*(sw2f+sz2f-sqrt((sw2f+sz2f)^2-4*sw2f*sz2f*(1-rhof^2))) 34 | 35 | thf=atan2(2*rhof*sqrt(sw2f*sz2f),sw2f-sz2f)/d2r/2 36 | 37 | sa=sqrt(sx2f) 38 | sb=sqrt(sy2f) 39 | % ------------------------------------------------- 40 | 41 | mu=min(sa,sb)/max(sa,sb) ; 42 | 43 | K=bf4(p,mu) ; 44 | R=K*max(sa,sb) ; 45 | 46 | disp(['R=CEP(' num2str(p*100) ')= ' num2str(R)]) 47 | disp(['R= ' num2str(K) ' times max{ sigma(x),sigma(y) }' ]) 48 | disp(['sigma(x)= ' num2str(max(sa,sb)) ]) 49 | disp(['sigma(y)= ' num2str(min(sa,sb)) ]) 50 | disp(['u= ' num2str(mu) ]) 51 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/r2p1d.m: -------------------------------------------------------------------------------- 1 | function r2p1d(k) 2 | % r2p1d.m 5-04-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % given k = multiple of SIGMA 9 | % given k = multiple of MAE 10 | % given k = multiple of LEP 11 | % r =k * SIGMA 12 | % r =k * MAE (MAE= Mean Absolute Error) 13 | % r =k * LEP (LEP= Linear Error Probable) 14 | % find p = F(r) = integral of f(x) from -r to r 15 | % f(x) is 1-dim normal distribution N(0,1) 16 | 17 | disp( ' ') 18 | disp( ' ') 19 | ic=menu('Options:','k*SIGMA','k*MAE','k*LEP','all the above'); 20 | 21 | if ic==1 22 | r= k; 23 | p=nf2(r) ; 24 | disp(['For r=' ,num2str(k), ' * SIGMA']) 25 | disp(['p=', num2str(p)]) 26 | end 27 | 28 | if ic==2 29 | r= k* sqrt(2/pi); 30 | p=nf2(r) ; 31 | disp(['For r=' ,num2str(k), ' * MAE']) 32 | disp(['p=', num2str(p)]) 33 | end 34 | 35 | if ic==3 36 | r0=nf3(0.5); 37 | r=k*r0; 38 | p=nf2(r) ; 39 | disp(['For r=' ,num2str(k), ' * LEP']) 40 | disp(['p=', num2str(p)]) 41 | end 42 | 43 | if ic==4 44 | r= k; 45 | p=nf2(r) ; 46 | disp(['For r=' ,num2str(k), ' * SIGMA']) 47 | disp([' p=', num2str(p)]) 48 | 49 | r= k* sqrt(2/pi); 50 | p=nf2(r) ; 51 | disp(['For r=' ,num2str(k), ' * MAE']) 52 | disp([' p=', num2str(p)]) 53 | 54 | r=k*nf3(0.5); 55 | p=nf2(r) ; 56 | disp(['For r=' ,num2str(k), ' * LEP']) 57 | disp([' p=', num2str(p)]) 58 | end 59 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/tf2.m: -------------------------------------------------------------------------------- 1 | function p=tf2(r,u,v) 2 | % tf2.m 2-20-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % for triple integration over [0,r], u, v as parameters 9 | % avoid u=0 and v=0 in tf1.m 10 | 11 | c0=-1/2; 12 | c1=(v^2-1)/(2*v^2); 13 | c2=(u^2-1)/(2*u^2); 14 | fac1=4/(sqrt(2*pi)*(pi*u*v)); 15 | 16 | mr2=32; % 32 17 | mw2=64 ; % 64 18 | mt2=36; % 36 19 | 20 | dr=r/mr2; 21 | dw=1/mw2; 22 | dt=(pi/2)/mt2; 23 | 24 | % w.r.t. r 25 | for ir=1:mr2 % skip ir=0 26 | r=dr*ir; 27 | 28 | % w.r.t. w 29 | for iw=0:mw2 30 | w=dw*iw; 31 | 32 | % w.r.t. t 33 | if c2==0 | w==1 34 | sum3=pi/2; 35 | else 36 | it=0:mt2; 37 | t=dt*it; 38 | y3=exp(c2*r.^2.*(1-w.^2).*cos(t).^2); 39 | sum3=sum(y3(1:mt2:mt2+1))+2*sum(y3(3:2:mt2-1))+4*sum(y3(2:2:mt2)); 40 | sum3=sum3*dt/3; 41 | end 42 | 43 | if c1==0 | w==0 44 | y2(iw+1)=sum3; 45 | else 46 | y2(iw+1)=exp(c1*r.^2.*w.^2)*sum3 ; 47 | end 48 | 49 | end 50 | 51 | sum2=sum(y2(1:mw2:mw2+1))+2*sum(y2(3:2:mw2-1))+4*sum(y2(2:2:mw2)); 52 | sum2=sum2*dw/3; 53 | y1(ir+1)=exp(c0*r.^2).*r.^2*sum2; 54 | end 55 | 56 | sum1=y1(mr2+1)+2*sum(y1(3:2:mr2-1))+4*sum(y1(2:2:mr2)); 57 | p=fac1*sum1*dr/3; 58 | 59 | if p>1 60 | p=1; 61 | end 62 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/chi.dat: -------------------------------------------------------------------------------- 1 | 0.10 0.0033 0.3683 1.2355 2.3063 2 | 0.12 0.0120 0.5196 1.3990 2.3775 3 | 0.15 0.0277 0.6461 1.5072 2.4067 4 | 0.18 0.0499 0.7492 1.5806 2.4140 5 | 0.20 0.0770 0.8330 1.6315 2.4093 6 | 0.30 0.2039 1.0433 1.7220 2.3461 7 | 0.40 0.3221 1.1479 1.7391 2.2700 8 | 0.50 0.4180 1.2052 1.7325 2.2001 9 | 0.60 0.4937 1.2384 1.7174 2.1388 10 | 0.70 0.5539 1.2584 1.6993 2.0853 11 | 0.80 0.6023 1.2704 1.6807 2.0384 12 | 0.90 0.6418 1.2776 1.6624 1.9969 13 | 1.00 0.6745 1.2816 1.6449 1.9600 14 | 2.00 0.8326 1.2686 1.5174 1.7308 15 | 3.00 0.8881 1.2439 1.4435 1.6140 16 | 4.00 0.9161 1.2236 1.3946 1.5401 17 | 5.00 0.9329 1.2074 1.3591 1.4880 18 | 6.00 0.9441 1.1943 1.3320 1.4487 19 | 7.00 0.9521 1.1834 1.3102 1.4176 20 | 8.00 0.9581 1.1742 1.2924 1.3923 21 | 9.00 0.9628 1.1663 1.2773 1.3711 22 | 10.00 0.9665 1.1594 1.2644 1.3530 23 | 20.00 0.9833 1.1189 1.1919 1.2532 24 | 30.00 0.9889 1.0992 1.1584 1.2079 25 | 40.00 0.9917 1.0871 1.1380 1.1807 26 | 50.00 0.9933 1.0786 1.1240 1.1619 27 | 60.00 0.9944 1.0722 1.1135 1.1481 28 | 70.00 0.9952 1.0671 1.1054 1.1372 29 | 80.00 0.9958 1.0630 1.0987 1.1285 30 | 90.00 0.9963 1.0596 1.0932 1.1212 31 | 100.00 0.9967 1.0567 1.0886 1.1151 32 | -------------------------------------------------------------------------------- /matlab/math/fitellip.m: -------------------------------------------------------------------------------- 1 | % fitellip gives the 6 parameter vector of the algebraic circle fit 2 | % to a(1)x^2 + a(2)xy + a(3)y^2 + a(4)x + a(5)y + a(6) = 0 3 | % X & Y are lists of point coordinates and must be column vectors. 4 | function a = fitellip(X,Y) 5 | 6 | % normalize data 7 | mx = mean(X); 8 | my = mean(Y); 9 | sx = (max(X)-min(X))/2; 10 | sy = (max(Y)-min(Y))/2; 11 | x = (X-mx)/sx; 12 | y = (Y-my)/sy; 13 | 14 | % Build design matrix 15 | D = [ x.*x x.*y y.*y x y ones(size(x)) ]; 16 | 17 | % Build scatter matrix 18 | S = D'*D; 19 | 20 | % Build 6x6 constraint matrix 21 | C(6,6) = 0; C(1,3) = -2; C(2,2) = 1; C(3,1) = -2; 22 | 23 | % Solve eigensystem 24 | [gevec, geval] = eig(S,C); 25 | 26 | % Find the negative eigenvalue 27 | [NegR, NegC] = find(geval < 0 & ~isinf(geval)); 28 | % sometimes the perfect data gives a zero eigenvalue. 29 | if(isempty(NegC)) 30 | [NegR, NegC] = find(geval <= 0 & ~isinf(geval)); 31 | end 32 | 33 | % Extract eigenvector corresponding to positive eigenvalue 34 | A = gevec(:,NegC); 35 | 36 | % unnormalize 37 | a = [ 38 | A(1)*sy*sy, ... 39 | A(2)*sx*sy, ... 40 | A(3)*sx*sx, ... 41 | -2*A(1)*sy*sy*mx - A(2)*sx*sy*my + A(4)*sx*sy*sy, ... 42 | -A(2)*sx*sy*mx - 2*A(3)*sx*sx*my + A(5)*sx*sx*sy, ... 43 | A(1)*sy*sy*mx*mx + A(2)*sx*sy*mx*my + A(3)*sx*sx*my*my ... 44 | - A(4)*sx*sy*sy*mx - A(5)*sx*sx*sy*my ... 45 | + A(6)*sx*sx*sy*sy ... 46 | ]'; 47 | -------------------------------------------------------------------------------- /matlab/target_error/montecarlo/generateRandFiducialTargets.m: -------------------------------------------------------------------------------- 1 | function probe = generateRandFiducialTargets(nMarkers, mrkRange, targetRange) 2 | 3 | nValid = 0; 4 | disThresh = 5; 5 | angThresh = 2; 6 | while(~nValid) 7 | probe.Rigid.mrk = (mrkRange/2)*(rand(nMarkers, 3) - 0.5); 8 | probe.Rigid.tip = (targetRange/2)*(rand(1, 3) - 0.5); 9 | 10 | % check for collinearity. 11 | mrkPairs = nchoosek(1:nMarkers,2); 12 | segPairs = nchoosek(1:size(mrkPairs,1),2); 13 | 14 | for i = 1:size(segPairs,1) 15 | vec1 = probe.Rigid.mrk(mrkPairs(segPairs(i,1),1),:)... 16 | - probe.Rigid.mrk(mrkPairs(segPairs(i,1),2),:); 17 | d1 = sqrt(sum(vec1.^2,2)); 18 | 19 | vec2 = probe.Rigid.mrk(mrkPairs(segPairs(i,2),1), :)... 20 | - probe.Rigid.mrk(mrkPairs(segPairs(i,2),2), :); 21 | d2 = sqrt(sum(vec2.^2,2)); 22 | theta = acosd( (vec1*vec2')/(sqrt(vec1*vec1')*sqrt(vec2*vec2'))); 23 | if( d1 < disThresh ) 24 | nValid = 0; 25 | warning('generateRandFiducialTargets:: distance threshold violated.'); 26 | break; 27 | elseif( d2 < disThresh ) 28 | nValid = 0; 29 | warning('generateRandFiducialTargets:: distance threshold violated.'); 30 | break; 31 | elseif(theta < angThresh && theta > (180-angThresh)) 32 | nValid = 0; 33 | warning('generateRandFiducialTargets:: angle threshold violated.'); 34 | break; 35 | else 36 | nValid = 1; 37 | end 38 | end 39 | end 40 | -------------------------------------------------------------------------------- /matlab/math/rm2quat.m: -------------------------------------------------------------------------------- 1 | function q = rm2quat(R) 2 | 3 | w = 1; x = 2; y = 3; z = 4; 4 | 5 | q(w) = 1 + R(1,1) + R(2,2) + R(3,3); 6 | q(x) = 1 + R(1,1) - R(2,2) - R(3,3); 7 | q(y) = 1 - R(1,1) + R(2,2) - R(3,3); 8 | q(z) = 1 - R(1,1) - R(2,2) + R(3,3); 9 | 10 | if( (q(w) > q(x)) && (q(w) > q(y)) && (q(w) > q(z)) ) 11 | %q0 is the largest value. 12 | q(w) = sqrt(q(w)) / 2; 13 | if(q(w) == 0) 14 | return; 15 | end 16 | q(x) = (R(3,2) - R(2,3)) / (q(w) * 4); 17 | q(y) = (R(1,3) - R(3,1)) / (q(w) * 4); 18 | q(z) = (R(2,1) - R(1,2)) / (q(w) * 4); 19 | elseif( (q(x) > q(w)) && (q(x) > q(y)) && (q(x) > q(z)) ) 20 | %qx is the largest value. 21 | q(x) = sqrt(q(x)) / 2; 22 | if(q(x) == 0) 23 | return; 24 | end 25 | q(w) = (R(3,2) - R(2,3)) / (q(x) * 4); 26 | q(y) = (R(2,1) + R(1,2)) / (q(x) * 4); 27 | q(z) = (R(1,3) + R(3,1)) / (q(x) * 4); 28 | elseif( (q(y) > q(w)) && (q(y) > q(x)) && (q(y) > q(z)) ) 29 | %qy is the largest value. 30 | q(y) = sqrt(q(y)) / 2; 31 | if(q(y) == 0) 32 | return; 33 | end 34 | q(w) = (R(1,3) - R(3,1)) / (q(y) * 4); 35 | q(x) = (R(2,1) + R(1,2)) / (q(y) * 4); 36 | q(z) = (R(3,2) + R(2,3)) / (q(y) * 4); 37 | else %( (q(z) > q(w)) && (q(z) > q(x)) && (q(z) > q(y)) ) 38 | %qz is the largest value. 39 | q(z) = sqrt(q(z)) / 2; 40 | if(q(z) == 0) 41 | return; 42 | end 43 | q(w) = (R(2,1) - R(1,2)) / (q(z) * 4); 44 | q(x) = (R(1,3) + R(3,1)) / (q(z) * 4); 45 | q(y) = (R(3,2) + R(2,3)) / (q(z) * 4); 46 | end 47 | 48 | q = getQuatNormalized(q); -------------------------------------------------------------------------------- /matlab/target_error/misc/square_tool.m: -------------------------------------------------------------------------------- 1 | %mrk = [0 25 0; 0 -25 0; -10 10 0; 10 -10 0]; 2 | mrk = [-75, 50, 0; 0, 50, 0; 75, 50, 0; -25, 0, 0;... 3 | 25, 0, 0; -75, -50, 0; 0, -50, 0; 75, -50, 0 ]; 4 | tip = [0 0 0]; 5 | FLE = 0.15; 6 | tip_dist = -250:10:250; 7 | 8 | %******** 9 | % test 1 10 | %******** 11 | for i=1:length(tip_dist) 12 | mrk0 = [mrk; tip + [0 tip_dist(i) 0] ]; 13 | tre(i) = calcTRE_NDI(FLE, mrk0,0); 14 | end 15 | 16 | figure(2); 17 | plot(tip_dist, tre); 18 | title('TRE for tool tip along the y-axis'); 19 | xlabel('Tool tip location on y-axis (mm)'); 20 | ylabel('TRE (mm)'); 21 | 22 | print -dtiff TRE_y-axis 23 | %******** 24 | % test 2 25 | %******** 26 | 27 | [X, Y] = meshgrid(-250:10:250, -250:10:250); 28 | for i = 1:size(X,1) 29 | for j = 1:size(X,2) 30 | mrk0 = [mrk; tip + [X(i,j), Y(i,j), 0]]; 31 | tre_plane(i,j) = calcTRE_NDI(FLE, mrk0,0); 32 | end 33 | end 34 | 35 | figure(3); 36 | surf(X,Y,tre_plane); 37 | 38 | title('TRE for tool tip in the xy-plane'); 39 | xlabel('Tool tip location on x-axis (mm)'); 40 | ylabel('Tool tip location on y-axis (mm)'); 41 | zlabel('TRE (mm)'); 42 | print -dtiff TRE_plane 43 | 44 | %******** 45 | % test 3 46 | %******** 47 | 48 | angle = 0:1:359; 49 | max_tip_dist = max(tip_dist); 50 | for i=1:length(angle) 51 | mrk0 = [mrk; tip + (max_tip_dist*[0 cosd(angle(i)) sind(angle(i))]) ]; 52 | tre_polar(i) = calcTRE_NDI(FLE, mrk0,0); 53 | end 54 | 55 | figure(4); 56 | plot(angle, tre_polar); 57 | title('TRE for tool tip rotated about the x-axis'); 58 | xlabel('Tool tip rotated about the x-axis (degrees)'); 59 | ylabel('TRE (mm)'); -------------------------------------------------------------------------------- /matlab/target_error/misc/toolPA.m: -------------------------------------------------------------------------------- 1 | FLE = 0.35; 2 | 3 | x = [-175 0 90; 4 | 0 18.61 -36.95; 5 | 0 47.98 63.97; 6 | 100 0 0]; 7 | pitch = 50; 8 | R = [cos(pitch) 0 -sin(pitch); 9 | 0 1 0; 10 | sin(pitch) 0 cos(pitch)]; 11 | 12 | xr = (R * x')' 13 | return; 14 | 15 | figure(1); 16 | scatter3(x(:,1), x(:,2), x(:,3)); 17 | hold on; 18 | scatter3(xr(:,1), xr(:,2), xr(:,3),'k'); 19 | hold off; 20 | xlabel('x'); 21 | ylabel('y'); 22 | zlabel('z'); 23 | 24 | X = x - repmat(mean(x(1:end-1,:)),size(x,1),1) 25 | 26 | [U,S,V] = svd(X(1:end-1,:),0) 27 | 28 | Xp = [X] * V * inv(S) 29 | 30 | figure(2) 31 | scatter(x(1:3,3), x(1:3,1)); 32 | hold on; 33 | scatter(xr(1:3,3), xr(1:3,1),'r'); 34 | scatter(x(4,3), x(4,1),'x'); 35 | scatter(xr(4,3), xr(4,1),'xr'); 36 | hold off; 37 | xlabel('z'); 38 | ylabel('x'); 39 | axis([-10 110 -120 120]); 40 | axis equal; 41 | set(gca,'XDir','reverse'); 42 | set(gca,'YDir','reverse'); 43 | 44 | tip = [100, 0, 0]; 45 | 46 | TRE = calcTRE(FLE,[x;tip]) 47 | 48 | 49 | 50 | % [U0,S0,V0] = svd(X,0); 51 | % 52 | % tail_1 = zeros(3); tip_1 = 10*eye(3); 53 | % tail_2 = zeros(3); tip_2 = 10*V'; 54 | % 55 | % scatter3(X(:,1), X(:,2), X(:,3)); 56 | % hold on; 57 | % quiver3(tail_1(:,1), tail_1(:,2), tail_1(:,3), tip_1(:,1), tip_1(:,2), tip_1(:,3), 'b'); 58 | % quiver3(tail_2(:,1), tail_2(:,2), tail_2(:,3), tip_2(:,1), tip_2(:,2), tip_2(:,3), 'g'); 59 | % hold off; 60 | % 61 | % axis equal; 62 | 63 | % x = [ 35.1791 -53.8979 9.0211; 64 | % -9.1391 -32.3543 -2.1145; 65 | % -13.8722 33.7664 -3.8523; 66 | % -65.6578 39.5779 -30.1377; 67 | % 53.4901 12.9080 27.0834 ] 68 | -------------------------------------------------------------------------------- /matlab/stats/WishartHypTest.m: -------------------------------------------------------------------------------- 1 | function [chistat, chi2prob, dof] = WishartHypTest(mu0, Sigma0, data, test, alpha) 2 | 3 | %% set the data up. 4 | % get the dimensions of the data. 5 | [N,p] = size(data); 6 | 7 | % convert the data into the identity form. 8 | % 1. demean the data. 9 | datatemp = data - repmat(mu0, size(data,1), 1); 10 | % 2. transform the data into the identity space. 11 | SigmaInvSqrt = inv(sqrtm(Sigma0)); 12 | data1 = (SigmaInvSqrt*datatemp')'; 13 | 14 | % compute the observational statistics of the data. 15 | mu1 = mean(data); 16 | Sigma1 = cov(data); 17 | 18 | V = data1'*data1; 19 | %VoverN = V/N; 20 | 21 | %% hypothesis test. 22 | 23 | switch(test) 24 | case {'covariance'} 25 | % H0: Sigma = I 26 | % compute the lambda statistic (lambda^* in Srivastava) 27 | % lambda = (exp(1)/n)^(0.5*p*n)* det(V)^(0.5*n)*exp(-0.5*trace(V)); 28 | % chistat = -2*log(lambda); 29 | n = N-1; 30 | chistat = -p*n + p*n*log(n) - n * log(det(V)) + trace(V); 31 | dof = 0.5*p*(p+1); 32 | case{'meanandcov'} 33 | % H0: mu = 0, Sigma = I 34 | % lambda = (exp(1)/N)^(0.5*p*N)* det(V)^(0.5*N)*exp(-0.5*trace(V + N*mu1'*mu1)); 35 | % chistat = -2*log(lambda); 36 | chistat = -p*N + p*N*log(N) - N * log(det(V)) + trace(V + N * mu1'*mu1); 37 | dof = 0.5*p*(p+1) + p; 38 | otherwise 39 | error('Invalid Hypothesis Test Type Provided'); 40 | end 41 | 42 | chi2prob = chidist(chistat,dof); 43 | 44 | % if(nargin > 4) 45 | % switch(alpha) 46 | % case 0.10 47 | % 48 | % case 0.05 49 | % case 0.01 50 | % otherwise 51 | % error('Invalid alpha value provided.'); 52 | % end 53 | % end -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateTRE/runTRESimScript.m: -------------------------------------------------------------------------------- 1 | % common parameters. 2 | parm.experimentDir = 'E:\docs\research\phd\experiments\TRE Experiments'; 3 | parm.M = 1000; 4 | parm.N =10000; 5 | fleSigmaType = 'aniso'; 6 | fleSigma = diag([0.0995 0.0995 0.2985].^2); 7 | toolDesign = 'e'; 8 | A = 71; 9 | B = 0.5*54; 10 | rho = 85*[1:8]; 11 | 12 | 13 | for i = 1:8 14 | parm.case{i}.fleSigma = fleSigma; 15 | [parm.case{i}.Rigid.mrk, parm.case{i}.Rigid.normals, parm.case{i}.Rigid.tip]... 16 | = getWestToolDesign(toolDesign, A, B, rho(i)); 17 | parm.case{i}.xfrm.pos = [0 0 -1000]; 18 | parm.case{i}.xfrm.R = eye(3); 19 | parm.case{i}.varyRotation = 1; 20 | parm.case{i}.varyRotationAngle = 30; 21 | parm.case{i}.key = rho(i); 22 | parm.case{i}.name = sprintf('Des_%s_%s_A%dB%drho%d',toolDesign, fleSigmaType,A,B,rho(i)); 23 | end 24 | 25 | test = runTRESimulations(parm); 26 | 27 | % common parameters. 28 | parm.experimentDir = 'E:\docs\research\phd\experiments\TRE Experiments'; 29 | parm.M = 1000; 30 | parm.N =10000; 31 | fleSigmaType = 'aniso'; 32 | fleSigma = diag([0.0995 0.0995 0.2985].^2); 33 | toolDesign = 'e'; 34 | A = 0.5*71; 35 | B = 0.5*54; 36 | rho = 85*[1:8]; 37 | 38 | 39 | for i = 1:8 40 | parm.case{i}.fleSigma = fleSigma; 41 | [parm.case{i}.Rigid.mrk, parm.case{i}.Rigid.normals, parm.case{i}.Rigid.tip]... 42 | = getWestToolDesign(toolDesign, A, B, rho(i)); 43 | parm.case{i}.xfrm.pos = [0 0 -1000]; 44 | parm.case{i}.xfrm.R = eye(3); 45 | parm.case{i}.varyRotation = 1; 46 | parm.case{i}.varyRotationAngle = 30; 47 | parm.case{i}.key = rho(i); 48 | parm.case{i}.name = sprintf('Des_%s_%s_A%dB%drho%d',toolDesign, fleSigmaType,A,B,rho(i)); 49 | end 50 | 51 | test = runTRESimulations(parm); -------------------------------------------------------------------------------- /matlab/target_error/misc/TREtest2.m: -------------------------------------------------------------------------------- 1 | % TREtest2.m 2 | close all; 3 | clear all; 4 | 5 | % polaris active case 3 on pg. 541(9) of West. 6 | A = 71; 7 | B = 54; 8 | rho = 170; 9 | r = 64; 10 | d = 200; 11 | 12 | % probe marker and tip positions 13 | xp = [-A/2 -B/2 0; -A/2 B/2 0; A/2 B/2 0; A/2 -B/2 0]; 14 | tp = [rho 0 0]; 15 | 16 | % coordinate reference frame 17 | ang = deg2rad(45); 18 | xc = [r*cos(ang) r*sin(ang) 0; -r*cos(ang) r*sin(ang) 0;... 19 | -r*cos(ang) -r*sin(ang) 0; r*cos(ang) -r*sin(ang) 0]; 20 | tc = [d 0 0]; 21 | 22 | % define an arbitrary FLE error value where each axis is identical. 23 | FLE = 0.33; 24 | TREp_West = calcTRE_NDI(FLE,[xp;tp],1); 25 | TREc_West = calcTRE_NDI(FLE,[xc;tc],1); 26 | TREpc_West = sqrt(TREp_West^2 + TREc_West^2); 27 | 28 | % redefine the FLE error to have 3 times the error on the z-axis. 29 | FLE = [0.0995 0.0995 0.2985].^2; 30 | TRE0 = calcTRE_NDI(FLE,[xp;tp],2); 31 | 32 | ang = [-45:5:45]; 33 | 34 | for i = 1:length(ang) 35 | R = getRotMatrix(0, ang(i), 0) 36 | xp_test = (R * xp')'; 37 | tp_test = (R * tp')'; 38 | xc_test = (R * xc')'; 39 | tc_test = (R * tc')'; 40 | 41 | TREp_ang(i) = calcTRE_NDI(FLE,[xp_test;tp_test],0); 42 | TREc_ang(i) = calcTRE_NDI(FLE,[xc_test;tc_test],0); 43 | TREpc_ang(i) = sqrt(TREp_ang(i)^2 + TREc_ang(i)); 44 | end 45 | 46 | figure(3); 47 | plot(ang, TREp_ang); 48 | hold on; 49 | plot(ang, TREc_ang,'g'); 50 | plot(ang, TREpc_ang,'k'); 51 | plot(0, TREp_West, 'ob'); 52 | plot(0, TREc_West, 'og'); 53 | plot(0, TREpc_West, 'ok'); 54 | hold off; 55 | xlabel('Rotation about y-axis from default orientation (degrees)'); 56 | ylabel('TRE (mm)'); 57 | legend('TRE - Probe', 'TRE - CRF', 'TRE - Combined',0); 58 | 59 | fprintf('Probe TRE = %4.2f\nCRF TRE = %4.2f\nCombined TRE = %4.2f\n', TREp_West, TREc_West, TREpc_West); -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateTRE/plotRandProbeXfrmExperiment.m: -------------------------------------------------------------------------------- 1 | function plotRandFiducialTargetExperiment(testname, fleModel, figFontSize) 2 | 3 | cd(testname) 4 | 5 | data = csvread('data.csv'); 6 | id = 1; 7 | covonly = 2; 8 | muandcov = 3; 9 | measRMS = 4; 10 | theoryRMS = 5; 11 | RMSDiff = 6; 12 | percentDiff = 7; 13 | 14 | 15 | 16 | load parm; 17 | 18 | %% histogram 19 | figure(1); 20 | hist(data(:,percentDiff)); 21 | axis([-1,1,0,300]); 22 | titlestring = sprintf('Histogram of the %% Differences for RMS'); 23 | subtitlestring = sprintf('%s FLE Model, RMS_{FLE} = %3.2f, Tool %s, {\\rho} = %d mm',... 24 | fleModel, sqrt(trace(parm.Sigma)), parm.design, parm.rho); 25 | title({titlestring, subtitlestring}, 'fontsize',figFontSize); 26 | set(gca,'fontsize',figFontSize); 27 | figurefilename = sprintf('Histogram_%s', parm.name); 28 | print('-depsc', '-tiff', '-r300', figurefilename); 29 | 30 | %% plot the scatter plot. 31 | figure(2); 32 | plot(data(:,theoryRMS), data(:,measRMS), '.k'); 33 | hold on; 34 | minRMS = min(min(data(:,measRMS:theoryRMS))); 35 | maxRMS = max(max(data(:,measRMS:theoryRMS))); 36 | plot([minRMS maxRMS], [minRMS,maxRMS], 'k-'); 37 | hold off; 38 | 39 | xlabel('Predicted TRE RMS (mm)', 'fontsize',figFontSize); 40 | ylabel('Simulated TRE RMS (mm)', 'fontsize',figFontSize); 41 | set(gca,'fontsize',figFontSize); 42 | 43 | titlestring = sprintf('Predicted versus Simulated TRE RMS'); 44 | subtitlestring = sprintf('%s FLE Model, RMS_{FLE} = %3.2f, Tool %s, {\\rho} = %d mm',... 45 | fleModel, sqrt(trace(parm.Sigma)), parm.design, parm.rho); 46 | title({titlestring, subtitlestring}, 'fontsize',figFontSize); 47 | 48 | axis([minRMS, maxRMS, minRMS, maxRMS]); 49 | figurefilename = sprintf('PredvMeas_%s', parm.name); 50 | print('-depsc', '-tiff', '-r300', figurefilename); 51 | 52 | copyfile('*.eps', 'E:\awiles\data\tretest\IEEE_Data\WestTools' ); 53 | 54 | cd .. 55 | -------------------------------------------------------------------------------- /matlab/target_error/estimateFLE/updateFREStats.m: -------------------------------------------------------------------------------- 1 | function [fremean, frecov, frewin] = updateFREStats(newfre, fremean, frecov, frewin) 2 | % Written by Andrew D. Wiles, 2009-12-03. 3 | % 4 | % Here we update the FRE statistics for a single marker. 5 | % Input: 6 | % newfre: 1x3 vector with the FRE components. 7 | % fremean: 1x3 vector with the mean computed on the last frame. 8 | % frecov: 3x3 covariance matrix computed on the last frame. 9 | % frewin: the sliding window that holds the N FRE measurements that 10 | % estimate the statistics. 11 | % 12 | % Here we add the newest data and remove the oldest. Two methods are 13 | % coded. The first method assumes that the FRE mean is zero and hence 14 | % the update can be reduced to adding the latest and removing the 15 | % oldest. The second method assumes that the mean is not zero and that we 16 | % need to recompute the covariance using the mean. In simulation this 17 | % works quite well but it still needs to be proven effective for real 18 | % data. 19 | 20 | winsize = size(frewin,1); 21 | 22 | % hard code the method for the covariance computation. 23 | covmethod = 1; % use the assumption that the FRE mean is zero. 24 | %covmethod = 2; % use the matlab function cov. 25 | 26 | switch covmethod 27 | case 1 28 | %update the mean. 29 | fremean = fremean + 1/winsize* (newfre - frewin(end,:)); 30 | %update the covariance 31 | frecov = frecov + 1/(winsize-1) * (newfre'*newfre - frewin(end,:)'*frewin(end,:)); 32 | %update the window. 33 | frewin = circshift(frewin,1); 34 | frewin(1,:) = newfre; 35 | case 2 36 | %update the window. 37 | frewin = circshift(frewin,1); 38 | frewin(1,:) = newfre; 39 | fremean = mean(frewin); 40 | frecov = cov(frewin); 41 | otherwise 42 | error('Invalid covariance method given.'); 43 | end -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/r2p3d.m: -------------------------------------------------------------------------------- 1 | function r2p3d(k) 2 | % r2p3d.m 2-26-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % 9 | % for 3-dim normal distribution f(x,y,z) with sigma(x)=sigma(y)=sigma(z) 10 | % given k = multiple of SIGMA 11 | % given k = multiple of MRE 12 | % given k = multiple of RMSR 13 | % given k = multiple of SEP 14 | % r =k * SIGMA 15 | % r =k * MSRE (MRE= Mean Spherical Radial Error) 16 | % r =k * RMSR (RMSR= Root Mean Square Spherical Radial Error) 17 | % r =k * SEP (SEP= Spherical Error Probable) 18 | % find p=F(r) 19 | % where F(r) = integral of f(x) from 0 to r. 20 | % Maxwell pdf used for f(x) here. 21 | % 22 | 23 | disp( ' ') 24 | disp( ' ') 25 | ic=menu('Options:','k*SIGMA','k*MRE','k*RMSR','k*SEP','all the above'); 26 | 27 | if ic==1 28 | r= k; 29 | p=sf2(r) ; 30 | disp(['For R=' ,num2str(k), ' * SIGMA']) 31 | disp(['p=', num2str(p)]) 32 | end 33 | 34 | if ic==2 35 | r= k* 2*sqrt(2/pi); 36 | p=sf2(r) ; 37 | disp(['For R=' ,num2str(k), ' * MRE']) 38 | disp(['p=', num2str(p)]) 39 | end 40 | 41 | if ic==3 42 | r= k*sqrt(3); 43 | p=sf2(r) ; 44 | disp(['For R=' ,num2str(k), ' * RMSR']) 45 | disp(['p=', num2str(p)]) 46 | end 47 | 48 | if ic==4 49 | r=k*sf4(0.5); 50 | p=sf2(r) ; 51 | disp(['For R=' ,num2str(k), ' * SEP']) 52 | disp(['p=', num2str(p)]) 53 | end 54 | 55 | if ic==5 56 | r= k; 57 | p=sf2(r) ; 58 | disp(['For R=' ,num2str(k), ' * SIGMA']) 59 | disp([' p=', num2str(p)]) 60 | 61 | r= k* 2*sqrt(2/pi); 62 | p=sf2(r) ; 63 | disp(['For R=' ,num2str(k), ' * MRE']) 64 | disp([' p=', num2str(p)]) 65 | 66 | r= k*sqrt(3); 67 | p=sf2(r) ; 68 | disp(['For R=' ,num2str(k), ' * RMSR']) 69 | disp([' p=', num2str(p)]) 70 | 71 | r=k*sf4(0.5); 72 | p=sf2(r) ; 73 | disp(['For R=' ,num2str(k), ' * SEP']) 74 | disp([' p=', num2str(p)]) 75 | end 76 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/r2p2d.m: -------------------------------------------------------------------------------- 1 | function r2p2d(k) 2 | % r2p2d.m 6-07-96 3 | % 4 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 5 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 6 | % dhsu@littongcs.com 7 | % 8 | % for 2-dim normal distribution with sigma(x)=sigma(y), rho=0. 9 | % given k = multiple of SIGMA 10 | % given k = multiple of MRE 11 | % given k = multiple of DRMS(RMSR) 12 | % given k = multiple of CEP 13 | % r =k * SIGMA 14 | % r =k * MRE (MRE= Mean Radial Error) 15 | % r =k * DRMS(RMSR) (RMSR=Root Mean Square Radial Error) 16 | % DRMS=Distance Root Mean Square 17 | % r =k * CEP (CEP= Circular Error Probable) 18 | % find p=F(r) 19 | % where F(x) = integral of f(x) from 0 to r. 20 | % Rayleigh pdf used for f(x) here. 21 | % 22 | 23 | disp( ' ') 24 | disp( ' ') 25 | ic=menu('Options:','k*SIGMA','k*MRE','k*DRMS','k*CEP','all the above'); 26 | 27 | if ic==1 28 | r= k; 29 | p=cf2(r) ; 30 | disp(['For r=' ,num2str(k), ' * SIGMA']) 31 | disp(['p=', num2str(p)]) 32 | end 33 | 34 | if ic==2 35 | r= k* sqrt(pi/2); 36 | p=cf2(r) ; 37 | disp(['For r=' ,num2str(k), ' * MRE']) 38 | disp(['p=', num2str(p)]) 39 | end 40 | 41 | if ic==3 42 | r= k* sqrt(2); 43 | p=cf2(r) ; 44 | disp(['For r=' ,num2str(k), ' * DRMS(RMSR)']) 45 | disp(['p=', num2str(p)]) 46 | end 47 | 48 | if ic==4 49 | r=k*cf3(0.5); 50 | p=cf2(r) ; 51 | disp(['For r=' ,num2str(k), ' * CEP']) 52 | disp(['p=', num2str(p)]) 53 | end 54 | 55 | if ic==5 56 | r= k; 57 | p=cf2(r) ; 58 | disp(['For r=' ,num2str(k), ' * SIGMA']) 59 | disp([' p=', num2str(p)]) 60 | 61 | r= k* sqrt(pi/2); 62 | p=cf2(r) ; 63 | disp(['For r=' ,num2str(k), ' * MRE']) 64 | disp([' p=', num2str(p)]) 65 | 66 | r= k* sqrt(2); 67 | p=cf2(r) ; 68 | disp(['For r=' ,num2str(k), ' * DRMS(RMSR)']) 69 | disp([' p=', num2str(p)]) 70 | 71 | r=k*cf3(0.5); 72 | p=cf2(r) ; 73 | disp(['For r=' ,num2str(k), ' * CEP']) 74 | disp([' p=', num2str(p)]) 75 | end 76 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/chi1.m: -------------------------------------------------------------------------------- 1 | % chi1.m 1-15-94 2 | % 3 | % Spatial Error Analysis ToolBox Version 1.0, October 5, 1997 4 | % Copyright 1997-1998 by David Y. Hsu All rights reserved. 5 | % dhsu@littongcs.com 6 | % 7 | clear 8 | ic=2; % skip step 1 9 | 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | if ic==1 12 | % part I 13 | % use y=sqrt(x/n), x has chi-square distribution with deg n 14 | clear global n p 15 | global n p 16 | 17 | q=[0.5 .8 .9 .95]; 18 | l=[(0.1:0.025:0.2) (0.3:.1:.9) (1:9) (10:10:100)]'; 19 | NL = length(l); 20 | NQ = length(q); 21 | 22 | for jj=1:NQ 23 | p=q(jj); 24 | x =1; %initial guess for iteration 25 | % x =0.01; %initial guess for fsolve 26 | for ii=1:NL 27 | n=l(ii); 28 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 29 | %% use fsolve from optimization toolbox %% 30 | %% xs=fsolve('x2disc2',x); 31 | %% xo= xs; 32 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 33 | %%%%%% iteration %%%%%%%%%%%% 34 | 35 | p0=x2cdf(x); 36 | for j=1:2:9 37 | while (p0 > p) 38 | x=x-(0.1)^j; 39 | p0=x2cdf(x); 40 | end 41 | 42 | while (p0 < p) 43 | x=x+(0.1)^j; 44 | p0=x2cdf(x); 45 | end 46 | end 47 | xo=x; 48 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%% 49 | r(ii,jj)=sqrt(xo/n) 50 | pause 51 | end 52 | end 53 | 54 | DAT =[l r]; 55 | save \mfile\chi.mat l r 56 | 57 | fname='c:\mfile\chi.dat' 58 | fid=fopen(fname,'wt') 59 | fprintf(fid,'%8.2f %8.4f %8.4f %8.4f %8.4f\n',DAT') 60 | fclose(fid) 61 | end 62 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 63 | if ic==2 64 | % part II 65 | load c:\mfile\chi.mat 66 | NQ =4 67 | adj= [ 0.7 0.5 0.15 -0.1]; 68 | 69 | semilogx(l,r) 70 | xlabel('n') 71 | ylabel(' R(p)/RMS ') 72 | % title(' R(p)/RMS vs n, p=0.5, 0.8, 0.9, 0.95') 73 | grid 74 | 75 | for kk=1:NQ 76 | if kk==1 p=0.5 77 | elseif kk==2 p=0.8 78 | elseif kk==3 p=0.9 79 | elseif kk==4 p=0.95 80 | end 81 | 82 | text(.5,r(5,kk)+adj(kk),['p=' num2str(100*p) '%']) 83 | end 84 | axis('square') 85 | figure(1) 86 | pause 87 | pf3('f62') 88 | end 89 | -------------------------------------------------------------------------------- /matlab/math/registration/AnisotropicWeighted/point_register.m: -------------------------------------------------------------------------------- 1 | function [R, t, FRE] = point_register(X, Y, weight_sqr) 2 | 3 | % This function performs point-based rigid body registration. It returns a 4 | % rotation matrix, translation vector and the FRE. If provided with square 5 | % of weights, it performs weighted version of registration and the FRE 6 | % returned is the weighted FRE. 7 | 8 | if nargin < 2 9 | error('At least two input arguments are required.'); 10 | end 11 | 12 | [Ncoords Npoints] = size(X); 13 | [Ncoords_Y Npoints_Y] = size(Y); 14 | 15 | if Ncoords ~= 3 | Ncoords_Y ~= 3 16 | error('Each argument must have exactly three rows.') 17 | elseif (Ncoords ~= Ncoords_Y) | (Npoints ~= Npoints_Y) 18 | error('X and Y must have the same number of columns.'); 19 | elseif Npoints < 3 20 | error('X and Y must each have 3 or more columns.'); 21 | end 22 | 23 | if nargin==2 24 | Xbar = mean(X,2); % X centroid 25 | Ybar = mean(Y,2); % Y centroid 26 | Xtilde = X-repmat(Xbar,1,Npoints); % X relative to centroid 27 | Ytilde = Y-repmat(Ybar,1,Npoints); % Y relative to centroid 28 | H = Xtilde*Ytilde'; % cross covariance matrix 29 | [U S V] = svd(H); % U*S*V' = H 30 | R = V*diag([1, 1, det(V*U)])*U'; 31 | t = Ybar - R*Xbar; 32 | if nargout == 3 33 | FREvect = R*X + repmat(t,1,Npoints) - Y; 34 | FRE = sqrt(mean(sum(FREvect.^2,1))); 35 | end 36 | else 37 | weight_sqr = diag(diag(weight_sqr))'; % make sure it is a row vector 38 | w_norm = weight_sqr/sum(weight_sqr); 39 | Xbar = X*w_norm'; 40 | Ybar = Y*w_norm'; 41 | Xtilde = X-repmat(Xbar,1,Npoints); % X relative to centroid 42 | Ytilde = Y-repmat(Ybar,1,Npoints); % Y relative to centroid 43 | H = Xtilde*diag(weight_sqr)*Ytilde'; 44 | [U S V] = svd(H); % U*S*V' = H 45 | R = V*diag([1, 1, det(V*U)])*U'; 46 | t = Ybar - R*Xbar; 47 | if nargout==3 % compute weighted FRE 48 | X1 = R*X + repmat(t,1,Npoints); 49 | FRE = sqrt(trace((X1 - Y)*diag(weight_sqr)*(X1-Y)'/Npoints)); 50 | end 51 | end 52 | 53 | return -------------------------------------------------------------------------------- /matlab/math/registration/AnisotropicWeighted/anisotropic_point_register.m: -------------------------------------------------------------------------------- 1 | function [R,t,FRE,n,isConverged] = anisotropic_point_register(X,Y,W,threshold) 2 | % X is the moving set, which is registered to the static set Y. Both are 3 3 | % by N, where N is the number of fiducials. W is a 3-by-3-by-N array, with 4 | % each page containing the weighting matrix for the Nth pair of points. 5 | % THRESHOLD is the size of the change to the moving set above which the 6 | % iteration continues. 7 | % 8 | % Creation: 9 | % R. Balachandran and J. M. Fitzpatrick 10 | % December 2008 11 | if nargin<2 12 | error('X and Y must be given as input'); 13 | end 14 | if size(X,1)~=3 && size(Y,1)~=3 15 | error('X and Y must be 3 by N.'); 16 | end 17 | N = size(X,2); 18 | if size(Y,2)~=N 19 | error('X and Y must have the same number of points.'); 20 | end 21 | 22 | isConverged = 1; 23 | 24 | % Initial estimate of the transformation assumes anisotropy: 25 | [R,t,FRE] = point_register(X,Y); 26 | if nargin<3 % if W not given, then give the isotropic solution 27 | n = 0; 28 | return 29 | end 30 | if nargin<4 31 | threshold = 1e-6; 32 | end 33 | n = 0; % iteration index = 0; 34 | config_change = Inf; 35 | Xold = R*X+repmat(t,1,N); 36 | while (config_change>threshold) 37 | if n > 1000 38 | isConverged = 0; 39 | return; 40 | end 41 | n = n+1; 42 | C = C_maker(Xold,W); 43 | e = e_maker(Xold,Y,W); 44 | q = C\e; 45 | if n > 1 46 | q = (q + oldq)/2; 47 | end %damps oscillations 48 | oldq = q; 49 | delta_t = [q(4) q(5) q(6)]'; 50 | delta_theta = [1 -q(3) q(2); q(3) 1 -q(1); -q(2) q(1) 1]; 51 | [U,Lambda,V] = svd(delta_theta); 52 | delta_R = U*V'; 53 | R = delta_R*R; % update rotation 54 | t = delta_R*t+delta_t; % update translation 55 | Xnew = R*X+repmat(t,1,N); % update moving points 56 | config_change = sqrt(sum(sum((Xnew-Xold).^2))/... 57 | sum(sum((Xold-repmat(mean(Xold,2),1,N)).^2))); 58 | Xold = Xnew; 59 | end 60 | for ii = 1:N 61 | D = W(:,:,ii)*(Xnew(:,ii)-Y(:,ii)); 62 | FRE(ii) = D'*D; 63 | end 64 | FRE = sqrt(mean(FRE)); -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cf4a.dat: -------------------------------------------------------------------------------- 1 | 0.000 & 0.0000 & 0.0000 & 0.0000 \\ 2 | 0.025 & 0.2250 & 0.1795 & 0.1911 \\ 3 | 0.050 & 0.3203 & 0.2556 & 0.2720 \\ 4 | 0.075 & 0.3949 & 0.3151 & 0.3354 \\ 5 | 0.100 & 0.4590 & 0.3663 & 0.3899 \\ 6 | 0.125 & 0.5168 & 0.4123 & 0.4389 \\ 7 | 0.150 & 0.5701 & 0.4549 & 0.4842 \\ 8 | 0.175 & 0.6203 & 0.4949 & 0.5268 \\ 9 | 0.200 & 0.6680 & 0.5330 & 0.5674 \\ 10 | 0.225 & 0.7140 & 0.5697 & 0.6064 \\ 11 | 0.250 & 0.7585 & 0.6052 & 0.6442 \\ 12 | 0.275 & 0.8020 & 0.6399 & 0.6811 \\ 13 | 0.300 & 0.8446 & 0.6739 & 0.7173 \\ 14 | 0.325 & 0.8866 & 0.7074 & 0.7530 \\ 15 | 0.350 & 0.9282 & 0.7406 & 0.7883 \\ 16 | 0.375 & 0.9695 & 0.7736 & 0.8235 \\ 17 | 0.400 & 1.0108 & 0.8065 & 0.8585 \\ 18 | 0.425 & 1.0520 & 0.8394 & 0.8935 \\ 19 | 0.450 & 1.0935 & 0.8725 & 0.9287 \\ 20 | 0.475 & 1.1352 & 0.9058 & 0.9642 \\ 21 | 0.500 & 1.1774 & 0.9394 & 1.0000 \\ 22 | 0.525 & 1.2202 & 0.9736 & 1.0363 \\ 23 | 0.550 & 1.2637 & 1.0083 & 1.0733 \\ 24 | 0.575 & 1.3082 & 1.0438 & 1.1111 \\ 25 | 0.600 & 1.3537 & 1.0801 & 1.1498 \\ 26 | 0.625 & 1.4006 & 1.1175 & 1.1896 \\ 27 | 0.650 & 1.4490 & 1.1561 & 1.2307 \\ 28 | 0.675 & 1.4993 & 1.1963 & 1.2734 \\ 29 | 0.700 & 1.5518 & 1.2381 & 1.3179 \\ 30 | 0.725 & 1.6069 & 1.2821 & 1.3647 \\ 31 | 0.750 & 1.6651 & 1.3286 & 1.4142 \\ 32 | 0.775 & 1.7272 & 1.3781 & 1.4670 \\ 33 | 0.800 & 1.7941 & 1.4315 & 1.5238 \\ 34 | 0.825 & 1.8671 & 1.4897 & 1.5857 \\ 35 | 0.850 & 1.9479 & 1.5542 & 1.6544 \\ 36 | 0.875 & 2.0393 & 1.6272 & 1.7321 \\ 37 | 0.900 & 2.1460 & 1.7122 & 1.8226 \\ 38 | 0.925 & 2.2761 & 1.8160 & 1.9331 \\ 39 | 0.950 & 2.4477 & 1.9530 & 2.0789 \\ 40 | 0.975 & 2.7162 & 2.1672 & 2.3069 \\ 41 | 0.990 & 3.0349 & 2.4215 & 2.5776 \\ 42 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/cf4b.dat: -------------------------------------------------------------------------------- 1 | 0.0 & 0.0000 & 0.0000 & 0.0000 \\ 2 | 0.1 & 0.0050 & 0.0078 & 0.0069 \\ 3 | 0.2 & 0.0198 & 0.0309 & 0.0273 \\ 4 | 0.3 & 0.0440 & 0.0682 & 0.0605 \\ 5 | 0.4 & 0.0769 & 0.1181 & 0.1050 \\ 6 | 0.5 & 0.1175 & 0.1783 & 0.1591 \\ 7 | 0.6 & 0.1647 & 0.2463 & 0.2208 \\ 8 | 0.7 & 0.2173 & 0.3194 & 0.2880 \\ 9 | 0.8 & 0.2739 & 0.3951 & 0.3583 \\ 10 | 0.9 & 0.3330 & 0.4707 & 0.4296 \\ 11 | 1.0 & 0.3935 & 0.5441 & 0.5000 \\ 12 | 1.1 & 0.4539 & 0.6134 & 0.5677 \\ 13 | 1.2 & 0.5132 & 0.6773 & 0.6314 \\ 14 | 1.3 & 0.5704 & 0.7348 & 0.6901 \\ 15 | 1.4 & 0.6247 & 0.7855 & 0.7430 \\ 16 | 1.5 & 0.6753 & 0.8292 & 0.7898 \\ 17 | 1.6 & 0.7220 & 0.8661 & 0.8304 \\ 18 | 1.7 & 0.7643 & 0.8967 & 0.8651 \\ 19 | 1.8 & 0.8021 & 0.9215 & 0.8942 \\ 20 | 1.9 & 0.8355 & 0.9413 & 0.9181 \\ 21 | 2.0 & 0.8647 & 0.9568 & 0.9375 \\ 22 | 2.1 & 0.8897 & 0.9687 & 0.9530 \\ 23 | 2.2 & 0.9111 & 0.9777 & 0.9651 \\ 24 | 2.3 & 0.9290 & 0.9843 & 0.9744 \\ 25 | 2.4 & 0.9439 & 0.9892 & 0.9815 \\ 26 | 2.5 & 0.9561 & 0.9926 & 0.9869 \\ 27 | 2.6 & 0.9660 & 0.9951 & 0.9908 \\ 28 | 2.7 & 0.9739 & 0.9967 & 0.9936 \\ 29 | 2.8 & 0.9802 & 0.9979 & 0.9956 \\ 30 | 2.9 & 0.9851 & 0.9986 & 0.9971 \\ 31 | 3.0 & 0.9889 & 0.9991 & 0.9980 \\ 32 | 3.1 & 0.9918 & 0.9995 & 0.9987 \\ 33 | 3.2 & 0.9940 & 0.9997 & 0.9992 \\ 34 | 3.3 & 0.9957 & 0.9998 & 0.9995 \\ 35 | 3.4 & 0.9969 & 0.9999 & 0.9997 \\ 36 | 3.5 & 0.9978 & 0.9999 & 0.9998 \\ 37 | 3.6 & 0.9985 & 1.0000 & 0.9999 \\ 38 | 3.7 & 0.9989 & 1.0000 & 0.9999 \\ 39 | 3.8 & 0.9993 & 1.0000 & 1.0000 \\ 40 | 3.9 & 0.9995 & 1.0000 & 1.0000 \\ 41 | 4.0 & 0.9997 & 1.0000 & 1.0000 \\ 42 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/nf5a.dat: -------------------------------------------------------------------------------- 1 | 0.000 & 0.0000 & 0.0000 & 0.0000 \\ 2 | 0.025 & 0.0313 & 0.0393 & 0.0465 \\ 3 | 0.050 & 0.0627 & 0.0786 & 0.0930 \\ 4 | 0.075 & 0.0941 & 0.1180 & 0.1396 \\ 5 | 0.100 & 0.1257 & 0.1575 & 0.1863 \\ 6 | 0.125 & 0.1573 & 0.1972 & 0.2332 \\ 7 | 0.150 & 0.1891 & 0.2370 & 0.2804 \\ 8 | 0.175 & 0.2211 & 0.2771 & 0.3278 \\ 9 | 0.200 & 0.2533 & 0.3175 & 0.3756 \\ 10 | 0.225 & 0.2858 & 0.3582 & 0.4238 \\ 11 | 0.250 & 0.3186 & 0.3994 & 0.4724 \\ 12 | 0.275 & 0.3518 & 0.4409 & 0.5216 \\ 13 | 0.300 & 0.3853 & 0.4829 & 0.5713 \\ 14 | 0.325 & 0.4193 & 0.5255 & 0.6216 \\ 15 | 0.350 & 0.4538 & 0.5687 & 0.6727 \\ 16 | 0.375 & 0.4888 & 0.6126 & 0.7247 \\ 17 | 0.400 & 0.5244 & 0.6572 & 0.7775 \\ 18 | 0.425 & 0.5607 & 0.7027 & 0.8313 \\ 19 | 0.450 & 0.5978 & 0.7492 & 0.8862 \\ 20 | 0.475 & 0.6357 & 0.7967 & 0.9424 \\ 21 | 0.500 & 0.6745 & 0.8453 & 1.0000 \\ 22 | 0.525 & 0.7144 & 0.8953 & 1.0591 \\ 23 | 0.550 & 0.7554 & 0.9468 & 1.1200 \\ 24 | 0.575 & 0.7978 & 0.9999 & 1.1828 \\ 25 | 0.600 & 0.8416 & 1.0548 & 1.2478 \\ 26 | 0.625 & 0.8871 & 1.1119 & 1.3153 \\ 27 | 0.650 & 0.9346 & 1.1713 & 1.3856 \\ 28 | 0.675 & 0.9842 & 1.2336 & 1.4592 \\ 29 | 0.700 & 1.0364 & 1.2990 & 1.5366 \\ 30 | 0.725 & 1.0916 & 1.3681 & 1.6184 \\ 31 | 0.750 & 1.1503 & 1.4417 & 1.7055 \\ 32 | 0.775 & 1.2133 & 1.5207 & 1.7989 \\ 33 | 0.800 & 1.2816 & 1.6062 & 1.9000 \\ 34 | 0.825 & 1.3563 & 1.6999 & 2.0109 \\ 35 | 0.850 & 1.4395 & 1.8042 & 2.1343 \\ 36 | 0.875 & 1.5341 & 1.9227 & 2.2745 \\ 37 | 0.900 & 1.6449 & 2.0615 & 2.4387 \\ 38 | 0.925 & 1.7805 & 2.2315 & 2.6397 \\ 39 | 0.950 & 1.9600 & 2.4565 & 2.9058 \\ 40 | 0.975 & 2.2414 & 2.8092 & 3.3231 \\ 41 | 0.990 & 2.5758 & 3.2283 & 3.8189 \\ 42 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/nf5b.dat: -------------------------------------------------------------------------------- 1 | 0.0 & 0.0000 & 0.0000 & 0.0000 \\ 2 | 0.1 & 0.0797 & 0.0636 & 0.0538 \\ 3 | 0.2 & 0.1585 & 0.1268 & 0.1073 \\ 4 | 0.3 & 0.2358 & 0.1892 & 0.1604 \\ 5 | 0.4 & 0.3108 & 0.2504 & 0.2127 \\ 6 | 0.5 & 0.3829 & 0.3101 & 0.2641 \\ 7 | 0.6 & 0.4515 & 0.3679 & 0.3143 \\ 8 | 0.7 & 0.5161 & 0.4235 & 0.3632 \\ 9 | 0.8 & 0.5763 & 0.4767 & 0.4105 \\ 10 | 0.9 & 0.6319 & 0.5273 & 0.4562 \\ 11 | 1.0 & 0.6827 & 0.5751 & 0.5000 \\ 12 | 1.1 & 0.7287 & 0.6199 & 0.5419 \\ 13 | 1.2 & 0.7699 & 0.6617 & 0.5817 \\ 14 | 1.3 & 0.8064 & 0.7004 & 0.6194 \\ 15 | 1.4 & 0.8385 & 0.7360 & 0.6550 \\ 16 | 1.5 & 0.8664 & 0.7686 & 0.6883 \\ 17 | 1.6 & 0.8904 & 0.7983 & 0.7195 \\ 18 | 1.7 & 0.9109 & 0.8250 & 0.7485 \\ 19 | 1.8 & 0.9281 & 0.8491 & 0.7753 \\ 20 | 1.9 & 0.9426 & 0.8705 & 0.8000 \\ 21 | 2.0 & 0.9545 & 0.8895 & 0.8227 \\ 22 | 2.1 & 0.9643 & 0.9062 & 0.8433 \\ 23 | 2.2 & 0.9722 & 0.9208 & 0.8622 \\ 24 | 2.3 & 0.9786 & 0.9335 & 0.8792 \\ 25 | 2.4 & 0.9836 & 0.9445 & 0.8945 \\ 26 | 2.5 & 0.9876 & 0.9539 & 0.9082 \\ 27 | 2.6 & 0.9907 & 0.9620 & 0.9205 \\ 28 | 2.7 & 0.9931 & 0.9688 & 0.9314 \\ 29 | 2.8 & 0.9949 & 0.9745 & 0.9411 \\ 30 | 2.9 & 0.9963 & 0.9793 & 0.9495 \\ 31 | 3.0 & 0.9973 & 0.9833 & 0.9570 \\ 32 | 3.1 & 0.9981 & 0.9866 & 0.9635 \\ 33 | 3.2 & 0.9986 & 0.9893 & 0.9691 \\ 34 | 3.3 & 0.9990 & 0.9915 & 0.9740 \\ 35 | 3.4 & 0.9993 & 0.9933 & 0.9782 \\ 36 | 3.5 & 0.9995 & 0.9948 & 0.9818 \\ 37 | 3.6 & 0.9997 & 0.9959 & 0.9848 \\ 38 | 3.7 & 0.9998 & 0.9968 & 0.9874 \\ 39 | 3.8 & 0.9999 & 0.9976 & 0.9896 \\ 40 | 3.9 & 0.9999 & 0.9981 & 0.9915 \\ 41 | 4.0 & 0.9999 & 0.9986 & 0.9930 \\ 42 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/sf5a.dat: -------------------------------------------------------------------------------- 1 | 0.000 & 0.0000 & 0.0000 & 0.0000 \\ 2 | 0.025 & 0.4645 & 0.2911 & 0.3020 \\ 3 | 0.050 & 0.5931 & 0.3717 & 0.3856 \\ 4 | 0.075 & 0.6870 & 0.4305 & 0.4466 \\ 5 | 0.100 & 0.7644 & 0.4790 & 0.4970 \\ 6 | 0.125 & 0.8321 & 0.5214 & 0.5409 \\ 7 | 0.150 & 0.8932 & 0.5597 & 0.5807 \\ 8 | 0.175 & 0.9496 & 0.5951 & 0.6174 \\ 9 | 0.200 & 1.0026 & 0.6283 & 0.6518 \\ 10 | 0.225 & 1.0529 & 0.6598 & 0.6845 \\ 11 | 0.250 & 1.1012 & 0.6900 & 0.7159 \\ 12 | 0.275 & 1.1478 & 0.7193 & 0.7462 \\ 13 | 0.300 & 1.1932 & 0.7477 & 0.7757 \\ 14 | 0.325 & 1.2377 & 0.7756 & 0.8046 \\ 15 | 0.350 & 1.2812 & 0.8029 & 0.8330 \\ 16 | 0.375 & 1.3244 & 0.8299 & 0.8610 \\ 17 | 0.400 & 1.3672 & 0.8568 & 0.8888 \\ 18 | 0.425 & 1.4098 & 0.8835 & 0.9165 \\ 19 | 0.450 & 1.4524 & 0.9102 & 0.9442 \\ 20 | 0.475 & 1.4951 & 0.9369 & 0.9720 \\ 21 | 0.500 & 1.5382 & 0.9639 & 1.0000 \\ 22 | 0.525 & 1.5817 & 0.9912 & 1.0283 \\ 23 | 0.550 & 1.6257 & 1.0188 & 1.0569 \\ 24 | 0.575 & 1.6706 & 1.0469 & 1.0861 \\ 25 | 0.600 & 1.7164 & 1.0756 & 1.1159 \\ 26 | 0.625 & 1.7635 & 1.1051 & 1.1465 \\ 27 | 0.650 & 1.8119 & 1.1355 & 1.1780 \\ 28 | 0.675 & 1.8621 & 1.1669 & 1.2106 \\ 29 | 0.700 & 1.9144 & 1.1997 & 1.2446 \\ 30 | 0.725 & 1.9691 & 1.2340 & 1.2802 \\ 31 | 0.750 & 2.0269 & 1.2702 & 1.3177 \\ 32 | 0.775 & 2.0884 & 1.3087 & 1.3577 \\ 33 | 0.800 & 2.1544 & 1.3501 & 1.4007 \\ 34 | 0.825 & 2.2264 & 1.3952 & 1.4474 \\ 35 | 0.850 & 2.3059 & 1.4450 & 1.4991 \\ 36 | 0.875 & 2.3957 & 1.5013 & 1.5575 \\ 37 | 0.900 & 2.5003 & 1.5668 & 1.6255 \\ 38 | 0.925 & 2.6277 & 1.6466 & 1.7083 \\ 39 | 0.950 & 2.7955 & 1.7518 & 1.8174 \\ 40 | 0.975 & 3.0575 & 1.9160 & 1.9878 \\ 41 | 0.990 & 3.3682 & 2.1107 & 2.1897 \\ 42 | -------------------------------------------------------------------------------- /matlab/stats/HsuSpatialError/sf5b.dat: -------------------------------------------------------------------------------- 1 | 0.0 & 0.0000 & 0.0000 & 0.0000 \\ 2 | 0.1 & 0.0003 & 0.0011 & 0.0010 \\ 3 | 0.2 & 0.0021 & 0.0084 & 0.0075 \\ 4 | 0.3 & 0.0070 & 0.0273 & 0.0245 \\ 5 | 0.4 & 0.0162 & 0.0613 & 0.0554 \\ 6 | 0.5 & 0.0309 & 0.1120 & 0.1016 \\ 7 | 0.6 & 0.0516 & 0.1786 & 0.1629 \\ 8 | 0.7 & 0.0789 & 0.2584 & 0.2372 \\ 9 | 0.8 & 0.1128 & 0.3473 & 0.3210 \\ 10 | 0.9 & 0.1529 & 0.4405 & 0.4101 \\ 11 | 1.0 & 0.1987 & 0.5331 & 0.5000 \\ 12 | 1.1 & 0.2494 & 0.6207 & 0.5867 \\ 13 | 1.2 & 0.3038 & 0.7003 & 0.6670 \\ 14 | 1.3 & 0.3608 & 0.7695 & 0.7384 \\ 15 | 1.4 & 0.4192 & 0.8275 & 0.7996 \\ 16 | 1.5 & 0.4778 & 0.8745 & 0.8504 \\ 17 | 1.6 & 0.5355 & 0.9111 & 0.8911 \\ 18 | 1.7 & 0.5911 & 0.9387 & 0.9227 \\ 19 | 1.8 & 0.6439 & 0.9589 & 0.9466 \\ 20 | 1.9 & 0.6932 & 0.9732 & 0.9639 \\ 21 | 2.0 & 0.7385 & 0.9829 & 0.9763 \\ 22 | 2.1 & 0.7795 & 0.9895 & 0.9848 \\ 23 | 2.2 & 0.8161 & 0.9937 & 0.9905 \\ 24 | 2.3 & 0.8482 & 0.9963 & 0.9942 \\ 25 | 2.4 & 0.8761 & 0.9979 & 0.9965 \\ 26 | 2.5 & 0.8999 & 0.9988 & 0.9980 \\ 27 | 2.6 & 0.9200 & 0.9994 & 0.9989 \\ 28 | 2.7 & 0.9368 & 0.9997 & 0.9994 \\ 29 | 2.8 & 0.9506 & 0.9998 & 0.9997 \\ 30 | 2.9 & 0.9617 & 0.9999 & 0.9998 \\ 31 | 3.0 & 0.9707 & 1.0000 & 0.9999 \\ 32 | 3.1 & 0.9778 & 1.0000 & 1.0000 \\ 33 | 3.2 & 0.9834 & 1.0000 & 1.0000 \\ 34 | 3.3 & 0.9877 & 1.0000 & 1.0000 \\ 35 | 3.4 & 0.9909 & 1.0000 & 1.0000 \\ 36 | 3.5 & 0.9934 & 1.0000 & 1.0000 \\ 37 | 3.6 & 0.9953 & 1.0000 & 1.0000 \\ 38 | 3.7 & 0.9966 & 1.0000 & 1.0000 \\ 39 | 3.8 & 0.9976 & 1.0000 & 1.0000 \\ 40 | 3.9 & 0.9984 & 1.0000 & 1.0000 \\ 41 | 4.0 & 0.9989 & 1.0000 & 1.0000 \\ 42 | -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateFLE/runEstimateFLEDynamicSingleRun.m: -------------------------------------------------------------------------------- 1 | %runEstimateFLEDynamic 2 | %email_setup; 3 | cd E:\docs\research\phd\experiments\FLEPrediction\patent-plots 4 | %cd E:\awiles\data\FLEPrediction 5 | %% 1. Tool design. 6 | %body = 'west'; 7 | body = 'west2'; 8 | %body = 'ta003-4'; 9 | %get the rigid body design. 10 | switch(body) 11 | case 'west' 12 | casename = 'WestTool'; 13 | [refmrk, normals, tip] = getWestToolDesign('e', 71, 54, 85); 14 | case 'west2' 15 | casename = 'WestTool2'; 16 | [refmrk, normals, tip] = getWestToolDesign('e', 71, 54, 85); 17 | refmrk = [refmrk; 0, 0, 5]; 18 | case 'ta003-4' 19 | casename = 'TA003-4'; 20 | refmrk = [0.0000, 0.0000, -0.0120;... 21 | -99.4066, -33.4878, 0.0417;... 22 | -153.6516, 0.2701, -0.0463;... 23 | -103.4595, 49.1463, 0.0287]; 24 | % refmrk = [0.0000, 0.0000, 0.0;... 25 | % -99.4066, -33.4878, 0.0;... 26 | % -153.6516, 0.2701, 0.0;... 27 | % -103.4595, 49.1463, 0.0]; 28 | tip = [200, 0, 0]; 29 | otherwise 30 | error('Invalid body design.'); 31 | end 32 | tooldesign.name = casename; 33 | tooldesign.refmrk = refmrk; 34 | tooldesign.tip = tip; 35 | [U, L, tooldesign.V0, x, tooldesign.A, tooldesign.Ainv] = xfrmToPA(refmrk); 36 | 37 | %% 2. FLE Model. 38 | model = 'quad1'; 39 | switch(model) 40 | case 'quad1' 41 | flemodel.name = 'RadialQuadratic500um'; 42 | flemodel.model = 'radial'; 43 | flemodel.rate = [5.7e-9, 5.7e-9, 5.1e-8]; 44 | flemodel.heteroscedastic = 0; 45 | case 'quad2' 46 | flemodel.name = 'RadialQuadratic500umHS'; 47 | flemodel.model = 'radial'; 48 | flemodel.rate = [5.7e-9, 5.7e-9, 5.1e-8]; 49 | flemodel.heteroscedastic = 1; 50 | otherwise 51 | error('Invalid FLE model given.'); 52 | end 53 | %% 3. Tool Path. 54 | path = 1; 55 | filename = sprintf('path-%02d', path); 56 | load(filename); 57 | toolpath.name = filename; 58 | toolpath.xfrm = xfrm; 59 | 60 | %% 4. Window Size. 61 | winsize = 500; 62 | 63 | %% set up the simulation. 64 | N = length(xfrm); 65 | casename = sprintf('%s-%s-%s-%03d', tooldesign.name, flemodel.name,... 66 | toolpath.name, winsize); 67 | 68 | %% Save the parameters. 69 | parmfilename = sprintf('%s-parameters', casename); 70 | save(parmfilename, 'tooldesign', 'flemodel', 'toolpath', 'winsize'); 71 | 72 | %% run the simulation 73 | [data, stats] = estimateFLEDynamic(tooldesign, flemodel, toolpath, winsize, 1001, 1); 74 | stats4 = stats; -------------------------------------------------------------------------------- /matlab/target_error/misc/TREtestNonUniform.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | 3 | %% set up the tool parameters. 4 | 5 | % define the rigid body defintion in local probe coordinates. 6 | 7 | %x = [0 0 0; 8 | % 0 18.61 -36.95; 9 | % 0 47.98 63.97]; 10 | 11 | 12 | % West design in Fig. 2(e). 13 | A = 71; 14 | B = 54; 15 | rho = 170; 16 | 17 | x = [ -A/2 B/2 0; ... 18 | -A/2 -B/2 0; ... 19 | A/2 -B/2 0; ... 20 | A/2 B/2 0 ]; 21 | 22 | % in drawing space, tip is located at 23 | %tip0 = [-175 0 90]; 24 | tip0 = [ rho 0 0 ]; 25 | 26 | % random fiducials 27 | probe = generateRandFiducialTargets(3, 100, 200); 28 | x = probe.Rigid.mrk 29 | tip0 = probe.Rigid.tip 30 | 31 | % x = [26.1085 34.4732 -10.4158; ... 32 | % 29.8527 -44.1469 -35.7065; ... 33 | % 1.4457 -8.9735 -35.2046 ]; 34 | % tip0 = [ -68.2192 39.5070 4.6551]; 35 | 36 | % now that the probe is created, let's rotate the probe into a useful 37 | % orientation. 38 | rotZ = 15*randn(1); 39 | R = getRotMatrixd([0, 0, rotZ]); 40 | x = (R * x')'; 41 | tip0 = (R * tip0')'; 42 | 43 | % define an arbitrary FLE error value where each axis is identical. 44 | FLE1 = 0.33; 45 | [TRE(1) cov covPA] = calcTRE(FLE1,[x;tip0], 0); 46 | fprintf('Test 1: TRE = %f\n', TRE(1)); 47 | cov 48 | 49 | FLE2 = (0.33)^2/3*eye(3); 50 | [TRE(2) cov covPA] = calcTRE(FLE2,[x;tip0], 0); 51 | fprintf('Test 2: TRE = %f\n', TRE(2)); 52 | cov 53 | 54 | FLE0 = (0.33)^2/3*eye(3); 55 | FLE3 = zeros(3,3,size(x,1)); 56 | for i = 1:size(x,1) 57 | FLE3(:,:,i) = FLE0; 58 | end 59 | FLE3; 60 | [TRE(3) cov covPA] = calcTRE(FLE3,[x;tip0], 0); 61 | fprintf('Test 3: TRE = %f\n', TRE(3)); 62 | cov 63 | 64 | 65 | FLE0 = (0.33)^2/3*eye(3); 66 | FLE4 = zeros(3,3,size(x,1)); 67 | for i = 1:size(x,1) 68 | FLE4(:,:,i) = 0.5 *i * FLE0; 69 | end 70 | [TRE(4) cov covPA] = calcTRE(FLE4,[x;tip0], 0); 71 | test.mrk = x; 72 | test.tip = tip0; 73 | [probe.Meas.error, probe.Meas.tip] = simTRE(FLE4, 100000, test, test ); 74 | probe = computeStats(probe); 75 | fprintf('Test 4: TRE_t = %f, TRE_m = %f\n', TRE(4), probe.Meas.stats.RMS); 76 | fprintf('Test #4 - theory\n'); 77 | cov 78 | fprintf('Test #4 - simulation\n'); 79 | probe.Meas.stats.Sigma 80 | 81 | FLE5 = weightMatrix(0.33^2, [1 1 3]); 82 | [TRE(5) cov covPA] = calcTRE(FLE5,[x;tip0], 0); 83 | fprintf('Test 5: TRE = %f\n', TRE(5)); 84 | cov 85 | 86 | FLE6 = zeros(3,3,size(x,1)); 87 | for i = 1:size(x,1) 88 | FLE6(:,:,i) = weightMatrix(0.33^2, [1 1 3]); 89 | end 90 | [TRE(6) cov covPA] = calcTRE(FLE6,[x;tip0], 0); 91 | fprintf('Test 6: TRE = %f\n', TRE(6)); 92 | cov 93 | 94 | FLE7 = mean(FLE6,3); 95 | [TRE(7) cov covPA] = calcTRE(FLE7,[x;tip0], 0); 96 | fprintf('Test 7: TRE = %f\n', TRE(7)); 97 | cov 98 | -------------------------------------------------------------------------------- /matlab/stats/get3DErrorStats.m: -------------------------------------------------------------------------------- 1 | function errPos = get3DErrorStats(refPos, measPos, varargin) 2 | %get3DErrorStats Compute the vector stats for a point cloud. 3 | % refPos - N x 3 matrix of reference positions. 4 | % measPos - N x 3 matrix of the measured positions. 5 | 6 | % default values for optional variables. 7 | bFit = 0; 8 | bPlot = 0; 9 | verbose = 0; 10 | 11 | if( nargin > 2 ) 12 | nVarArgs = length(varargin); 13 | i = 1; 14 | while( i <= nVarArgs ) 15 | if( strcmp(varargin{i}, 'BestFit')) 16 | bFit = 1; 17 | elseif( strcmp(varargin{i}, 'Plot')) 18 | bPlot = 1; 19 | elseif( strcmp(varargin{i}, 'Verbose')) 20 | verbose = 1; 21 | else 22 | warning('Unknown parameter: %s -- Ignoring it.', varargin{i}) 23 | end 24 | i=i+1; 25 | end 26 | end 27 | 28 | if( size(refPos) ~= size(measPos) ) 29 | errString = sprintf( 'The number of reference positions is different than the number of measured positions.\n'); 30 | error(errString ); 31 | end 32 | 33 | if( bFit ) 34 | [xfrm, refPos0, rmsError] = TransformRigidBody(refPos, measPos); 35 | else 36 | refPos0 = refPos; 37 | end 38 | 39 | errPos = measPos - refPos0; 40 | errDist = (sum(errPos.^2,2)).^(.5); 41 | 42 | %% mean errors 43 | 44 | meanErr = mean([errPos errDist]); 45 | stdErr = std([errPos errDist]); 46 | rmsErr = sqrt(mean([errPos errDist].^2)); 47 | maxErr = max([errPos errDist]); 48 | covErr = cov(errPos); 49 | 50 | fprintf('\tRMS\t\tMean\tSt.Dev\tMax\t\t2.5%%\t97.5%%\t95.0%%\n'); 51 | fprintf('x\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\tN/A\n', rmsErr(1), meanErr(1), stdErr(1), maxErr(1), getPercentile(errPos(:,1),0.025), getPercentile(errPos(:,1),0.975) ); 52 | fprintf('y\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\tN/A\n', rmsErr(2), meanErr(2), stdErr(2), maxErr(2), getPercentile(errPos(:,2),0.025), getPercentile(errPos(:,2),0.975) ); 53 | fprintf('z\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\tN/A\n', rmsErr(3), meanErr(3), stdErr(3), maxErr(3), getPercentile(errPos(:,3),0.025), getPercentile(errPos(:,3),0.975) ); 54 | fprintf('3D\t%3.2f\t%3.2f\t%3.2f\t%3.2f\tN/A \tN/A \t%3.2f\n', rmsErr(4), meanErr(4), stdErr(4), maxErr(4), getPercentile(errDist, 0.95) ); 55 | fprintf('\nCovariance Matrix:\n'); 56 | fprintf('% 3.2f\t% 3.2f\t% 3.2f\n', covErr); 57 | 58 | if( bFit ) 59 | fprintf('The data was registered for a best fit set of statistics.\n'); 60 | fprintf(' Xfrm: %3.6f, %3.6f, %3.6f, %3.6f, %3.2f, %3.2f, %3.2f\n', ... 61 | xfrm.rot, xfrm.pos); 62 | end 63 | 64 | if( bPlot ) 65 | plot3(refPos0(:,1), refPos0(:,2), refPos0(:,3), '.k'); 66 | hold on; 67 | plot3(measPos(:,1), measPos(:,2), measPos(:,3), '.r'); 68 | hold off; 69 | legend({'Reference','Measured'}); 70 | end 71 | end -------------------------------------------------------------------------------- /matlab/math/registration/getRigidXfrm.m: -------------------------------------------------------------------------------- 1 | function xfrm = getRigidXfrm( refPos, measPos ) 2 | % computes the rigid body transform for the meas positions. 3 | % based on the Horn algorithm: Horn, BKP. Closed-form solution of absolute 4 | % orientation using unit quarternions. J. Opt. Soc. Am. A 4(4):629-642. 5 | % April 1987. 6 | %#eml 7 | nRefMarkers = size(refPos, 1); 8 | nMeasMarkers = size(measPos, 1); 9 | if( nRefMarkers ~= nMeasMarkers) 10 | error('getRigidXfrm: Number of reference markers is not equal to the number of measured markers.\n'); 11 | end 12 | 13 | % need to allocate for simulink. 14 | xfrm.rot = [1 0 0 0]; 15 | xfrm.pos = [0 0 0]; 16 | 17 | %% find the mean positions for both the reference and the measured. 18 | avgRefPos = mean(refPos); 19 | avgMeasPos = mean(measPos); 20 | 21 | %% "demean" the collection of points. 22 | dmRefPos = refPos - repmat(avgRefPos, size(refPos,1), 1); 23 | dmMeasPos = measPos - repmat(avgMeasPos, size(measPos,1), 1); 24 | 25 | %% compute the matrix sums of products 26 | 27 | % first the sums of products: 28 | % is this just dmRefPos'*dmMeasPos to give the appropriate 3x3 matrix? 29 | Sxx=0; Sxy=0; Syx=0; Sxz=0; Szx=0; Syy=0; Syz=0; Szy=0; Szz=0; 30 | for i = 1:nRefMarkers 31 | Sxx = Sxx + dmRefPos(i,1)*dmMeasPos(i,1); 32 | Sxy = Sxy + dmRefPos(i,1)*dmMeasPos(i,2); 33 | Syx = Syx + dmRefPos(i,2)*dmMeasPos(i,1); 34 | Sxz = Sxz + dmRefPos(i,1)*dmMeasPos(i,3); 35 | Szx = Szx + dmRefPos(i,3)*dmMeasPos(i,1); 36 | Syy = Syy + dmRefPos(i,2)*dmMeasPos(i,2); 37 | Syz = Syz + dmRefPos(i,2)*dmMeasPos(i,3); 38 | Szy = Szy + dmRefPos(i,3)*dmMeasPos(i,2); 39 | Szz = Szz + dmRefPos(i,3)*dmMeasPos(i,3); 40 | end 41 | 42 | % test question above. 43 | % M = dmRefPos' * dmMeasPos; 44 | % Sxx = M(1,1); Sxy = M(1,2); Sxz = M(1,3); 45 | % Syx = M(2,1); Syy = M(2,2); Syz = M(2,3); 46 | % Szx = M(3,1); Szy = M(3,2); Szz = M(3,3); 47 | 48 | % second build the N matrix. 49 | N = zeros(4,4); 50 | N(1,1) = Sxx + Syy + Szz; 51 | N(1,2) = Syz - Szy; N(2,1) = N(1,2); 52 | N(1,3) = Szx - Sxz; N(3,1) = N(1,3); 53 | N(1,4) = Sxy - Syx; N(4,1) = N(1,4); 54 | N(2,2) = Sxx - Syy - Szz; 55 | N(2,3) = Sxy + Syx; N(3,2) = N(2,3); 56 | N(2,4) = Szx + Sxz; N(4,2) = N(2,4); 57 | N(3,3) = -Sxx + Syy - Szz; 58 | N(3,4) = Syz + Szy; N(4,3) = N(3,4); 59 | N(4,4) = -Sxx - Syy + Szz; 60 | 61 | %% compute the eigenvalues and eigenvectors of the N matrix. 62 | [V,D] = eig(N); 63 | 64 | if( ~any(any(imag(V))) && ~any(any(imag(D)))) 65 | [maxEigVal, maxEigId] = max(diag(real(D))); 66 | maxEigVec = real(V(:,maxEigId)'); 67 | xfrm.rot = getQuatNormalized( maxEigVec ); 68 | else 69 | error('Complex eigenvalue decomposition.'); 70 | end 71 | 72 | %% compute the best fit transformation. 73 | rotAvgRefPos = getRotPointQuat( xfrm.rot, avgRefPos ); 74 | xfrm.pos = avgMeasPos - rotAvgRefPos; 75 | -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateTRE/readme.txt: -------------------------------------------------------------------------------- 1 | This file describes the experiments performed to validate the estimation of the TRE. 2 | 3 | The main script is masterTRESimulations.m which is actually a function with lots of command line options. Use the help to see all the command line options. 4 | 5 | There are four main experiments run by this script: 6 | 7 | 1. Test the TRE estimation formula for anisotropic FLE with random rigid fiducial configurations. Published in thesis and IEEE TMI 2008 paper. 8 | 9 | Matlab command to recreate data in thesis and paper: 10 | 11 | masterTRESimulations('DataPath', '', 'RandomFiducial', 1, 'NumSamples', 10^5, 'NumTrials', 1000, 'NumOrientations', 100 ); 12 | 13 | The other default values as described in help masterTRESimulations are maintained. 14 | 15 | 2. Test the TRE estimation formula for anisotropic FLE with a particular probe rigid body configuration. Published in thesis and IEEE TMI 2008 paper. 16 | 17 | Matlab command to recreate data in thesis and paper: 18 | 19 | masterTRESimulations('DataPath', '', 'RandomFiducial', 0,'RandomProbe', 1, 'SigmaIso', diag([1/27, 1/27, 1/27]), 'SigmaAniso', diag([1/99, 1/99, 1/11]), 'NumSamples', 10^5, 'NumOrientations', 1000, 'WestProbeDesign', {'d', 'e'}, 'WestProbe_rho', [85, 170]); 20 | 21 | The other default values as described in help masterTRESimulations are maintained. 22 | 23 | 3. Test the TRE estimation formula for anisotropic FLE with a particular probe and reference tool configuration. Published in MICCAI 2007 paper. 24 | 25 | Matlab command to recreate data in thesis and paper: 26 | 27 | masterTRESimulations('DataPath', '', 'RandomFiducial', 0,'RandomProbeRef', 1, 'SigmaIso', diag([1/27, 1/27, 1/27]), 'SigmaAniso', diag([1/99, 1/99, 1/11]), 'NumSamples', 10^5, 'NumOrientations', 1000, 'WestRef_r', [32, 64]); 28 | 29 | 4. Test the TRE estimation formula for non-homogenous anisotropic FLE with random rigid fiducial configurations. 30 | 31 | Matlab command to recreate data in thesis: 32 | 33 | 1. FLE Model 1 - radial model, see getFLEMatrix.m or pg. 112 of Wiles Thesis. 34 | masterTRESimulations('DataPath', '', 'RandomFiducial', 0,'RandomFiducialNH', 1, 'NumBodies', 100, 'NumTrials', 1000, 'NumOrientations', 10, 'NH_FLEModelType', 1, 'NH_FLEWeight', [1.0, 1.5, 3.0]) 35 | 36 | 2. FLE Model 2 - random model, see getFLEMatrix or pg. 112 of Wiles Thesis. 37 | masterTRESimulations('DataPath', '', 'RandomFiducial', 0,'RandomFiducialNH', 1, 'NumBodies', 100, 'NumTrials', 1000, 'NumOrientations', 10, 'NH_FLEModelType', 2); 38 | 39 | 3. FLE Model 3 - random model and rotated FLE, see getFLEMatrix or pg. 112 of Wiles Thesis. 40 | masterTRESimulations('DataPath', '', 'RandomFiducial', 0,'RandomFiducialNH', 1, 'NumBodies', 100, 'NumTrials', 1000, 'NumOrientations', 10, 'NH_FLEModelType', 3); 41 | -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateFLE/testRandomWalk.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | 4 | casestudy = 10; 5 | body = 'west'; 6 | %body = 'west2'; 7 | %body = 'ta003-4'; 8 | cd E:\docs\research\phd\experiments\FLEPrediction\simulated-tool-paths 9 | 10 | %% get the rigid body design 11 | switch(body) 12 | case 'west' 13 | casename = 'WestTool'; 14 | [refmrk, normals, tip] = getWestToolDesign('e', 71, 54, 85); 15 | case 'west2' 16 | casename = 'WestTool2'; 17 | [refmrk, normals, tip] = getWestToolDesign('e', 71, 54, 85); 18 | refmrk = [refmrk; 0, 0, 5]; 19 | case 'ta003-4' 20 | casename = 'TA003-4'; 21 | refmrk = [0.0000, 0.0000, -0.0120;... 22 | -99.4066, -33.4878, 0.0417;... 23 | -153.6516, 0.2701, -0.0463;... 24 | -103.4595, 49.1463, 0.0287]; 25 | % refmrk = [0.0000, 0.0000, 0.0;... 26 | % -99.4066, -33.4878, 0.0;... 27 | % -153.6516, 0.2701, 0.0;... 28 | % -103.4595, 49.1463, 0.0]; 29 | tip = [200, 0, 0]; 30 | otherwise 31 | error('Invalid body design.'); 32 | end 33 | nMrks = size(refmrk,1); 34 | %% pre-allocate memory and set up parameters. 35 | N = 12000; 36 | pos = zeros(N,3); 37 | rot = zeros(N,3,3); 38 | truemrks = cell(4); 39 | %xfrm = cell(N); 40 | for i = 1:nMrks 41 | truemrks{i} = zeros(N,3); 42 | end 43 | tooltip = zeros(N,3); 44 | curpos = [0, 0, -1500]; 45 | curxfrm.pos = curpos; 46 | curxfrm.R = eye(3); 47 | 48 | step = [0, 0, 1]; 49 | stepsize = 0.1; 50 | sigma = 0.01; 51 | e_sigma = 0.1; 52 | 53 | %% build a random walk path. 54 | i = 1; 55 | while i <= N 56 | % update the current pos. 57 | pos(i,:) = curpos; 58 | tooltip(i,:) = (curxfrm.R * tip')' + curxfrm.pos; 59 | mrk = (curxfrm.R * refmrk')' + repmat(curxfrm.pos, size(refmrk,1), 1); 60 | for j = 1:nMrks 61 | truemrks{j}(i,:) = mrk(j,:); 62 | end 63 | % save the current xfrm. 64 | xfrm{i} = curxfrm; 65 | 66 | % compute the next translation. 67 | lastpos = curpos; 68 | curpos = lastpos + stepsize*(step + sigma*randn(1,3)); 69 | diff = curpos - lastpos; 70 | step = diff/sqrt(diff*diff'); 71 | curxfrm.pos = curpos; 72 | % compute the next rotation. 73 | euler = e_sigma * randn(1,3); 74 | rot = getRotMatrixd(euler); 75 | curxfrm.R = rot*curxfrm.R; 76 | % increment the frame. 77 | i = i+1; 78 | end 79 | 80 | x = 2; 81 | y = 3; 82 | z = 1; 83 | plot3(pos(:,x), pos(:,y), pos(:,z), 'b-'); 84 | hold on; 85 | plot3(truemrks{1}(:,x), truemrks{1}(:,y), truemrks{1}(:,z), 'r-'); 86 | plot3(truemrks{2}(:,x), truemrks{2}(:,y), truemrks{2}(:,z), 'g-'); 87 | plot3(truemrks{3}(:,x), truemrks{3}(:,y), truemrks{3}(:,z), 'g-'); 88 | plot3(truemrks{4}(:,x), truemrks{4}(:,y), truemrks{4}(:,z), 'g-'); 89 | plot3(tooltip(:,x), tooltip(:,y), tooltip(:,z), 'y-'); 90 | for i = [1, 500:500:N] 91 | body = [truemrks{1}(i,:); truemrks{2}(i,:); truemrks{3}(i,:); truemrks{4}(i,:); truemrks{1}(i,:)]; 92 | plot3(body(:,x), body(:,y), body(:,z), 'k-', 'linewidth', 2); 93 | shaft = [pos(i,:); tooltip(i,:)]; 94 | plot3(shaft(:,x), shaft(:,y), shaft(:,z), 'k-', 'linewidth', 2); 95 | plot3(tooltip(i,x), tooltip(i,y), tooltip(i,z), 'k.', 'markersize', 12); 96 | end 97 | hold off; 98 | axis equal; 99 | xlabel('Y (mm)', 'fontsize', 18); 100 | ylabel('Z (mm)', 'fontsize', 18); 101 | zlabel('X (mm)', 'fontsize', 18); 102 | set(gca,'YDir','reverse'); 103 | set(gca,'ZDir','reverse'); 104 | set(gca, 'fontsize', 18); 105 | 106 | filename = sprintf('path-%02d', casestudy); 107 | save(filename, 'xfrm'); 108 | filename = sprintf('%s-path-%02d', casename, casestudy); 109 | print('-depsc2','-tiff','-r300', filename); 110 | -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateTRE/plotRandFiducialTargetExperiment.m: -------------------------------------------------------------------------------- 1 | function plotRandFiducialTargetExperiment(testname, datetime, fleModel, figFontSize, varargin) 2 | 3 | nToPlot = -1; 4 | verbose = 0; 5 | 6 | if( nargin > 4 ) 7 | nVarArgs = length(varargin); 8 | i = 1; 9 | while( i <= nVarArgs ) 10 | if( strcmp(varargin{i}, 'PlotDataSamples') ) 11 | i=i+1; 12 | nToPlot = varargin{i}; 13 | if( ~isscalar(nToPlot) && ~isvector(nToPlot) ) 14 | error('Value for PlotDataSamples is invalid'); 15 | end 16 | elseif( strcmp(varargin{i}, 'Verbose')) 17 | verbose = 1; 18 | else 19 | error('Unknown paramter: %s', varargin{i}); 20 | end 21 | i=i+1; 22 | end 23 | end 24 | 25 | cd(testname) 26 | cd(datetime) 27 | 28 | data = csvread('data.csv'); 29 | id = 1; 30 | covonly = 2; 31 | muandcov = 3; 32 | measRMS = 4; 33 | theoryRMS = 5; 34 | RMSDiff = 6; 35 | percentDiff = 7; 36 | 37 | load parm; 38 | 39 | plot(data(:,theoryRMS), data(:,measRMS), '.k'); 40 | hold on; 41 | minRMS = min(min(data(:,measRMS:theoryRMS))); 42 | maxRMS = max(max(data(:,measRMS:theoryRMS))); 43 | plot([minRMS maxRMS], [minRMS,maxRMS], 'k-'); 44 | hold off; 45 | 46 | xlabel('Predicted TRE RMS (mm)', 'fontsize',figFontSize); 47 | ylabel('Simulated TRE RMS (mm)', 'fontsize',figFontSize); 48 | set(gca,'fontsize',figFontSize); 49 | 50 | titlestring = sprintf('Predicted versus Simulated TRE RMS of Random Configurations'); 51 | subtitlestring = sprintf('%s FLE Model, RMS_{FLE} = %3.2f, %d Fiducials',... 52 | fleModel, sqrt(trace(parm.Sigma)), parm.nMarkers); 53 | title({titlestring, subtitlestring}, 'fontsize',figFontSize); 54 | 55 | axis([minRMS, maxRMS, minRMS, maxRMS]); 56 | figurefilename = sprintf('PredvMeas_%s', parm.name); 57 | %print('-depsc', '-tiff', '-r300', figurefilename); 58 | print('-dpng', '-r600', figurefilename); 59 | 60 | %copyfile('*.eps', 'E:\awiles\data\tretest\IEEE_Data\RandDesigns' ); 61 | 62 | %% lets plot the histograms for the distance error for each case. 63 | nTotalCount = parm.nBodies*parm.nTrials*parm.nOrientations*parm.nPositions; 64 | %histbins = 0.01:0.01:1 65 | 66 | if( isscalar(nToPlot) && nToPlot < 1 ) 67 | nToPlot = 1:nTotalCount; 68 | end 69 | 70 | for i = nToPlot 71 | filename = sprintf('data%06d.mat', i); 72 | fprintf('Plotting for %s ...\n', filename); 73 | load(filename); 74 | % generate histogram for theoterical data. 75 | 76 | % generate experimental histogram. 77 | disterr = sqrt(sum((probe.Meas.error).^2,2)); 78 | %mchistdata = hist(disterr, histbins); 79 | % plot each one. 80 | figure(2); 81 | [mu, sigma, rms, histbins] = plotCumHist(disterr,[], '.-k'); 82 | hold on; 83 | %plotMaxwellDist( testResult.probe.RMS.Meas, histbins, '-r'); 84 | plotRadius3DDist( probe.Meas.stats.Sigma, histbins, '-r'); 85 | probe.Meas.stats.Sigma; 86 | %plotMaxwellDist( testResult.probe.RMS.Theory, histbins, '-g'); 87 | plotRadius3DDist( probe.Actual.stats.Sigma, histbins, '-g'); 88 | probe.Actual.stats.Sigma; 89 | %plotMaxwellDist( testResult.probeAvgFLE.RMS.Theory, histbins, '-b'); 90 | %plotRadius3DDist( probe.AvgFLE.stats.Sigma, histbins, '-b'); 91 | %probe.AvgFLE.stats.Sigma; 92 | hold off; 93 | xlim([0, max(histbins)]); 94 | legend('Monte Carlo', 'Measured TRE', 'NH TRE'); 95 | titlestring = sprintf('TRE Distribution %06d', i); 96 | subtitlestring = sprintf('Meas. RMS: %3.2f, Theory RMS: %3.2f', ... 97 | testResult.probe.RMS.Meas, testResult.probe.RMS.Theory); 98 | title({titlestring, subtitlestring}); 99 | figurefilename = sprintf('hist_%s_%06d', parm.name, i); 100 | %print('-depsc', '-tiff', '-r300', figurefilename); 101 | print('-dpng', '-r600', figurefilename); 102 | end 103 | 104 | cd ../.. 105 | -------------------------------------------------------------------------------- /matlab/target_error/experiments/estimateFLE/runEstimateFLEDynamic.m: -------------------------------------------------------------------------------- 1 | %runEstimateFLEDynamic 2 | email_setup; 3 | %cd E:\docs\research\phd\experiments\FLEPrediction\simulated-tool-paths 4 | cd E:\awiles\data\FLEPrediction 5 | %% 1. Tool design. 6 | %body = 'west'; 7 | %body = 'west2'; 8 | body = 'ta003-4'; 9 | %get the rigid body design. 10 | switch(body) 11 | case 'west' 12 | casename = 'WestTool'; 13 | [refmrk, normals, tip] = getWestToolDesign('e', 71, 54, 85); 14 | case 'west2' 15 | casename = 'WestTool2'; 16 | [refmrk, normals, tip] = getWestToolDesign('e', 71, 54, 85); 17 | refmrk = [refmrk; 0, 0, 5]; 18 | case 'ta003-4' 19 | casename = 'TA003-4'; 20 | refmrk = [0.0000, 0.0000, -0.0120;... 21 | -99.4066, -33.4878, 0.0417;... 22 | -153.6516, 0.2701, -0.0463;... 23 | -103.4595, 49.1463, 0.0287]; 24 | % refmrk = [0.0000, 0.0000, 0.0;... 25 | % -99.4066, -33.4878, 0.0;... 26 | % -153.6516, 0.2701, 0.0;... 27 | % -103.4595, 49.1463, 0.0]; 28 | tip = [200, 0, 0]; 29 | otherwise 30 | error('Invalid body design.'); 31 | end 32 | tooldesign.name = casename; 33 | tooldesign.refmrk = refmrk; 34 | tooldesign.tip = tip; 35 | [U, L, tooldesign.V0, x, tooldesign.A, tooldesign.Ainv] = xfrmToPA(refmrk); 36 | 37 | %% 2. FLE Model. 38 | model = 'quad1'; 39 | switch(model) 40 | case 'quad1' 41 | flemodel.name = 'RadialQuadratic500um'; 42 | flemodel.model = 'radial'; 43 | flemodel.rate = [5.7e-9, 5.7e-9, 5.1e-8]; 44 | otherwise 45 | error('Invalide FLE model given.'); 46 | end 47 | %% 3. Tool Path. 48 | path = 3; 49 | filename = sprintf('path-%02d', path); 50 | load(filename); 51 | toolpath.name = filename; 52 | toolpath.xfrm = xfrm; 53 | 54 | %% 4. Window Size. 55 | winsize = 500; 56 | 57 | %% 5. Number of samples. 58 | M = 500; 59 | 60 | %% set up the simulation. 61 | N = length(xfrm); 62 | sumdata = zeros(7, N, M); 63 | casename = sprintf('%s-%s-%s-%03d', tooldesign.name, flemodel.name,... 64 | toolpath.name, winsize); 65 | mkdir(casename); 66 | cd(casename); 67 | 68 | %% Save the parameters. 69 | parmfilename = sprintf('%s-parameters', casename); 70 | save(parmfilename, 'tooldesign', 'flemodel', 'toolpath', 'winsize'); 71 | 72 | %% run the simulation M times. 73 | starttime = cputime; 74 | for i = 1:M 75 | data = estimateFLEDynamic(tooldesign, flemodel, toolpath, winsize, i, 1); 76 | sumdata(:, :, i) = [data{1}.estimated]; 77 | if(i==1) 78 | truedata = data{1}.true; 79 | end 80 | 81 | % compute estimated completion time. 82 | avgTime = (cputime - starttime)/i; %in seconds. 83 | estTimeToFinish = avgTime*(M-i); %in seconds. 84 | estHours = floor(estTimeToFinish/3600); 85 | estMinutes = floor((estTimeToFinish - 3600*estHours)/60); 86 | 87 | fprintf('Completed %3.1f %%... Avg. time per run: %4.2f seconds. Finish in approx. %d hours %d min \n',... 88 | (100*i/M), avgTime, estHours, estMinutes); 89 | if(rem(i,100) == 0 || i == 1) 90 | msg = sprintf('Completed %3.1f percent... Avg. time per run: %4.2f seconds. Finish in approx. %d hours %d min \n',... 91 | (100*i/M), avgTime, estHours, estMinutes); 92 | curlogfile = sprintf('%s-%04d.txt', casename, i); 93 | curRMSplot = sprintf('%s-%04d-estimatedFLE-AllMarkers.eps', casename, i); 94 | curRMSplot2 = sprintf('%s-%04d-estimatedFLE-Mrk01.eps', casename, i); 95 | sendmail('adwiles@gmail.com', 'FLE Simulation Update', msg, {curlogfile, curRMSplot, curRMSplot2}); 96 | end 97 | 98 | end 99 | 100 | save(casename, 'sumdata', 'truedata'); 101 | clear sumdata truedata; 102 | [logfilename, filenamerms] = plotEstimateFLEDynamicSummaryData(casename, {'Best', 'Best'}); 103 | sendmail('adwiles@gmail.com', 'FLE Simulation Complete', 'FLE Simulation Complete', {logfilename,filenamerms}); 104 | 105 | cd .. 106 | 107 | --------------------------------------------------------------------------------