#include "raylib.h"

#include <math.h>

#include <stdbool.h>

#include <stdarg.h>

#include <stdlib.h>

#include <stdio.h>

#include <string.h>

#define MAX_HOT_ITEMS 512

#define PI_F 3.14159265358979323846f

#define SENSOR_HIST_MAX 160

typedef enum {

PHASE_SIMULATE = 0,

PHASE_PREDICT_STATE,

PHASE_PREDICT_COV,

PHASE_INNOVATION,

PHASE_GAIN,

PHASE_UPDATE,

PHASE_COUNT

} EkfPhase;

typedef struct {

float m[4][4];

} Mat4;

typedef struct {

float m[2][4];

} Mat24;

typedef struct {

float m[2][2];

} Mat2;

typedef struct {

Rectangle rect;

char title[96];

char desc[320];

} HotItem;

typedef struct {

float x[4];

Mat4 P;

Mat4 Q;

Mat24 H;

Mat2 R;

float trueState[4];

float z[2];

float y[2];

Mat2 S;

float K[4][2];

float dt;

float simTime;

EkfPhase phase;

int frameCounter;

bool hasMeasurement;

bool paused;

float stepSpeed;

float uiScale;

bool showHelp;

bool showGoal;

Vector2 worldOrigin;

float worldScale;

float splitMainX;

float splitLeftY;

float splitBottomX;

float splitRightY;

bool dragMain;

bool dragLeftY;

bool dragBottomX;

bool dragRightY;

float goalSplitX;

float goalSplitY;

bool dragGoalX;

bool dragGoalY;

float histT[SENSOR_HIST_MAX];

float histZR[SENSOR_HIST_MAX];

float histZB[SENSOR_HIST_MAX];

float histPR[SENSOR_HIST_MAX];

float histPB[SENSOR_HIST_MAX];

float histYR[SENSOR_HIST_MAX];

float histYB[SENSOR_HIST_MAX];

int histCount;

int histHead;

} AppState;

static HotItem gHotItems[MAX_HOT_ITEMS];

static int gHotCount = 0;

static void UpdateMeasurementJacobian(AppState *s, const float x[4]);

static float RandNormal(void) {

float u1 = ((float)GetRandomValue(1, 10000)) / 10001.0f;

float u2 = ((float)GetRandomValue(1, 10000)) / 10001.0f;

float r = sqrtf(-2.0f * logf(u1));

float theta = 2.0f * PI_F * u2;

return r * cosf(theta);

}

static void HotBegin(void) {

gHotCount = 0;

}

static void HotAdd(Rectangle rect, const char *title, const char *desc) {

if (gHotCount >= MAX_HOT_ITEMS) return;

HotItem *h = &gHotItems[gHotCount++];

h->rect = rect;

snprintf(h->title, sizeof(h->title), "%s", title ? title : "");

snprintf(h->desc, sizeof(h->desc), "%s", desc ? desc : "");

}

static void HotAddFmt(Rectangle rect, const char *title, const char *fmt, ...) {

if (gHotCount >= MAX_HOT_ITEMS) return;

HotItem *h = &gHotItems[gHotCount++];

h->rect = rect;

snprintf(h->title, sizeof(h->title), "%s", title ? title : "");

va_list args;

va_start(args, fmt);

vsnprintf(h->desc, sizeof(h->desc), fmt ? fmt : "", args);

va_end(args);

}

static void DrawTextScaled(const char *text, Vector2 pos, float size, Color color) {

Font f = GetFontDefault();

DrawTextEx(f, text, pos, size, 1.0f, color);

}

static float ClampF(float v, float lo, float hi) {

if (v < lo) return lo;

if (v > hi) return hi;

return v;

}

static Vector2 MeasureTextScaled(const char *text, float size) {

Font f = GetFontDefault();

return MeasureTextEx(f, text, size, 1.0f);

}

static void HotAddText(const char *text, Vector2 pos, float size, const char *title, const char *desc) {

Vector2 m = MeasureTextScaled(text, size);

HotAdd((Rectangle){ pos.x - 2, pos.y - 2, m.x + 4, m.y + 4 }, title, desc);

}

static bool DrawButton(Rectangle r, const char *label, float fontSize, Color bg, Color fg) {

Vector2 m = GetMousePosition();

bool hover = CheckCollisionPointRec(m, r);

bool click = hover && IsMouseButtonPressed(MOUSE_LEFT_BUTTON);

Color c = bg;

if (hover) {

c.r = (unsigned char)fminf(255.0f, c.r + 22.0f);

c.g = (unsigned char)fminf(255.0f, c.g + 22.0f);

c.b = (unsigned char)fminf(255.0f, c.b + 22.0f);

}

DrawRectangleRounded(r, 0.18f, 8, c);

DrawRectangleRoundedLinesEx(r, 0.18f, 8, 2.0f, Fade(fg, 0.7f));

Vector2 t = MeasureTextScaled(label, fontSize);

DrawTextScaled(label,

(Vector2){ r.x + (r.width - t.x) * 0.5f, r.y + (r.height - t.y) * 0.5f },

fontSize, fg);

return click;

}

static float DrawSlider(Rectangle r, float minv, float maxv, float *value, float fontSize) {

Vector2 m = GetMousePosition();

bool hover = CheckCollisionPointRec(m, r);

bool down = IsMouseButtonDown(MOUSE_LEFT_BUTTON);

if (hover && down) {

float t = (m.x - r.x) / r.width;

if (t < 0.0f) t = 0.0f;

if (t > 1.0f) t = 1.0f;

*value = minv + t * (maxv - minv);

}

float t = (*value - minv) / (maxv - minv);

float knobX = r.x + t * r.width;

DrawRectangleRounded(r, 0.45f, 8, (Color){ 32, 43, 60, 255 });

Rectangle fill = r;

fill.width *= t;

DrawRectangleRounded(fill, 0.45f, 8, (Color){ 86, 188, 255, 255 });

DrawCircleV((Vector2){ knobX, r.y + r.height * 0.5f }, r.height * 0.45f,

hover ? (Color){ 240, 248, 255, 255 } : (Color){ 206, 232, 255, 255 });

char buf[64];

snprintf(buf, sizeof(buf), "%.2f", *value);

DrawTextScaled(buf, (Vector2){ r.x + r.width + 10, r.y - 4 }, fontSize, (Color){ 220, 235, 255, 255 });

return *value;

}

static Mat4 Mat4Identity(void) {

Mat4 a = {0};

for (int i = 0; i < 4; i++) a.m[i][i] = 1.0f;

return a;

}

static Mat4 Mat4Add(Mat4 a, Mat4 b) {

Mat4 c = {0};

for (int i = 0; i < 4; i++)

for (int j = 0; j < 4; j++)

c.m[i][j] = a.m[i][j] + b.m[i][j];

return c;

}

static Mat4 Mat4Sub(Mat4 a, Mat4 b) {

Mat4 c = {0};

for (int i = 0; i < 4; i++)

for (int j = 0; j < 4; j++)

c.m[i][j] = a.m[i][j] - b.m[i][j];

return c;

}

static Mat4 Mat4Mul(Mat4 a, Mat4 b) {

Mat4 c = {0};

for (int i = 0; i < 4; i++) {

for (int j = 0; j < 4; j++) {

float s = 0.0f;

for (int k = 0; k < 4; k++) s += a.m[i][k] * b.m[k][j];

c.m[i][j] = s;

}

}

return c;

}

static Mat4 Mat4Transpose(Mat4 a) {

Mat4 t = {0};

for (int i = 0; i < 4; i++)

for (int j = 0; j < 4; j++)

t.m[i][j] = a.m[j][i];

return t;

}

static Mat2 Mat2Add(Mat2 a, Mat2 b) {

Mat2 c = {0};

for (int i = 0; i < 2; i++)

for (int j = 0; j < 2; j++)

c.m[i][j] = a.m[i][j] + b.m[i][j];

return c;

}

static Mat2 Mat2Inverse(Mat2 a) {

float det = a.m[0][0] * a.m[1][1] - a.m[0][1] * a.m[1][0];

if (fabsf(det) < 1e-6f) det = (det >= 0.0f) ? 1e-6f : -1e-6f;

float invd = 1.0f / det;

Mat2 inv = {0};

inv.m[0][0] = a.m[1][1] * invd;

inv.m[0][1] = -a.m[0][1] * invd;

inv.m[1][0] = -a.m[1][0] * invd;

inv.m[1][1] = a.m[0][0] * invd;

return inv;

}

static void ResetFilter(AppState *s) {

memset(s->x, 0, sizeof(s->x));

s->x[0] = -6.0f;

s->x[1] = -3.0f;

s->x[2] = 1.8f;

s->x[3] = 0.8f;

s->trueState[0] = -8.0f;

s->trueState[1] = -5.0f;

s->trueState[2] = 2.4f;

s->trueState[3] = 1.3f;

s->P = Mat4Identity();

s->P.m[0][0] = 4.0f;

s->P.m[1][1] = 4.0f;

s->P.m[2][2] = 1.5f;

s->P.m[3][3] = 1.5f;

s->Q = (Mat4){0};

s->Q.m[0][0] = 0.02f;

s->Q.m[1][1] = 0.02f;

s->Q.m[2][2] = 0.06f;

s->Q.m[3][3] = 0.06f;

s->H = (Mat24){0};

UpdateMeasurementJacobian(s, s->x);

s->R = (Mat2){0};

s->R.m[0][0] = 0.65f; // range variance

s->R.m[1][1] = 0.030f; // bearing variance (rad^2)

s->phase = PHASE_SIMULATE;

s->simTime = 0.0f;

s->hasMeasurement = false;

s->histCount = 0;

s->histHead = 0;

// Prefill sensor history so the Goal table/plots are populated immediately.

const int seedN = 24;

float sigmaR = sqrtf(s->R.m[0][0]);

float sigmaB = sqrtf(s->R.m[1][1]);

for (int i = 0; i < seedN; i++) {

float tt = i * s->dt;

float tpx = s->trueState[0] + s->trueState[2] * tt;

float tpy = s->trueState[1] + s->trueState[3] * tt;

float ppx = s->x[0] + s->x[2] * tt;

float ppy = s->x[1] + s->x[3] * tt;

float tr = sqrtf(tpx * tpx + tpy * tpy);

float tb = atan2f(tpy, tpx);

float pr = sqrtf(fmaxf(1e-6f, ppx * ppx + ppy * ppy));

float pb = atan2f(ppy, ppx);

float zr = tr + sigmaR * 0.35f * sinf(0.7f * i);

float zb = tb + sigmaB * 0.40f * cosf(0.5f * i);

while (zb > PI_F) zb -= 2.0f * PI_F;

while (zb < -PI_F) zb += 2.0f * PI_F;

float yr = zr - pr;

float yb = zb - pb;

while (yb > PI_F) yb -= 2.0f * PI_F;

while (yb < -PI_F) yb += 2.0f * PI_F;

int idx = s->histHead;

s->histT[idx] = tt;

s->histZR[idx] = zr;

s->histZB[idx] = zb;

s->histPR[idx] = pr;

s->histPB[idx] = pb;

s->histYR[idx] = yr;

s->histYB[idx] = yb;

s->histHead = (s->histHead + 1) % SENSOR_HIST_MAX;

if (s->histCount < SENSOR_HIST_MAX) s->histCount++;

}

}

static Mat4 TransitionMatrix(float dt) {

Mat4 F = Mat4Identity();

F.m[0][2] = dt;

F.m[1][3] = dt;

return F;

}

static float NormalizeAngle(float a) {

while (a > PI_F) a -= 2.0f * PI_F;

while (a < -PI_F) a += 2.0f * PI_F;

return a;

}

static void UpdateMeasurementJacobian(AppState *s, const float x[4]) {

float px = x[0];

float py = x[1];

float r2 = px * px + py * py;

float r = sqrtf(fmaxf(r2, 1e-6f));

if (r2 < 1e-6f) r2 = 1e-6f;

s->H = (Mat24){0};

s->H.m[0][0] = px / r;

s->H.m[0][1] = py / r;

s->H.m[1][0] = -py / r2;

s->H.m[1][1] = px / r2;

}

static void SimulateTruthAndMeasurement(AppState *s) {

float t = s->simTime;

float ax = 0.7f * sinf(0.7f * t) + 0.25f * cosf(1.5f * t);

float ay = 0.65f * cosf(0.8f * t);

s->trueState[2] += ax * s->dt;

s->trueState[3] += ay * s->dt;

s->trueState[0] += s->trueState[2] * s->dt;

s->trueState[1] += s->trueState[3] * s->dt;

float px = s->trueState[0];

float py = s->trueState[1];

float range = sqrtf(px * px + py * py);

float bearing = atan2f(py, px);

float sigmaR = sqrtf(s->R.m[0][0]);

float sigmaB = sqrtf(s->R.m[1][1]);

s->z[0] = range + sigmaR * RandNormal();

s->z[1] = NormalizeAngle(bearing + sigmaB * RandNormal());

s->hasMeasurement = true;

}

static void PredictState(AppState *s) {

Mat4 F = TransitionMatrix(s->dt);

float nx[4] = {0};

for (int i = 0; i < 4; i++)

for (int j = 0; j < 4; j++)

nx[i] += F.m[i][j] * s->x[j];

memcpy(s->x, nx, sizeof(nx));

}

static void PredictCovariance(AppState *s) {

Mat4 F = TransitionMatrix(s->dt);

s->P = Mat4Add(Mat4Mul(Mat4Mul(F, s->P), Mat4Transpose(F)), s->Q);

}

static void ComputeInnovation(AppState *s) {

float px = s->x[0];

float py = s->x[1];

float range = sqrtf(fmaxf(1e-6f, px * px + py * py));

float bearing = atan2f(py, px);

float hx[2] = { range, bearing };

UpdateMeasurementJacobian(s, s->x);

s->y[0] = s->z[0] - hx[0];

s->y[1] = NormalizeAngle(s->z[1] - hx[1]);

int idx = s->histHead;

s->histT[idx] = s->simTime;

s->histZR[idx] = s->z[0];

s->histZB[idx] = s->z[1];

s->histPR[idx] = hx[0];

s->histPB[idx] = hx[1];

s->histYR[idx] = s->y[0];

s->histYB[idx] = s->y[1];

s->histHead = (s->histHead + 1) % SENSOR_HIST_MAX;

if (s->histCount < SENSOR_HIST_MAX) s->histCount++;

float HP[2][4] = {0};

for (int i = 0; i < 2; i++)

for (int j = 0; j < 4; j++)

for (int k = 0; k < 4; k++)

HP[i][j] += s->H.m[i][k] * s->P.m[k][j];

Mat2 S = {0};

for (int i = 0; i < 2; i++) {

for (int j = 0; j < 2; j++) {

float v = 0.0f;

for (int k = 0; k < 4; k++) v += HP[i][k] * s->H.m[j][k];

S.m[i][j] = v;

}

}

s->S = Mat2Add(S, s->R);

}

static void ComputeGain(AppState *s) {

Mat2 SInv = Mat2Inverse(s->S);

float PHt[4][2] = {0};

for (int i = 0; i < 4; i++) {

for (int j = 0; j < 2; j++) {

float v = 0.0f;

for (int k = 0; k < 4; k++) v += s->P.m[i][k] * s->H.m[j][k];

PHt[i][j] = v;

}

}

for (int i = 0; i < 4; i++) {

for (int j = 0; j < 2; j++) {

s->K[i][j] = PHt[i][0] * SInv.m[0][j] + PHt[i][1] * SInv.m[1][j];

}

}

}

static void UpdateFilter(AppState *s) {

for (int i = 0; i < 4; i++) {

s->x[i] += s->K[i][0] * s->y[0] + s->K[i][1] * s->y[1];

}

Mat4 I = Mat4Identity();

Mat4 KH = {0};

for (int i = 0; i < 4; i++) {

for (int j = 0; j < 4; j++) {

KH.m[i][j] = s->K[i][0] * s->H.m[0][j] + s->K[i][1] * s->H.m[1][j];

}

}

s->P = Mat4Mul(Mat4Sub(I, KH), s->P);

s->simTime += s->dt;

}

static void StepPhase(AppState *s) {

switch (s->phase) {

case PHASE_SIMULATE:

SimulateTruthAndMeasurement(s);

s->phase = PHASE_PREDICT_STATE;

break;

case PHASE_PREDICT_STATE:

PredictState(s);

s->phase = PHASE_PREDICT_COV;

break;

case PHASE_PREDICT_COV:

PredictCovariance(s);

s->phase = PHASE_INNOVATION;

break;

case PHASE_INNOVATION:

if (s->hasMeasurement) {

ComputeInnovation(s);

s->phase = PHASE_GAIN;

}

break;

case PHASE_GAIN:

ComputeGain(s);

s->phase = PHASE_UPDATE;

break;

case PHASE_UPDATE:

UpdateFilter(s);

s->phase = PHASE_SIMULATE;

break;

default:

s->phase = PHASE_SIMULATE;

break;

}

}

static Vector2 WorldToScreen(AppState *s, Rectangle view, Vector2 p) {

Vector2 center = { view.x + view.width * 0.5f, view.y + view.height * 0.5f };

return (Vector2){ center.x + p.x * s->worldScale, center.y - p.y * s->worldScale };

}

static void DrawCovarianceEllipse(AppState *s, Rectangle view, Color color) {

float a = s->P.m[0][0];

float b = s->P.m[0][1];

float c = s->P.m[1][1];

float tr = a + c;

float det = a * c - b * b;

float disc = fmaxf(0.0f, tr * tr * 0.25f - det);

float l1 = tr * 0.5f + sqrtf(disc);

float l2 = tr * 0.5f - sqrtf(disc);

if (l1 < 1e-4f) l1 = 1e-4f;

if (l2 < 1e-4f) l2 = 1e-4f;

float angle = 0.5f * atan2f(2.0f * b, a - c);

float rx = 2.2f * sqrtf(l1);

float ry = 2.2f * sqrtf(l2);

Vector2 centerW = { s->x[0], s->x[1] };

const int N = 64;

Vector2 prev = {0};

for (int i = 0; i <= N; i++) {

float t = (float)i / (float)N * 2.0f * PI_F;

float ex = rx * cosf(t);

float ey = ry * sinf(t);

float wx = centerW.x + ex * cosf(angle) - ey * sinf(angle);

float wy = centerW.y + ex * sinf(angle) + ey * cosf(angle);

Vector2 p = WorldToScreen(s, view, (Vector2){ wx, wy });

if (i > 0) DrawLineEx(prev, p, 2.4f, color);

prev = p;

}

}

static void DrawWorldGrid(AppState *s, Rectangle view) {

DrawRectangleRounded(view, 0.02f, 4, (Color){ 19, 27, 39, 255 });

DrawRectangleRoundedLinesEx(view, 0.02f, 4, 2.0f, (Color){ 82, 119, 159, 120 });

for (int i = -12; i <= 12; i++) {

Vector2 a = WorldToScreen(s, view, (Vector2){ (float)i, -12.0f });

Vector2 b = WorldToScreen(s, view, (Vector2){ (float)i, 12.0f });

DrawLineEx(a, b, (i == 0) ? 2.2f : 1.0f, (i == 0) ? (Color){ 120, 180, 235, 150 } : (Color){ 70, 95, 120, 75 });

}

for (int j = -12; j <= 12; j++) {

Vector2 a = WorldToScreen(s, view, (Vector2){ -12.0f, (float)j });

Vector2 b = WorldToScreen(s, view, (Vector2){ 12.0f, (float)j });

DrawLineEx(a, b, (j == 0) ? 2.2f : 1.0f, (j == 0) ? (Color){ 120, 180, 235, 150 } : (Color){ 70, 95, 120, 75 });

}

HotAdd(view,

"State Space",

"2D position space. True target, noisy sensor point, EKF estimate, velocity vector, and the covariance ellipse live here.");

}

static void DrawStateViews(AppState *s, Rectangle view, float fontSize) {

DrawWorldGrid(s, view);

Vector2 pTrue = WorldToScreen(s, view, (Vector2){ s->trueState[0], s->trueState[1] });

Vector2 pEst = WorldToScreen(s, view, (Vector2){ s->x[0], s->x[1] });

float mzx = s->z[0] * cosf(s->z[1]);

float mzy = s->z[0] * sinf(s->z[1]);

Vector2 pZ = WorldToScreen(s, view, (Vector2){ mzx, mzy });

Vector2 pSensor = WorldToScreen(s, view, (Vector2){ 0.0f, 0.0f });

DrawCovarianceEllipse(s, view, (Color){ 255, 205, 94, 220 });

DrawCircleV(pTrue, 7.5f, (Color){ 113, 234, 143, 255 });

DrawCircleV(pZ, 6.0f, (Color){ 255, 110, 110, 240 });

DrawCircleV(pEst, 7.5f, (Color){ 110, 200, 255, 255 });

DrawCircleV(pSensor, 6.0f, (Color){ 255, 226, 150, 240 });

DrawLineEx(pSensor, pZ, 1.3f, (Color){ 255, 208, 130, 160 });

DrawCircleLines((int)pSensor.x, (int)pSensor.y, s->z[0] * s->worldScale, (Color){ 255, 208, 130, 70 });

Vector2 vHead = WorldToScreen(s, view, (Vector2){ s->x[0] + s->x[2] * 0.8f, s->x[1] + s->x[3] * 0.8f });

DrawLineEx(pEst, vHead, 2.6f, (Color){ 130, 205, 255, 220 });

DrawCircleV(vHead, 4.0f, (Color){ 130, 205, 255, 220 });

DrawLineEx(pEst, pZ, 1.9f, (Color){ 255, 130, 130, 220 });

DrawTextScaled("true", (Vector2){ pTrue.x + 8, pTrue.y - 16 }, fontSize * 0.78f, (Color){ 170, 245, 180, 255 });

DrawTextScaled("measurement", (Vector2){ pZ.x + 8, pZ.y - 16 }, fontSize * 0.78f, (Color){ 255, 170, 170, 255 });

DrawTextScaled("estimate", (Vector2){ pEst.x + 8, pEst.y - 16 }, fontSize * 0.78f, (Color){ 168, 218, 255, 255 });

DrawTextScaled("sensor", (Vector2){ pSensor.x + 8, pSensor.y - 16 }, fontSize * 0.72f, (Color){ 255, 228, 162, 255 });

HotAddText("true", (Vector2){ pTrue.x + 8, pTrue.y - 16 }, fontSize * 0.78f, "True Label", "Ground-truth trajectory used only for debugging, never fed into the filter.");

HotAddText("measurement", (Vector2){ pZ.x + 8, pZ.y - 16 }, fontSize * 0.78f, "Measurement Label", "Sensor output after noise. This is what EKF actually receives.");

HotAddText("estimate", (Vector2){ pEst.x + 8, pEst.y - 16 }, fontSize * 0.78f, "Estimate Label", "Current posterior state estimate after EKF correction.");

HotAddText("sensor", (Vector2){ pSensor.x + 8, pSensor.y - 16 }, fontSize * 0.72f, "Sensor Label", "Range-bearing sensor origin.");

HotAdd((Rectangle){ pTrue.x - 10, pTrue.y - 10, 20, 20 },

"True State",

"Ground truth from simulation. The EKF never directly sees this value.");

HotAdd((Rectangle){ pEst.x - 10, pEst.y - 10, 20, 20 },

"Estimated State x",

"Filter estimate x = [px, py, vx, vy]. This is what EKF believes right now.");

HotAdd((Rectangle){ pZ.x - 9, pZ.y - 9, 18, 18 },

"Measurement z",

"Noisy sensor reading in polar (range, bearing), plotted in Cartesian for intuition.");

HotAdd((Rectangle){ pSensor.x - 9, pSensor.y - 9, 18, 18 },

"Sensor Origin",

"Range/bearing sensor frame origin. Circles and rays show measured geometry.");

HotAdd((Rectangle){ pEst.x - 2, pEst.y - 2, vHead.x - pEst.x + 4, vHead.y - pEst.y + 4 },

"Velocity Vector",

"Heading and speed from the estimated velocity components.");

}

static void DrawCovBars(AppState *s, Rectangle r, float fontSize) {

DrawRectangleRounded(r, 0.04f, 6, (Color){ 20, 30, 43, 255 });

DrawRectangleRoundedLinesEx(r, 0.04f, 6, 1.5f, (Color){ 90, 130, 170, 120 });

const char *labels[4] = { "Pxx", "Pyy", "Pvx", "Pvy" };

float vals[4] = { s->P.m[0][0], s->P.m[1][1], s->P.m[2][2], s->P.m[3][3] };

float maxv = 8.0f;

for (int i = 0; i < 4; i++) {

float y = r.y + 30 + i * 40;

Vector2 lp = { r.x + 10, y };

DrawTextScaled(labels[i], lp, fontSize * 0.8f, (Color){ 196, 215, 235, 255 });

const char *ld[4] = {

"Variance of position x error.",

"Variance of position y error.",

"Variance of velocity x error.",

"Variance of velocity y error."

};

HotAddText(labels[i], lp, fontSize * 0.8f, labels[i], ld[i]);

Rectangle bar = { r.x + 70, y + 3, r.width - 85, 18 };

DrawRectangleRounded(bar, 0.35f, 6, (Color){ 40, 58, 78, 255 });

Rectangle fill = bar;

fill.width *= fminf(vals[i] / maxv, 1.0f);

DrawRectangleRounded(fill, 0.35f, 6, (Color){ 255, 202, 102, 240 });

HotAddFmt(bar, "Covariance Bar", "%s bar. Normalized uncertainty %.1f%% of displayed range.", labels[i], fminf(vals[i] / maxv, 1.0f) * 100.0f);

char buf[32];

snprintf(buf, sizeof(buf), "%.3f", vals[i]);

Vector2 vp = { bar.x + bar.width - 55, y };

DrawTextScaled(buf, vp, fontSize * 0.74f, (Color){ 244, 234, 188, 255 });

Vector2 vm = MeasureTextScaled(buf, fontSize * 0.74f);

HotAddFmt((Rectangle){ vp.x - 2, vp.y - 2, vm.x + 4, vm.y + 4 },

"Covariance Value", "%s = %.5f. This is variance, not standard deviation.", labels[i], vals[i]);

}

Vector2 covTitle = { r.x + 10, r.y + 8 };

DrawTextScaled("Uncertainty by state axis", covTitle, fontSize * 0.86f, (Color){ 220, 234, 250, 255 });

HotAddText("Uncertainty by state axis", covTitle, fontSize * 0.86f, "Covariance Summary", "Bar chart of uncertainty magnitudes for each state component.");

HotAdd(r,

"Covariance Diagonal",

"How uncertain the filter is in each state component. Bigger means less confidence.");

}

static void DrawArrowLine(Vector2 a, Vector2 b, float thick, Color c) {

DrawLineEx(a, b, thick, c);

Vector2 d = (Vector2){ b.x - a.x, b.y - a.y };

float n = sqrtf(d.x * d.x + d.y * d.y);

if (n < 1e-3f) return;

d.x /= n;

d.y /= n;

Vector2 p = (Vector2){ -d.y, d.x };

float h = 8.0f + thick * 0.9f;

Vector2 l = (Vector2){ b.x - d.x * h, b.y - d.y * h };

Vector2 t1 = (Vector2){ l.x + p.x * (4.0f + thick), l.y + p.y * (4.0f + thick) };

Vector2 t2 = (Vector2){ l.x - p.x * (4.0f + thick), l.y - p.y * (4.0f + thick) };

DrawTriangle(b, t1, t2, c);

}

static Color JacobianCellColor(float v, float vmax) {

float t = fminf(fabsf(v) / vmax, 1.0f);

if (v >= 0.0f) {

return (Color){

(unsigned char)(50 + 130 * t),

(unsigned char)(80 + 150 * t),

(unsigned char)(120 + 90 * t),

235

};

}

return (Color){

(unsigned char)(120 + 120 * t),

(unsigned char)(65 + 70 * t),

(unsigned char)(50 + 80 * t),

235

};

}

static void DrawMatrixHeatmap4(Rectangle r, float m[4][4], const char *title, const char *sym, float fontSize) {

static const char *stateName[4] = { "px", "py", "vx", "vy" };

DrawTextScaled(title, (Vector2){ r.x + 8, r.y + 6 }, fontSize * 0.78f, (Color){ 222, 235, 250, 255 });

float x = r.x + 8;

float y = r.y + 28;

float cw = (r.width - 16) / 4.0f;

float ch = (r.height - 36) / 4.0f;

for (int i = 0; i < 4; i++) {

for (int j = 0; j < 4; j++) {

Rectangle c = { x + j * cw, y + i * ch, cw - 2, ch - 2 };

DrawRectangleRounded(c, 0.14f, 4, JacobianCellColor(m[i][j], 1.0f));

char b[32];

snprintf(b, sizeof(b), "%.2f", m[i][j]);

DrawTextScaled(b, (Vector2){ c.x + 4, c.y + ch * 0.25f }, fontSize * 0.58f, (Color){ 246, 246, 248, 255 });

HotAddFmt(c, "Jacobian Cell",

"%s[%d,%d] = %.4f. Local derivative d(%s_next)/d(%s).",

sym, i, j, m[i][j], stateName[i], stateName[j]);

}

}

}

static void DrawMatrixHeatmap24(Rectangle r, Mat24 m, const char *title, const char *sym, float fontSize) {

static const char *measName[2] = { "range", "bearing" };

static const char *stateName[4] = { "px", "py", "vx", "vy" };

DrawTextScaled(title, (Vector2){ r.x + 8, r.y + 6 }, fontSize * 0.78f, (Color){ 222, 235, 250, 255 });

float x = r.x + 8;

float y = r.y + 28;

float cw = (r.width - 16) / 4.0f;

float ch = (r.height - 36) / 2.0f;

for (int i = 0; i < 2; i++) {

for (int j = 0; j < 4; j++) {

Rectangle c = { x + j * cw, y + i * ch, cw - 2, ch - 2 };

DrawRectangleRounded(c, 0.14f, 4, JacobianCellColor(m.m[i][j], 1.0f));

char b[32];

snprintf(b, sizeof(b), "%.2f", m.m[i][j]);

DrawTextScaled(b, (Vector2){ c.x + 4, c.y + ch * 0.24f }, fontSize * 0.56f, (Color){ 246, 246, 248, 255 });

HotAddFmt(c, "Jacobian Cell",

"%s[%d,%d] = %.4f. Local derivative d(%s)/d(%s).",

sym, i, j, m.m[i][j], measName[i], stateName[j]);

}

}

}

static void DrawJacobianPanel(AppState *s, Rectangle r, float fontSize) {

DrawRectangleRounded(r, 0.04f, 6, (Color){ 20, 30, 43, 255 });

DrawRectangleRoundedLinesEx(r, 0.04f, 6, 1.5f, (Color){ 90, 130, 170, 120 });

DrawTextScaled("Jacobian view (visual + numeric)", (Vector2){ r.x + 10, r.y + 8 }, fontSize * 0.86f, (Color){ 220, 234, 250, 255 });

Rectangle top = { r.x + 8, r.y + 28, r.width - 16, r.height * 0.58f };

Rectangle geom = { r.x + 8, top.y + top.height + 6, r.width - 16, r.height - (top.height + 42) };

Rectangle fRect = { top.x, top.y, top.width * 0.62f, top.height };

Rectangle hRect = { fRect.x + fRect.width + 8, top.y, top.width - (fRect.width + 8), top.height };

Mat4 F = TransitionMatrix(s->dt);

DrawMatrixHeatmap4(fRect, F.m, "F = df/dx (4x4)", "F", fontSize);

DrawMatrixHeatmap24(hRect, s->H, "H = dh/dx (2x4)", "H", fontSize);

DrawRectangleRounded(geom, 0.08f, 6, (Color){ 14, 22, 34, 255 });

DrawRectangleRoundedLinesEx(geom, 0.08f, 6, 1.2f, (Color){ 86, 120, 160, 140 });

DrawTextScaled("Geometric influence graph", (Vector2){ geom.x + 8, geom.y + 6 }, fontSize * 0.72f, (Color){ 245, 210, 140, 255 });

float t = (float)GetTime();

float pulse = 0.6f + 0.4f * sinf(t * 4.2f);

float sy = geom.y + geom.height * 0.56f;

Vector2 npx = { geom.x + 48, sy - 20 };