/*
* kalman_sort_c.c
*
* SORT-like Kalman filter in C (7D state, 4D measurement)
*
* State x = [u, v, s, r, u_dot, v_dot, s_dot]^T
* Measurement z = [u, v, s, r]^T
*
* Dependencies: none (pure C)
*
* Compile: gcc kalman_sort_c.c -o kalman_sort_c -lm
*
* Note: for clarity this code uses double and simple matrix ops.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "sort_kalman_filter.h"
/* ----------------- 小矩阵工具（行主序 flat 数组） ----------------- */
/* set identity n x n into A */
static void mat_identity(double *A, int n) {
for (int i = 0; i < n * n; ++i) A[i] = 0.0;
for (int i = 0; i < n; ++i) A[i*n + i] = 1.0;
}
/* copy matrix a(r x c) -> b (assumed allocated) */
static void mat_copy(const double *a, double *b, int r, int c) {
memcpy(b, a, sizeof(double) * r * c);
}
/* C = A (r x m) * B (m x c) */
static void mat_mul(const double *A, int r, int m, const double *B, int c, double *C) {
/* C size r x c */
for (int i=0;i<r;i++){
for (int j=0;j<c;j++){
double s = 0.0;
for (int k=0;k<m;k++){
s += A[i*m + k] * B[k*c + j];
}
C[i*c + j] = s;
}
}
}
/* C = A + B (r x c) */
static void mat_add(const double *A, const double *B, int r, int c, double *C) {
for (int i=0;i<r*c;i++) C[i] = A[i] + B[i];
}
/* C = A - B (r x c) */
static void mat_sub(const double *A, const double *B, int r, int c, double *C) {
for (int i=0;i<r*c;i++) C[i] = A[i] - B[i];
}
/* transpose (r x c) -> out (c x r) */
static void mat_transpose(const double *A, int r, int c, double *out) {
for (int i=0;i<r;i++) for (int j=0;j<c;j++) out[j*r + i] = A[i*c + j];
}
/*
* Gauss-Jordan inverse: in-place on a copy.
* Input: A (n x n), output Inv (n x n). Returns 0 on success, -1 on singular.
*/
static int mat_inverse(const double *A_in, int n, double *Inv) {
double aug[MAXN * (MAXN*2)];
if (n > MAXN) return -1;
/* build augmented [A | I] */
for (int i=0;i<n;i++){
for (int j=0;j<n;j++){
aug[i*(2*n) + j] = A_in[i*n + j];
}
for (int j=0;j<n;j++){
aug[i*(2*n) + (n + j)] = (i==j) ? 1.0 : 0.0;
}
}
/* forward elimination */
for (int col = 0; col < n; ++col) {
/* pivot */
int pivot = col;
double maxval = fabs(aug[pivot*(2*n) + col]);
for (int r = col+1; r < n; ++r) {
double val = fabs(aug[r*(2*n) + col]);
if (val > maxval) { maxval = val; pivot = r; }
}
if (fabs(aug[pivot*(2*n) + col]) < 1e-12) return -1; /* singular */
/* swap rows if needed */
if (pivot != col) {
for (int c = 0; c < 2*n; ++c) {
double tmp = aug[col*(2*n) + c];
aug[col*(2*n) + c] = aug[pivot*(2*n) + c];
aug[pivot*(2*n) + c] = tmp;
}
}
/* normalize pivot row */
double diag = aug[col*(2*n) + col];
for (int c = 0; c < 2*n; ++c) aug[col*(2*n) + c] /= diag;
/* eliminate other rows */
for (int r = 0; r < n; ++r) {
if (r == col) continue;
double factor = aug[r*(2*n) + col];
if (fabs(factor) < 1e-15) continue;
for (int c = 0; c < 2*n; ++c) {
aug[r*(2*n) + c] -= factor * aug[col*(2*n) + c];
}
}
}
/* extract inverse */
for (int i=0;i<n;i++){
for (int j=0;j<n;j++){
Inv[i*n + j] = aug[i*(2*n) + (n + j)];
}
}
return 0;
}
/* initialize with measurement z = [u,v,s,r] */
void KalmanBoxTracker_init(KalmanBoxTracker *kf, double u, double v, double s, double r) {
/* initial state */
for (int i = 0; i < NX; ++i) kf->x[i] = 0.0;
kf->x[0] = u; kf->x[1] = v; kf->x[2] = s; kf->x[3] = r;
for (int i=0;i<NX*NX;i++) kf->P[i] = 0.0;
for (int i=0;i<NX;i++) kf->P[i*NX + i] = 10.0; /* initial uncertainty */
/* F matrix (state transition) */
for (int i=0;i<NX*NX;i++) kf->F[i] = 0.0;
for (int i=0;i<4;i++) kf->F[i*NX + i] = 1.0;
for (int i=4;i<NX;i++) kf->F[i*NX + i] = 1.0; /* velocity terms identity */
/* set position update by velocity: u += u_dot, v += v_dot, s += s_dot */
kf->F[0*NX + 4] = 1.0;
kf->F[1*NX + 5] = 1.0;
kf->F[2*NX + 6] = 1.0;
/* r has no velocity */
/* H matrix (4x7) */
for (int i=0;i<NZ*NX;i++) kf->H[i] = 0.0;
kf->H[0*NX + 0] = 1.0;
kf->H[1*NX + 1] = 1.0;
kf->H[2*NX + 2] = 1.0;
kf->H[3*NX + 3] = 1.0;
/* default Q (process noise) */
for (int i=0;i<NX*NX;i++) kf->Q[i] = 0.0;
kf->Q[0*NX + 0] = 1.0; /* position */
kf->Q[1*NX + 1] = 1.0;
kf->Q[2*NX + 2] = 1.0;
kf->Q[3*NX + 3] = 1e-6;
kf->Q[4*NX + 4] = 0.1; /* velocity */
kf->Q[5*NX + 5] = 0.1;
kf->Q[6*NX + 6] = 0.1;
/* default R (measurement noise) 4x4 */
for (int i=0;i<NZ*NZ;i++) kf->R[i] = 0.0;
kf->R[0*NZ + 0] = 1.0; /* u */
kf->R[1*NZ + 1] = 1.0; /* v */
kf->R[2*NZ + 2] = 10.0; /* s (area) */
kf->R[3*NZ + 3] = 1e-6;
/* zero temp buffers */
memset(kf->tmp7x7, 0, sizeof(kf->tmp7x7));
memset(kf->tmp7x4, 0, sizeof(kf->tmp7x4));
memset(kf->tmp4x7, 0, sizeof(kf->tmp4x7));
memset(kf->tmp4x4, 0, sizeof(kf->tmp4x4));
}
/* Predict: optionally pass Q_new (7x7) pointer to override internal Q this step (NULL to use internal) */
void KalmanBoxTracker_predict(KalmanBoxTracker *kf, const double *Q_new) {
/* optionally update Q */
if (Q_new) memcpy(kf->Q, Q_new, sizeof(double) * NX * NX);
/* x = F * x */
double x_in[NX];
memcpy(x_in, kf->x, sizeof(x_in));
for (int i=0;i<NX;i++){
double s = 0.0;
for (int j=0;j<NX;j++) s += kf->F[i*NX + j] * x_in[j];
kf->x[i] = s;
}
/* P = F P F^T + Q */
/* tmp7x7 = F * P */
mat_mul(kf->F, NX, NX, kf->P, NX, kf->tmp7x7); /* tmp7x7 is NX x NX but computed using m=NX, c=NX */
/* tmp7x7 = tmp7x7 * F^T */
double Ft[NX*NX];
mat_transpose(kf->F, NX, NX, Ft);
double Pnew[NX*NX];
mat_mul(kf->tmp7x7, NX, NX, Ft, NX, Pnew);
/* Pnew += Q */
for (int i=0;i<NX*NX;i++) kf->P[i] = Pnew[i] + kf->Q[i];
}
/* Update with measurement z[4]; optionally pass R_new (4x4) pointer */
int KalmanBoxTracker_update(KalmanBoxTracker *kf, const double z[NZ], const double *R_new) {
if (R_new) memcpy(kf->R, R_new, sizeof(double) * NZ * NZ);
/* y = z - H x */
for (int i=0;i<NZ;i++){
double hx = 0.0;
for (int j=0;j<NX;j++) hx += kf->H[i*NX + j] * kf->x[j];
kf->tmp4x1[i] = z[i] - hx;
}
/* S = H P H^T + R (4x4) */
/* tmp4x7 = H * P (4x7) */
mat_mul(kf->H, NZ, NX, kf->P, NX, kf->tmp4x7);
double Ht[NX*NZ];
mat_transpose(kf->H, NZ, NX, Ht);
double S[NZ*NZ];
mat_mul(kf->tmp4x7, NZ, NX, Ht, NZ, S); /* (4x7)*(7x4) => 4x4 */
for (int i=0;i<NZ*NZ;i++) S[i] += kf->R[i];
/* compute S inverse */
double S_inv[NZ*NZ];
if (mat_inverse(S, NZ, S_inv) != 0) {
/* inversion failed (singular). fallback: add small diag and retry */
for (int i=0;i<NZ;i++) S[i*NZ + i] += 1e-6;
if (mat_inverse(S, NZ, S_inv) != 0) {
return -1; /* fail */
}
}
/* K = P H^T S^-1 -> compute P * H^T (7x4), then * S_inv (4x4) => 7x4 */
mat_mul(kf->P, NX, NX, Ht, NZ, kf->tmp7x4); /* tmp7x4 is NX x NZ */
double Kmat[NX * NZ];
mat_mul(kf->tmp7x4, NX, NZ, S_inv, NZ, Kmat); /* (7x4)*(4x4) => 7x4 */
/* x = x + K y (7x1) */
for (int i=0;i<NX;i++){
double s = 0.0;
for (int j=0;j<NZ;j++) s += Kmat[i*NZ + j] * kf->tmp4x1[j];
kf->x[i] += s;
}
/* P = (I - K H) P -> compute (I - K H) first (7x7), then * P */
double KH[NX * NX];
/* KH = K (7x4) * H (4x7) => 7x7 */
mat_mul(Kmat, NX, NZ, kf->H, NX, KH); /* careful dims: here mat_mul expects (r,m)*(m,c) with sizes, adapt: Kmat is 7x4, H is 4x7 */
/* compute I - KH */
double IminusKH[NX*NX];
for (int i=0;i<NX;i++){
for (int j=0;j<NX;j++){
double val = ((i==j)?1.0:0.0) - KH[i*NX + j];
IminusKH[i*NX + j] = val;
}
}
double Pnew[NX*NX];
mat_mul(IminusKH, NX, NX, kf->P, NX, Pnew);
/* copy back */
memcpy(kf->P, Pnew, sizeof(double)*NX*NX);
return 0;
}
/* Mahalanobis distance between measurement z and predicted measurement H x */
static double KalmanBoxTracker_mahalanobis(KalmanBoxTracker *kf, const double z[NZ]) {
/* y = z - H x */
double y[NZ];
for (int i=0;i<NZ;i++){
double hx = 0.0;
for (int j=0;j<NX;j++) hx += kf->H[i*NX + j] * kf->x[j];
y[i] = z[i] - hx;
}
/* S = H P H^T + R */
mat_mul(kf->H, NZ, NX, kf->P, NX, kf->tmp4x7);
double Ht[NX*NZ]; mat_transpose(kf->H, NZ, NX, Ht);
double S[NZ*NZ]; mat_mul(kf->tmp4x7, NZ, NX, Ht, NZ, S);
for (int i=0;i<NZ*NZ;i++) S[i] += kf->R[i];
double S_inv[NZ*NZ];
if (mat_inverse(S, NZ, S_inv) != 0) {
return 1e12; /* large distance if singular */
}
/* d^2 = y^T S^-1 y */
double tmp[NZ];
for (int i=0;i<NZ;i++){
tmp[i] = 0.0;
for (int j=0;j<NZ;j++) tmp[i] += S_inv[i*NZ + j] * y[j];
}
double d2 = 0.0;
for (int i=0;i<NZ;i++) d2 += y[i] * tmp[i];
return d2;
}
/* get current estimated measurement state [u,v,s,r] from state x */
void KalmanBoxTracker_get_state(KalmanBoxTracker *kf, double out[NZ]) {
out[0] = kf->x[0];
out[1] = kf->x[1];
out[2] = kf->x[2];
out[3] = kf->x[3];
}
请分析这段sort代码，给出注释
本文介绍了通过多个相关表如INOB、KSSK等来查询物料特性值的方法，并详细解释了如何利用这些表之间的关系来获取物料的具体特性值，包括历史记录。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
