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_ekf_check.m
View on GitHubfunction r = mas_ekf_check(model, opts)
%MAS_EKF_CHECK Is the filter's uncertainty honest? NEES / NIS consistency test.
%
% r = mas_ekf_check(struct('A',A,'H',H,'Q',Q,'R',R,'x0',x0,'P0',P0))
%
% r.anees average normalised estimation error squared, per step
% r.aneesMean overall mean; must be near n (the state dimension)
% r.aneesBand [lo hi] chi-square acceptance band
% r.inBandFrac fraction of steps inside the band
% r.nisMean mean normalised innovation squared; near m (measurement dim)
% r.verdict
%
% THE QUESTION THIS ANSWERS
% A filter can be stable, smooth, and confidently wrong. Its own covariance P is the
% only claim it makes about accuracy, and a Kalman filter is only meaningful if that
% claim matches reality. NEES tests exactly that:
%
% eps_k = (x_k - xhat_k)' P_k^{-1} (x_k - xhat_k) E[eps] = n
%
% If eps >> n the filter is over-confident -- it believes itself more than the data
% justifies, and downstream gating will reject good measurements. If eps << n it is
% under-confident and is throwing away information. Both are defects, and neither is
% visible in a plot of the estimate: the estimate tracks the truth in all three cases.
%
% WHY IT NEEDS MONTE CARLO AND A CHI-SQUARE BAND
% A single run gives one noisy number, so this averages over `nRuns` independent
% ground-truth realisations drawn from the SAME Q and R the filter is given. Under
% the filter's own assumptions N * ANEES ~ chi2(N*n), which gives a two-sided band
% rather than an arbitrary tolerance.
%
% The band is two-sided on purpose: "close to n" is not the same as "consistent", and
% a filter tuned by making R enormous is pessimistic, not correct.
%
% The verdict checks the band fraction, not just the mean, because a filter can have
% the right mean while being inconsistent at particular times (during a transient,
% for instance). `inBandFrac` at 0.9 with a 99 % band catches that.
%
% Requires Statistics and Machine Learning Toolbox for chi2inv; without it the
% verdict falls back to a fixed +/-20 % band around n and says which was used.
if nargin < 2, opts = struct(); end
opts = dflt(opts, struct('nSteps', 60, 'nRuns', 200, 'alpha', 0.01, ...
'burnIn', 10, 'seed', 20240, 'tolFrac', 0.20));
m = model;
A = m.A; H = m.H; Q = m.Q; R = m.R;
% Q and R are what the filter is TOLD. Qtrue / Rtrue are what the world actually
% does. They default to being equal, which is the case a correctly tuned filter
% must pass. Separating them is what lets this function test a MIS-tuned filter:
% with a single covariance there is no way to build the negative control, so the
% consistency check could not be shown to detect anything.
Qt = pick(m, 'Qtrue', Q);
Rt = pick(m, 'Rtrue', R);
n = size(A, 1);
mm = size(H, 1);
rng(opts.seed);
Lq = chol_safe(Qt, n);
Lr = chol_safe(Rt, mm);
anees = zeros(opts.nSteps, 1);
nis = zeros(opts.nSteps, 1);
for run = 1:opts.nRuns
x = m.x0(:);
xh = m.x0(:);
P = m.P0;
for k = 1:opts.nSteps
% process the ACTUAL system with the ACTUAL noise
x = A*x + Lq.'*randn(n, 1);
% predict
xh = A*xh;
P = A*P*A.' + Q;
% update
z = H*x + Lr.'*randn(mm, 1);
nu = z - H*xh;
S = H*P*H.' + R;
K = P*H.'/S;
xh = xh + K*nu;
P = (eye(n) - K*H)*P;
% metrics against ground truth
ee = x - xh;
anees(k) = anees(k) + ee.'*(P\ee);
nis(k) = nis(k) + nu.'*(S\nu);
end
end
anees = anees / opts.nRuns;
nis = nis / opts.nRuns;
keep = (opts.burnIn+1):opts.nSteps;
aneesMean = mean(anees(keep));
nisMean = mean(nis(keep));
[band, bandSrc] = anees_band(n, opts.nRuns, opts.alpha, opts.tolFrac);
inBand = (anees(keep) >= band(1)) & (anees(keep) <= band(2));
inBandFrac = mean(inBand);
ok = (inBandFrac >= 0.90) && (aneesMean >= band(1)) && (aneesMean <= band(2)) && ...
isfinite(aneesMean) && isfinite(nisMean);
warn = {};
if ~ok
if aneesMean > band(2)
warn{end+1} = sprintf(['ANEES %.3f is above the %.3f bound: the filter is ' ...
'OVER-CONFIDENT (P too small, or Q/R too small). It will reject good ' ...
'measurements and its error bars are lies.'], aneesMean, band(2));
elseif aneesMean < band(1)
warn{end+1} = sprintf(['ANEES %.3f is below the %.3f bound: the filter is ' ...
'UNDER-CONFIDENT (P too large, or Q/R too large). It is discarding ' ...
'information it was given.'], aneesMean, band(1));
end
if inBandFrac < 0.90
warn{end+1} = sprintf(['only %.0f%% of steps fall inside the band: the ' ...
'inconsistency is time-varying, not a constant mis-scaling.'], 100*inBandFrac);
end
end
r = struct();
r.anees = anees; r.aneesMean = aneesMean; r.nis = nis; r.nisMean = nisMean;
r.aneesBand = band; r.bandSource = bandSrc;
r.inBandFrac = inBandFrac;
r.n = n; r.m = mm; r.nRuns = opts.nRuns; r.nSteps = opts.nSteps;
r.warnings = warn;
r.ok = ok;
r.verdict = tern(ok, 'PASS', 'FAIL');
r.detail = sprintf(['n=%d m=%d, %d runs x %d steps: ANEES %.3f (band %.3f..%.3f, %s), ' ...
'NIS %.3f (target %d), in-band %.0f%% -- %s'], n, mm, opts.nRuns, opts.nSteps, ...
aneesMean, band(1), band(2), bandSrc, nisMean, mm, 100*inBandFrac, r.verdict);
end
% ---------------------------------------------------------------------------
function v = pick(s, name, dfltVal)
if isstruct(s) && isfield(s, name), v = s.(name); else, v = dfltVal; end
end
function L = chol_safe(M, k)
% Factor M = L'*L without letting a semidefinite matrix silently succeed or a
% slightly indefinite one (rounding) fail. Falls back to an eigen-square-root so a
% noise covariance that is 0 in one direction still samples correctly.
M = (M + M.')/2;
[L, p] = chol(M);
if p == 0, return; end
[V, D] = eig(M);
d = max(real(diag(D)), 0);
L = sqrt(diag(d)) * V.';
end
function [band, src] = anees_band(n, nRuns, alpha, tolFrac)
% Two-sided chi-square band for the average NEES.
%
% Each run contributes one chi2(n); N independent runs sum to chi2(N*n), so
%
% N * ANEES ~ chi2(N*n) => ANEES ~ chi2(N*n) / N
%
% and the band divides by N (the number of runs), NOT by N*n. Dividing by N*n
% centres the band on 1 instead of on the state dimension n, so a perfectly
% consistent 2-state filter reports ANEES 2.0 against an acceptance band of
% 0.83..1.19 and is called over-confident. The first version made exactly
% this error and rejected every filter it was given, including the correct one.
if exist('chi2inv', 'file') == 2
nu = nRuns * n;
band = [chi2inv(alpha/2, nu)/nRuns, chi2inv(1 - alpha/2, nu)/nRuns];
src = sprintf('chi2, %g%%', 100*(1-alpha));
else
band = [n*(1 - tolFrac), n*(1 + tolFrac)];
src = sprintf('fixed +/-%g%% (chi2inv unavailable)', 100*tolFrac);
end
end
% ---------------------------------------------------------------------------
function o = dflt(o, d)
f = fieldnames(d);
for k = 1:numel(f)
if ~isfield(o, f{k}), o.(f{k}) = d.(f{k}); end
end
end
function s = tern(c, a, b)
if c, s = a; else, s = b; 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 |