https://github.com/RomaTeng/EKF-SLAM-on-Manifold
Tip revision: 12d7d8d88c84161baed173e38d49dedb4adb2b96 authored by Teng Zhang on 26 February 2017, 15:09:17 UTC
Record my control paper
Record my control paper
Tip revision: 12d7d8d
compute_rms_nees_ave.m
function [rms, nees] = compute_rms_nees_ave( rmsall, neesall )
% compute rms and nees from multiple tests
nsteps = size(rmsall.position, 2);
ntests = size(rmsall.position, 1);
% compute rms
rmspos_sum = sum(rmsall.position.^2, 1);
rms.position = sqrt(rmspos_sum/ntests);
rmsori_sum = sum(rmsall.orientation.^2, 1);
rms.orientation = sqrt(rmsori_sum/ntests);
% compute nees
neesall_pose = neesall.pose(:, 2:end);
neesall_orientation = neesall.orientation(:, 2:end);
neespose_sum = sum(neesall_pose, 1);
nees.pose = neespose_sum/ntests;
neesorientation_sum = sum(neesall_orientation, 1);
nees.orientation = neesorientation_sum/ntests;