diff options
| author | leochanj105 <leochanj@live.unc.edu> | 2020-10-19 23:09:30 -0400 |
|---|---|---|
| committer | leochanj105 <leochanj@live.unc.edu> | 2020-10-20 02:40:39 -0400 |
| commit | f618466c25d43f3bae9e40920273bf77de1e1149 (patch) | |
| tree | 460e739e2165b8a9c37a9c7ab1b60f5874903543 /SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m | |
| parent | 47ced4e96bbb782b9e780e8f2cfc637b2c21ff44 (diff) | |
initial sd-vbs
initial sd-vbs
add sd-vbs
sd-vbs
Diffstat (limited to 'SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m')
| -rw-r--r-- | SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m | 12 |
1 files changed, 12 insertions, 0 deletions
diff --git a/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m b/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m new file mode 100644 index 0000000..3d2ac76 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/addEulNoise.m | |||
| @@ -0,0 +1,12 @@ | |||
| 1 | function retQuat=addEulNoise(quat, STD) | ||
| 2 | n=size(quat,1); | ||
| 3 | |||
| 4 | % KVS: replacing randnWrapper(n,3) with rand(n,3) | ||
| 5 | gyro=randnWrapper(n,3)*STD; | ||
| 6 | |||
| 7 | norm_gyro=sqrt(sum(gyro.^2,2)); | ||
| 8 | angleAlpha=norm_gyro; | ||
| 9 | quatDelta=[cos(angleAlpha/2), gyro./(norm_gyro*ones(1,3)).*(sin(angleAlpha/2)*ones(1,3))]; | ||
| 10 | |||
| 11 | retQuat=quatMul(quat, quatDelta); | ||
| 12 | |||