Skip to content

CODE

Localization Implementation — COMAC Per-DOF + Probability Normalization

void tiny_damage_locate(const tiny_sysid_result_t *cur, tiny_damage_locate_t *out)
{
    memset(out, 0, sizeof(*out));
    out->dof = -1;
    if (!g_has_baseline || !cur) return;

    int nch = cur->n_ch, nm = min(cur->n_modes, g_baseline.n_modes);

    float comac[TINY_DAMAGE_MAX_DOF];
    for (int d = 0; d < nch; d++) {
        double num = 0, den_a = 0, den_b = 0;
        for (int m = 0; m < nm; m++) {
            num  += (double)cur->shapes[m][d] * g_baseline.shapes[m][d];
            den_a += (double)cur->shapes[m][d] * cur->shapes[m][d];
            den_b += (double)g_baseline.shapes[m][d] * g_baseline.shapes[m][d];
        }
        comac[d] = (den_a > 1e-30 && den_b > 1e-30)
                   ? (float)(num * num / (den_a * den_b)) : 1.0f;
    }

    float pmax = 0;
    for (int d = 0; d < nch; d++) {
        out->prob[d] = 1.0f - comac[d];
        if (out->prob[d] > pmax) pmax = out->prob[d];
    }
    if (pmax > 1e-12f)
        for (int d = 0; d < nch; d++) out->prob[d] /= pmax;

    int best = 0;
    for (int d = 1; d < nch; d++)
        if (out->prob[d] > out->prob[best]) best = d;
    out->dof = best;

    float cmin = 1.0f;
    for (int d = 0; d < nch; d++) if (comac[d] < cmin) cmin = comac[d];
    out->comac_min = cmin;
}