wzyn20051216/matlab-agent-skills / skills/matlab-robotics-autonomy
matlab-robotics-autonomy
VerifiedMATLAB robotics and autonomy workflow for rigid-body transforms, quaternions, forward/inverse kinematics, trajectory planning, rigid-body dynamics, ROS/ROS 2 bags, sensor fusion, state estimation, SLAM, and reproducible robot experiments. Use whenever the user asks MATLAB to verify a rotation or quaternion, compute FK/IK/Jacobians, check a trajectory against waypoints and limits, check dynamics (M/C/g, energy), or judge whether a recorded state estimate is consistent with its covariance. Missing toolbox, bag, robot model or hardware means SKIP, never a claimed result.
| File | Kind | Size |
|---|---|---|
| SKILL.md | skill md | 10.1 KB |
| references/robotics-verification-contract.md | markdown | 17.6 KB |
| scripts/mas_dyn_check.m | other | 11.9 KB |
| scripts/mas_ekf_check.m | other | 6.9 KB |
| scripts/mas_estimate_check.m | other | 13.2 KB |
| scripts/mas_frame_check.m | other | 8.6 KB |
| scripts/mas_kin_check.m | other | 4.5 KB |
| scripts/mas_kin_ik.m | other | 6.1 KB |
| scripts/mas_quat_check.m | other | 12.2 KB |
| scripts/mas_traj_check.m | other | 11.0 KB |
| tests/test_robotics.m | other | 13.5 KB |
| tests/test_robotics_contract.m | other | 25.1 KB |
scripts/mas_estimate_check.m
View on GitHubfunction r = mas_estimate_check(truth, est, P, claims)
%MAS_ESTIMATE_CHECK Is a recorded state estimate consistent with its own covariance?
%
% r = mas_estimate_check(truth, est, P)
% r = mas_estimate_check(truth, est, P, struct('alpha',0.05,'maxBias',0.1, ...
% 'innovation', nu, 'S', S))
%
% truth, est N x n (one run) or N x n x R (R independent runs)
% P n x n x N x R, n x n x N (one run), or n x n (constant)
%
% r.aneesPerStep mean NEES at each step (R runs)
% r.aneesMean average over the accepted steps; target n
% r.aneesBand two-sided chi-square band for the average over all accepted samples
% r.aneesStepBand chi-square band for one step's mean over runs (wider)
% r.inBandFrac fraction of steps inside the per-step band
% r.halfMeans [first-half second-half] mean ANEES, reported not graded
% r.minEigP smallest covariance eigenvalue over every step and run
% r.bias mean(truth - est) over the accepted samples
% r.maxAutocorr largest normalised innovation autocorrelation over the lags
%
% WHY MONTE CARLO RUNS ARE REQUIRED
% A single estimate sequence gives one NEES per step; that number is as noisy as the
% noise it measures, so "ANEES is 2.4, close to 2" from one run proves nothing. With
% R independent runs the average over samples is ~ chi2(R*(N-burnIn)*n)/(R*(N-burnIn)),
% which is where the mean band comes from; a single step's mean over runs follows the
% wider chi2(R*n)/R band. Fewer than `minRuns` (default 20) runs makes both bands so
% wide that they cannot reject a bad filter; the tool returns UNDETERMINED instead of
% pretending.
%
% The band is two-sided. A filter with ANEES far below n is not "better": it is
% under-confident, discarding information it was given. Both directions are defects.
%
% COVARIANCE HYGIENE IS CHECKED SEPARATELY
% Symmetry and positive definiteness are not implied by a small NEES at one step, and
% a covariance with a negative eigenvalue makes `P\e` meaningless (or negative), so
% those checks run before NEES and their failure is reported in its own right.
%
% INNOVATION WHITENESS
% When the innovations and their predicted covariance S are supplied, the normalised
% innovations should be white. A correlated sequence means the filter is not using
% the information it has (a lagged state, a missing input), even when the marginal
% covariance looks right. The threshold is family-wise over the tested lags,
% sqrt(2)*erfcinv(alpha/L)/sqrt(m*N_eff): the metric is the MAX of L autocorrelations,
% so a per-lag 95% bound would reject white noise roughly 40% of the time.
if nargin < 4, claims = struct(); end
[truth, est, P, nRuns, N, n] = normalize(truth, est, P);
if nRuns < 1, error('mas_estimate_check:runs', 'no runs in the input'); end
burnIn = getf(claims, 'burnIn', 0);
if burnIn < 0 || burnIn >= N
error('mas_estimate_check:burnIn', 'burnIn must be in [0, N)');
end
keep = (burnIn+1):N;
C = struct('id', {}, 'status', {}, 'detail', {});
if ~all(isfinite(truth(:))) || ~all(isfinite(est(:))) || ~all(isfinite(P(:)))
C = add(C, 'finiteInputs', false, ...
'truth, est or P contains NaN/Inf: the consistency statistic is undefined');
r = pack(C, struct(), N, n, nRuns, burnIn);
return
end
C = add(C, 'finiteInputs', true, 'truth, est and P are finite');
% ---- covariance hygiene ----------------------------------------------------
symErr = 0; minEig = Inf;
for k = keep
for rr = 1:nRuns
Pk = P(:,:,k,rr);
sc = max(1, max(abs(Pk(:))));
symErr = max(symErr, max(max(abs(Pk - Pk.')))/sc);
minEig = min(minEig, min(eig((Pk + Pk.')/2)));
end
end
C = add(C, 'covSymmetry', symErr <= 1e-8, sprintf( ...
'max relative |P-P''| = %.3e', symErr));
C = add(C, 'covPositiveDefinite', minEig > 0, sprintf( ...
'smallest eigenvalue over all steps/runs = %.3e (must be > 0)', minEig));
% ---- NEES ------------------------------------------------------------------
% Two bands, because two different statistics are being tested:
% band for the mean over ALL accepted samples, ~ chi2(n*nRuns*nStep)/(nRuns*nStep)
% stepBand for a single step's mean over runs, ~ chi2(n*nRuns)/nRuns
% Using the mean band on individual steps (or vice versa) makes a correct filter fail:
% with 40 runs the mean band is +/-0.1 while a per-step ANEES has sd ~0.32, so at most
% ~25% of steps could ever land inside it.
alpha = getf(claims, 'alpha', 0.01);
minRuns = getf(claims, 'minRuns', 20);
nStep = numel(keep);
aneesStep = zeros(nStep, 1);
for kk = 1:nStep
k = keep(kk);
s = 0;
for rr = 1:nRuns
e = truth(k,:,rr).' - est(k,:,rr).';
s = s + e.'*(P(:,:,k,rr)\e);
end
aneesStep(kk) = s / nRuns;
end
aneesMean = mean(aneesStep);
[nTot, band, bandSrc] = nees_band(n, nRuns*nStep, alpha);
[~, stepBand] = nees_band(n, nRuns, alpha);
if nRuns < minRuns
C = add(C, 'nees', NaN, sprintf([ ...
'%d run(s), minimum %d: the chi-square band would be too wide to reject a ' ...
'bad filter, so NEES is reported but not graded'], nRuns, minRuns));
inFrac = NaN;
elseif nStep < 5
C = add(C, 'nees', NaN, sprintf( ...
'only %d accepted step(s): too few for a chi-square band (need >= 5)', nStep));
inFrac = NaN;
else
inFrac = mean(aneesStep >= stepBand(1) & aneesStep <= stepBand(2));
ok = isfinite(aneesMean) && aneesMean >= band(1) && aneesMean <= band(2) && inFrac >= 0.90;
if aneesMean > band(2)
why = 'OVER-CONFIDENT: P is too small, so the filter trusts itself too much';
elseif aneesMean < band(1)
why = 'UNDER-CONFIDENT: P is too large, so the filter discards information';
elseif inFrac < 0.90
why = 'the mean is in band but per-step coverage is not: the inconsistency is time-varying';
else
why = 'mean and per-step coverage agree';
end
C = add(C, 'nees', ok, sprintf( ...
'ANEES %.3f vs mean band %.3f..%.3f (%s), per-step in-band %.0f%% (band %.3f..%.3f) -- %s', ...
aneesMean, band(1), band(2), bandSrc, 100*inFrac, stepBand(1), stepBand(2), why));
end
% Half means are reported, not graded: the same runs are reused at every step, so the
% per-step ANEES values are serially correlated and a chi-square band on a half mean
% (which assumes independence across steps) rejects correct filters. A large split is
% still visible for the reader, and a real time-varying inconsistency shows up as
% per-step ANEES outside the step band above.
if nStep >= 4
half = floor(nStep/2);
halfMeans = [mean(aneesStep(1:half)), mean(aneesStep(half+1:end))];
else
halfMeans = [NaN NaN];
end
% ---- bias ------------------------------------------------------------------
e = truth(keep,:,:) - est(keep,:,:);
bias = squeeze(mean(mean(e, 1), 3));
if n == 1, bias = bias(:); end
biasNorm = norm(bias);
if isfield(claims, 'maxBias')
C = add(C, 'bias', biasNorm <= claims.maxBias, sprintf( ...
'||mean error|| = %.3e vs maxBias %.3e', biasNorm, claims.maxBias));
end
% ---- innovation whiteness --------------------------------------------------
maxAc = NaN; thr = NaN;
if isfield(claims, 'innovation') && ~isempty(claims.innovation)
nu = claims.innovation;
if ismatrix(nu) && nRuns == 1
nu = reshape(nu, [size(nu,1), size(nu,2), 1]);
end
if ndims(nu) ~= 3 || size(nu,1) ~= N
error('mas_estimate_check:innovation', 'innovation must be N x m (x R)');
end
if ~isfield(claims, 'S') || isempty(claims.S)
C = add(C, 'whiteness', NaN, ...
'innovations supplied without S: normalised innovations cannot be formed');
else
S = claims.S;
m = size(nu, 2);
% A scalar or a single m x m covariance is a constant S: broadcast it over
% every step and run. A 3-D S carries one page per step and is shared.
if isscalar(S)
S = repmat(S, [m m N nRuns]);
elseif isequal(size(S), [m m])
S = repmat(S, [1 1 N nRuns]);
elseif ndims(S) == 3 && isequal(size(S), [m m N])
S = repmat(reshape(S, [m m N 1]), [1 1 1 nRuns]);
end
if ndims(S) ~= 4 || size(S,1) ~= m || size(S,2) ~= m
error('mas_estimate_check:S', 'S must be m x m x N (x R)');
end
z = zeros(N, m, nRuns);
badS = false;
for rr = 1:nRuns
for k = keep
Sk = S(:,:,k,rr);
[L, p] = chol((Sk + Sk.')/2, 'lower');
if p ~= 0, badS = true; break; end
z(k,:,rr) = (L \ nu(k,:,rr).').';
end
if badS, break; end
end
if badS
C = add(C, 'whiteness', NaN, ...
'an innovation covariance is not positive definite: cannot whiten');
else
lags = getf(claims, 'whitenessLags', 1:10);
acs = zeros(numel(lags), 1);
for q = 1:numel(lags)
L = lags(q);
if L >= N, acs(q) = NaN; continue; end
a = z(keep(1):end-L, :, :);
b = z(keep(1)+L:end, :, :);
acs(q) = mean(a(:).*b(:)); % pooled over components, steps, runs
end
nEff = nRuns * max(1, nStep - max(lags));
% The metric is the MAX over lags, so a per-lag 95% bound rejects white
% noise about 40% of the time with 10 lags. Split alpha across the lags:
% P(|Z| > z) = alpha/L => z = sqrt(2)*erfcinv(alpha/L).
alphaW = getf(claims, 'whitenessAlpha', 0.05);
zq = sqrt(2)*erfcinv(alphaW/numel(lags));
thr = zq / sqrt(m * nEff);
maxAc = max(abs(acs));
C = add(C, 'whiteness', maxAc <= thr, sprintf( ...
'max |autocorrelation| %.4f over lags %s vs family-wise %.0f%% threshold %.4f', ...
maxAc, mat2str(lags), 100*(1-alphaW), thr));
end
end
end
r = pack(C, struct('aneesStep', aneesStep, 'aneesMean', aneesMean, ...
'band', band, 'stepBand', stepBand, 'bandSrc', bandSrc, 'inFrac', inFrac, 'minEig', minEig, ...
'symErr', symErr, 'bias', bias, 'biasNorm', biasNorm, 'halfMeans', halfMeans, ...
'maxAc', maxAc, 'thr', thr, 'nTot', nTot), N, n, nRuns, burnIn);
end
% ===========================================================================
function [truth, est, P, R, N, n] = normalize(truth, est, P)
if ~isnumeric(truth) || ~isnumeric(est)
error('mas_estimate_check:type', 'truth and est must be numeric');
end
R = 1;
if ndims(truth) == 2
N = size(truth, 1); n = size(truth, 2);
truth = reshape(truth, [N n 1]);
elseif ndims(truth) == 3
N = size(truth, 1); n = size(truth, 2); R = size(truth, 3);
else
error('mas_estimate_check:dim', 'truth must be N x n or N x n x R');
end
if ~isequal(size(est), size(truth))
error('mas_estimate_check:size', 'est must have the same size as truth');
end
if isempty(P)
error('mas_estimate_check:P', 'P is required: without a covariance there is no consistency claim');
end
if ndims(P) == 2
if ~isequal(size(P), [n n]), error('mas_estimate_check:P', 'P must be n x n'); end
P = repmat(P, [1 1 N R]);
elseif ndims(P) == 3
if R ~= 1 || size(P,3) ~= N
error('mas_estimate_check:P', '3-D P requires one run and N pages');
end
P = reshape(P, [n n N 1]);
elseif ndims(P) == 4
if ~isequal(size(P), [n n N R])
error('mas_estimate_check:P', '4-D P must be n x n x N x R');
end
else
error('mas_estimate_check:P', 'P must be 2-D, 3-D or 4-D');
end
end
function [nTot, band, src] = nees_band(n, nTot, alpha)
nu = nTot * n;
band = [chi2inv_base(alpha/2, nu)/nTot, chi2inv_base(1-alpha/2, nu)/nTot];
src = sprintf('chi2, %g%%', 100*(1-alpha));
end
function x = chi2inv_base(p, nu)
% chi2inv without Statistics Toolbox: chi2(nu) is gamma(nu/2, 2).
x = 2*gammaincinv(p, nu/2);
end
function r = pack(C, v, N, n, nRuns, burnIn)
r = struct();
r.n = n; r.nSteps = N; r.nRuns = nRuns; r.burnIn = burnIn;
if isfield(v, 'aneesStep'), r.aneesPerStep = v.aneesStep; end
if isfield(v, 'aneesMean'), r.aneesMean = v.aneesMean; end
if isfield(v, 'band'), r.aneesBand = v.band; r.bandSource = v.bandSrc; end
if isfield(v, 'stepBand'), r.aneesStepBand = v.stepBand; end
if isfield(v, 'inFrac'), r.inBandFrac = v.inFrac; end
if isfield(v, 'minEig'), r.minEigP = v.minEig; r.covSymErr = v.symErr; end
if isfield(v, 'bias'), r.bias = v.bias; r.biasNorm = v.biasNorm; end
if isfield(v, 'maxAc'), r.maxAutocorr = v.maxAc; r.whitenessThreshold = v.thr; end
if isfield(v, 'halfMeans'), r.halfMeans = v.halfMeans; end
if isfield(v, 'nTot'), r.neesSamples = v.nTot; end
r.checks = C; r.nChecks = numel(C);
r.nPass = nnz(strcmp({C.status}, 'pass'));
r.nFail = nnz(strcmp({C.status}, 'fail'));
r.nUndetermined = nnz(strcmp({C.status}, 'undetermined'));
r.nNa = nnz(strcmp({C.status}, 'na'));
r.verdict = verdict_of(C);
r.detail = sprintf('%d run(s) x %d step(s), n=%d, %d/%d checks -- %s', ...
nRuns, N, n, r.nPass, r.nChecks, r.verdict);
end
function v = getf(s, name, dflt)
if isstruct(s) && isfield(s, name), v = s.(name); else, v = dflt; end
end
function C = add(C, id, pass, detail)
if ischar(pass), st = pass;
elseif isnan(pass), st = 'undetermined';
elseif pass, st = 'pass';
else, st = 'fail'; end
C(end+1) = struct('id', id, 'status', st, 'detail', detail);
end
function v = verdict_of(C)
st = {C.status};
if any(strcmp(st, 'fail')), v = 'FAIL';
elseif any(strcmp(st, 'undetermined')), v = 'UNDETERMINED';
else, v = 'PASS'; end
end
Related skills
| Skill | Stars | Updated | Works with | Status | Install |
|---|---|---|---|---|---|
| planning-and-task-breakdownaddyosmani/agent-skillsBreaks work into ordered tasks. Use when you have a spec or clear requirements and need to break work into implementable tasks. Use when a task feels too large to start, when you need to estimate scope, or when parallel work is possible. | Stars: 119k | Updated | Claude CodeCodex | Verified | Install |
| screenpipethedotmack/claude-memSearch screen and audio history recorded by Screenpipe and optionally save selected context into claude-mem. Use when the user asks to recall desktop activity, a meeting, or something seen in another app through Screenpipe. | Stars: 118k | Updated | Claude CodeCodex | Verified | Install |
| contact-widgetnexu-io/open-designSelf-contained floating chat widget with welcome screen, social links, meeting button, and message input. Single HTML file, zero dependencies. | Stars: 108k | Updated | Claude CodeCodex | Verified | Install |
| meeting-notesnexu-io/open-designMeeting notes page — title bar with attendees, agenda checklist, decisions block, action items table with owners + dates, and a "next meeting" footer. Use when the brief mentions "meeting notes", "minutes", "1:1 notes", "all-hands recap", or "会议纪要". | Stars: 108k | Updated | Claude CodeCodex | Verified | Install |