From f618466c25d43f3bae9e40920273bf77de1e1149 Mon Sep 17 00:00:00 2001 From: leochanj105 Date: Mon, 19 Oct 2020 23:09:30 -0400 Subject: initial sd-vbs initial sd-vbs add sd-vbs sd-vbs --- SD-VBS/benchmarks/localization/src/matlab/mclWhole.m | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 SD-VBS/benchmarks/localization/src/matlab/mclWhole.m (limited to 'SD-VBS/benchmarks/localization/src/matlab/mclWhole.m') diff --git a/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m b/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m new file mode 100644 index 0000000..4e78b8f --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/mclWhole.m @@ -0,0 +1,18 @@ +function retW=mclWhole(quat, Ovel, STDDEV_GPSVel, accl, STDDEV_ACCL) +n=size(quat,1); +OvelNorm=norm(Ovel); +if (OvelNorm>0.5) + Ovel=Ovel/norm(Ovel); + %trick + %quat=addEulNoise(quat, STDDEV_GPSVel); + orgWorld=quatRot([1 0 0],quatConj(quat)); + p1=get3DGaussianProb(orgWorld, Ovel, eye(3)./STDDEV_GPSVel); +else + p1=zeros(n,1); +end + +gravity=quatRot(ones(n,1)*[0 0 -9.8], quat); +p2=get3DGaussianProb(gravity, accl, eye(3)./(STDDEV_ACCL)); +retW=p1+p2; +retW=retW/sum(retW); + \ No newline at end of file -- cgit v1.2.2