kalman滤波输出位置信息，输出位置以及分速度 C语言移植版本

#include <stdlib.h>
#include <stdio.h>

float data[101][3] = {
        0, -49.5300285913927, 22.0522882008860,
        0, -47.3171283081363, 21.3171649980885,
        0, -48.4247431409728, 23.1785056041819,
        0, -46.9647519096169, 21.5665519556675,
        0, -46.4034741475859, 22.3620158415987,
        0, -46.8672570741805, 21.1131094164378,
        0, -46.0198706880489, 21.9540356133334,
        0, -47.8006923410084, 21.4634019155481,
        0, -45.5401698332158, 23.4343372939823,
        0, -44.3176058996535, 23.9633333657462,
        0, -44.4322788329923, 22.6023302346577,
        0, -47.2350618805460, 23.8166071262426,
        0, -45.7742535347232, 22.8624459013036,
        0, -45.4032047826799, 23.8018879115707,
        0, -43.7598861093536, 24.9472701588556,
        0, -43.7082334584645, 24.4924705756177,
        0, -42.5225442254324, 23.9659339921157,
        0, -43.7693883262485, 24.1276750699993,
        0, -43.9558885619792, 24.8677776247080,
        0, -43.9533338104179, 25.0679369596614,
        0, -40.7122643316036, 25.1788189539167,
        0, -42.4573019141410, 26.0849575996079,
        0, -41.1869134758088, 26.9576360971058,
        0, -39.9728858721711, 26.3330043508771,
        0, -40.6841869919076, 26.6880673657286,
        0, -38.4322375184033, 26.1246182761594,
        0, -38.4420785104073, 26.1833020481511,
        0, -39.1326178995349, 27.3505557953922,
        0, -35.9522721336480, 27.8125617486977,
        0, -37.9975747105503, 29.2110704018994,
        0, -36.5237346649655, 30.5208077722121,
        0, -36.0305079211777, 30.8671926183546,
        0, -34.1165109718883, 29.8536590015631,
        0, -35.1140050746424, 29.6314351904963,
        0, -33.0787383204776, 31.6416938328594,
        0, -33.4806150115370, 32.0779821493933,
        0, -32.6308641513759, 32.4819441949359,
        0, -29.1022884370243, 32.9610650244620,
        0, -30.2137187283723, 31.5277612501983,
        0, -28.0858662449965, 33.7446812914110,
        0, -25.7423093311364, 33.5897817357812,
        0, -26.9500600241186, 33.3464896157648,
        0, -24.9609912215282, 33.5255562130643,
        0, -23.7476028988422, 36.2307234217835,
        0, -24.0694306954116, 34.8250292121462,
        0, -22.1780078407859, 35.3407097293550,
        0, -23.3409797971078, 37.4079616492333,
        0, -19.0049606064587, 38.6631681260627,
        0, -20.9301567743133, 38.1096746250538,
        0, -19.5352846443694, 39.2581069208514,
        0, -16.3086789591536, 38.2107711298582,
        0, -16.4295848982643, 41.2203712096587,
        0, -14.1641669437645, 40.6026522437244,
        0, -14.4959730191285, 41.6492016946314,
        0, -13.8082448235052, 40.3596890541454,
        0, -13.5510614637187, 43.3276115538814,
        0, -9.78682034356539, 43.8058566153286,
        0, -9.52174216299259, 42.5905610539612,
        0, -8.17304306038107, 44.4851354942878,
        0, -6.26057132283914, 44.5775008862897,
        0, -7.16238681256690, 45.9393483197092,
        0, -4.81675424664455, 46.2066422571496,
        0, -1.64911147654827, 46.9752801598391,
        0, -0.59426804397563, 47.0022132807644,
        0, -1.49867127556330, 47.9661602214642,
        0, -0.07939419844232, 48.0116073239746,
        0, 1.18940935487679, 51.4810268378612,
        0, 2.29128117427732, 51.4410996670248,
        0, 5.41243754065664, 53.0578407161457,
        0, 7.82229269975163, 51.5405371055826,
        0, 9.59169009876807, 54.6132917676766,
        0, 10.3448371743297, 55.0439736561663,
        0, 10.1952717530922, 55.2121825901199,
        0, 13.8470654941139, 55.6301067872808,
        0, 13.4059994613172, 55.9228412101373,
        0, 15.0372132172543, 58.2558391816023,
        0, 16.5143763708439, 57.5460084463165,
        0, 18.3281089937277, 57.9225612621757,
        0, 20.0930051939661, 60.9119885323297,
        0, 21.8849956890278, 61.5136065560696,
        0, 24.0699191005977, 62.5456375443575,
        0, 25.7857159965851, 63.2361824853015,
        0, 27.1302532376912, 63.4871448808746,
        0, 29.0241673840629, 64.3322862466125,
        0, 33.3716896211438, 66.5484203086260,
        0, 34.4128928982251, 66.0022835909675,
        0, 35.6429517708775, 68.4536193187019,
        0, 35.9977346710305, 69.3472205448169,
        0, 37.9681233701292, 70.5167916710315,
        0, 39.3093872324506, 70.4669098527153,
        0, 44.5552016431182, 71.8569841665841,
        0, 45.6468608707876, 73.8126832461344,
        0, 46.9911558670194, 73.3118924650564,
        0, 47.9737159597463, 73.1900564593384,
        0, 49.3648142516085, 76.6502496909979,
        0, 53.1899890371195, 76.9935662028070,
        0, 56.6717389398099, 77.2252209556365,
        0, 55.4417280922274, 80.2210098148199,
        0, 57.8511690022879, 78.9825144969493,
        0, 60.4871972745325, 81.3573532189058,
        0, 61.2959790783076, 82.3149719257247};

/// knn.h已经定义的量
#define MAINTAIN_FRAME 10 //保存多少帧的目标数据
#define MAX_CLUSTER_NUM 20 //单帧最大目标数
typedef struct knn_out_s {
    float x;
    float y;
    float v;
    int classifications;
    int id;
} knn_out_t;
///


//矩阵乘法
void gtrack_matrixMultiply(uint16_t m1, uint16_t m2, uint16_t m3, float *A, float *B, float *C);//矩阵乘法 C=A*B
//矩阵加法
void gtrack_matrixAdd(uint16_t rows, uint16_t cols, float *A, float *B, float *C); //矩阵加法 C=A+B
//矩阵转置相乘
void gtrack_matrixTransposeMultiply(uint16_t rows, uint16_t m, uint16_t cols, float *A, float *B, float *C);//C=A*B'
//矩阵相减
void gtrack_matrixSub(uint16_t m1, uint16_t m2, float *A, float *B, float *C); //矩阵相减
//矩阵求逆相关
int matrixInv(float *p, int num, float *invmat);

void matrixOpp(float *A, int m, int n, float *invmat);

void matrixInver(float *A, int m, int n, float *invmat);

float surplus(float A[], int m, int n);

typedef struct kalman_obj_s {
    int id; //类别
    int noFindCount; //目标消失的计数器
    int continueFindCount; //连续检测到该目标的计数器
    float posX; //位置x
    float posY; //位置y
    float dopplerX; //x方向速度
    float dopplerY; //y方向速度
    int classification; //类别
    float  kalmanGain[8]; //kalman增益 4*2
    float stateCovMat[16]; //状态协方差矩阵 4*4
} kalman_obj_t;

void kalmanCopy(kalman_obj_t *dst, kalman_obj_t *source);

int kalmanFilter(knn_out_t *currentObj, int currentNum,
                 kalman_obj_t *historyKalmanObj, int *hisTotalNum);

void initKalmanPar(kalman_obj_t *initObj);

void unitKalman(kalman_obj_t *objPre, kalman_obj_t *objIn, kalman_obj_t *objOut);

const float dt = 0.1; //fps = 1/dt

float stateTransMat[16] = {1.0f, 0.0f, dt, 0.0f,
                           0.0f, 1.0f, 0.0f, dt,
                           0.0f, 0.0f, 1.0f, 0.0f,
                           0.0f, 0.0f, 0.0f, 1.0f}; //状态转移矩阵 (4*4)

float state2MeasMat[8] = {1.0f, 0.0f, 0.0f, 0.0f,
                          0.0f, 1.0f, 0.0f, 0.0f}; //状态到测量的状态转移矩阵 (2*4)

const float u = 0.004; //状态转移控制量

float stateControlMat[4] = {dt * dt / 2 * u,
                            dt * dt / 2 * u,
                            dt * u,
                            dt * u}; //状态控制矩阵

const float measUncertainty = 1.0; //控制测量噪声控制量

float sysNoiseCovMat[16] = {dt * dt / 4.0f, 0.0f, dt * dt * dt / 2, 0,
                            0, dt * dt / 4, 0, dt * dt * dt / 2,
                            dt * dt * dt / 2, 0, dt * dt, 0,
                            0, dt * dt * dt / 2, 0, dt * dt}; // 4*4 系统噪声协方差矩阵

float measNoiseMat[4] = {measUncertainty, 0,
                         0, measUncertainty}; //测量噪声矩阵 2*2

float eyeMatrix[16] = {1, 0, 0, 0,
                       0, 1, 0, 0,
                       0, 0, 1, 0,
                       0, 0, 0, 1};//单位矩阵 (4*4) cons量

const float stateUncertainty = 5.0; //状态方差矩阵不确定度

const float continueFindTh = 10.0;//连续帧检测到目标计数 大于该值则作为有效目标输出

const float noFindTh = 10.0; //连续消失帧计数 大于该值则删除该目标


int kalmanFilter(knn_out_t *currentObj, int currentNum,
                 kalman_obj_t *historyKalmanObj, int *hisTotalNum) {
    /* currentObj 传入当前帧目标数据
       currentNum 当前帧目标个数
       historyKalmanObj 历史kalman目标信息
       hisTotalNum 历史帧长度（上一帧结束时的长度） 只保留上一帧的数据
    */

    int i, j;
    int is_find; //是否找到对应点的flag
    int validNum;
    int isUpdateId[MAX_CLUSTER_NUM]; //记录当前传入点已更新的 id
    int isUpdateNum; //记录由历史帧更新的目标点的个数
    int is_Update;//当前帧点被更新
    kalman_obj_t objPre;//上一帧滤波结果目标
    kalman_obj_t objIn;//当前传入目标
    kalman_obj_t objOut;//当前滤波结果目标

    validNum = 0;
    isUpdateNum = 0;

    //历史帧无数据 本帧无数据 直接返回
    if (currentNum == 0 && *hisTotalNum == 0) {
        return 0;
    }

    //本帧无数据 上一帧有数据 所有目标消失计时器+1
    if ((currentNum == 0) && (*hisTotalNum != 0)) {
        for (i = 0; i < *hisTotalNum; i++) {
            //首先判断消失帧数是否大于th 大于则删除该目标 否则保留
            if (historyKalmanObj[i].noFindCount > noFindTh) {
                continue;
            }
            kalmanCopy(&historyKalmanObj[validNum], &historyKalmanObj[i]);
            historyKalmanObj[validNum].noFindCount = historyKalmanObj[i].noFindCount + 1; //消失帧数
            historyKalmanObj[validNum].continueFindCount = 0;//连续检测计数为0
            validNum++; //有效目标数
        }
        *hisTotalNum = validNum;
        return 0;
    }

    //本帧有数据 历史帧无数据 相当于对新目标进行kalman进行滤波
    if ((currentNum != 0) && (*hisTotalNum == 0)) {
        //初始化kalman目标
        for (i = 0; i < currentNum; i++) {
            //初始化（相当于造一个上一帧的目标
            initKalmanPar(&objPre);

            //初始化当前帧单目标输入
            initKalmanPar(&objIn);
            objIn.posX = currentObj[i].x;
            objIn.posY = currentObj[i].y;

            //卡尔曼滤波
            unitKalman(&objPre, &objIn, &objOut);
            //将滤波结果存入历史帧kalman信息数据
            kalmanCopy(&historyKalmanObj[i], &objOut);
            historyKalmanObj[i].id = currentObj[i].id;
            historyKalmanObj[i].classification = currentObj[i].classifications;

            historyKalmanObj[i].noFindCount = 0; //消失计数=0
            historyKalmanObj[i].continueFindCount = 1; //连续检测计数+1
        }
        *hisTotalNum = currentNum;
        return 0;
    }

    //历史帧有数据 本帧有数据 进行相应的kalman滤波
    if ((currentNum != 0) && (*hisTotalNum != 0)) {
        //首先对历史目标点进行更新
        for (i = 0; i < *hisTotalNum; i++) {
            is_find = 0;
            //情况一：历史目标点与当前传入点有对应关系
            for (j = 0; j < currentNum; j++) { //在当前传入目标中寻找对应点
                if (historyKalmanObj[validNum].id == currentObj[j].id) {
                    //编号相同 则由pre数据和current数据进行kalman滤波
                    objIn.posX = currentObj[j].x;
                    objIn.posY = currentObj[j].y;
                    objIn.id = currentObj[j].id;
                    objIn.classification = currentObj[j].classifications;

                    //进行滤波
                    unitKalman(&historyKalmanObj[validNum], &objIn, &objOut);

                    //将滤波结果赋值给对应历史目标点
                    kalmanCopy(&historyKalmanObj[validNum], &objOut);
                    historyKalmanObj[validNum].noFindCount = 0; //消失计数=0
                    historyKalmanObj[validNum].continueFindCount++; //连续检测计数+1

                    //记录已经更新点的id
                    isUpdateId[isUpdateNum] = currentObj[j].id;//记录当前帧已更新的点
                    isUpdateNum++; //当前传入点已更新的计数
                    validNum++; //有效目标计数+1

                    //是否找到flag=1
                    is_find = 1;
                    break;//找到对应的点，则遍历下一个历史点
                }
            }

            if (is_find == 0) {
                //情况二：无对应点 则保留 且消失计数+1 连续检测计数=0
                //消失计数器大于阈值 删掉该目标
                if (historyKalmanObj[validNum].noFindCount > continueFindTh) {
                    continue;
                }
                //消失计数器小于阈值 复制该目标
                kalmanCopy(&historyKalmanObj[validNum], &historyKalmanObj[i]);
                historyKalmanObj[validNum].noFindCount++;
                historyKalmanObj[validNum].continueFindCount = 0;
                validNum++; //有效目标计数+1
            }
        }

        //对当前传入的 在历史帧无对应的点进行更新
        for (i = 0; i < currentNum; i++) {
            is_Update = 0;
            for (j = 0; j < isUpdateNum; j++) {
                if (currentObj[i].id == isUpdateId[j]) {
                    is_Update = 1;
                    break; //若该传入目标已经更新 则返回遍历下一个目标
                }
            }
            if(is_Update == 0){
                //当前帧传入的目标中 未进行更新的点
                initKalmanPar(&objPre);//初始化（相当于造一个上一帧的目标

                //初始化当前帧单目标输入
                objIn.posX = currentObj[i].x;
                objIn.posY = currentObj[i].y;

                //卡尔曼滤波
                unitKalman(&objPre, &objIn, &objOut);

                //将滤波结果存入历史帧kalman信息数据
                kalmanCopy(&historyKalmanObj[validNum], &objOut);
                historyKalmanObj[validNum].id = currentObj[i].id;
                historyKalmanObj[validNum].classification = currentObj[i].classifications;
                historyKalmanObj[validNum].noFindCount = 0; //消失计数=0
                historyKalmanObj[validNum].continueFindCount++; //连续检测计数+1
                validNum++;
            }
        }
    }
    *hisTotalNum = validNum;//重新赋值历史目标点数
    return 0;
}

void initKalmanPar(kalman_obj_t *initObj) {
    //初始化kalman参数
    int i, j;
    initObj->id = -1;
    initObj->posX = 1;
    initObj->posY = 1;
    initObj->dopplerX = 0;
    initObj->dopplerY = 0;
    initObj->noFindCount = 0;
    initObj->continueFindCount = 0;
    initObj->stateCovMat[0] = 5;
    initObj->stateCovMat[1] = 0;
    initObj->stateCovMat[2] = 0;
    initObj->stateCovMat[3] = 0;
    initObj->stateCovMat[4] = 0;
    initObj->stateCovMat[5] = 5;
    initObj->stateCovMat[6] = 0;
    initObj->stateCovMat[7] = 0;
    initObj->stateCovMat[8] = 0;
    initObj->stateCovMat[9] = 0;
    initObj->stateCovMat[10] = 5;
    initObj->stateCovMat[11] = 0;
    initObj->stateCovMat[12] = 0;
    initObj->stateCovMat[13] = 0;
    initObj->stateCovMat[14] = 0;
    initObj->stateCovMat[15] = 5;
    for (i = 0; i < 4; i++) {
        initObj->kalmanGain[i] = 0;
    }
}

void unitKalman(kalman_obj_t *objPre, kalman_obj_t *objIn, kalman_obj_t *objOut) {
    //对传入的单个目标进行kalman滤波
    //objPre 上一时刻状态
    //objIn输入状态
    //objOut输出状态
    int i;
    float temp4x1[4];

    float temp4x4[16];

    float temp2x2[4];

    float *inverR = NULL;

    float resultSH[8];
    float temp2x4[8];

    float temp2x1[2];
    float residual[2]; // 输入与预测的残差
    float modification[4]; // kalman修正值
    float covErr[16]; // E(4*4)-result_KH = (4*4)
    float updateState[4]; //更新后状态
    float updateCovMat[16];//更新后协方差矩阵(4*4)
    float stateUncertainty; // 状态不确定控制变量
    float kalmanIn[2]; //kalman输入 2*1
    float kalmanOut[2]; //kalman结果 2*1

    int flag;


    objOut->id = objIn->id;
    //上一时刻状态
    float initState[4] = {objPre->posX, objPre->posY, objPre->dopplerX, objPre->dopplerY};

    //kalman输入值
    kalmanIn[0] = objIn->posX; //位置x
    kalmanIn[1] = objIn->posY; //位置y

    /*                        预测步骤                        */
    //预测状态 x = F*x + Bu
    // (4*4) * (4*1) 由上一时刻状态预测当前状态
    gtrack_matrixMultiply(4, 4, 1, stateTransMat, initState, temp4x1);

    // 状态(4*1) + 控制(4*1) = 预测状态 (4*1)
    gtrack_matrixAdd(4, 1, temp4x1, stateControlMat, initState);//先验状态矩阵

    // 预测状态协方差矩阵 S = A*S*A' + Q
    // A(4*4) * S(4*4) = (4*4)
    gtrack_matrixMultiply(4, 4, 4, stateTransMat, objPre->stateCovMat, temp4x4);

    //A*S(4*4) * A'(4*4) = (4*4)
    gtrack_matrixTransposeMultiply(4, 4, 4, temp4x4, stateTransMat, objPre->stateCovMat);

    //A*S*A'(4*4) + Q(4*4) = (4*4)
    gtrack_matrixAdd(4, 4, objPre->stateCovMat, sysNoiseCovMat, objPre->stateCovMat); //先验状态协方差矩阵

    /*                        更新步骤                        */
    //更新kalman增益 K = S*H'*inv(H*S*H'+R)
    //H(2*4) * S(4*4) = (2*4)
    gtrack_matrixMultiply(2, 4, 4, state2MeasMat, objPre->stateCovMat, temp2x4);

    //H*S(2*4) * H'(4*2) = (2*2)
    gtrack_matrixTransposeMultiply(2, 4, 2, temp2x4, state2MeasMat, temp2x2);

    //H*S*H'(2*2) + R(2*2) = (2*2)
    gtrack_matrixAdd(2, 2, temp2x2, measNoiseMat, temp2x2);

    //逆矩阵
    inverR = (float *) malloc(sizeof(float) * 2 * 2);
    //inv(H*S*H'+R) = (2*2)
    flag = matrixInv(temp2x2, 2, inverR);

    // S(4*4) * H'(4*2) = (4*2)
    gtrack_matrixTransposeMultiply(4, 4, 2, objPre->stateCovMat, state2MeasMat, resultSH);

    //S*H'(4*2) * inv(H*S*H'+R) (2*2) = (4*2)
    gtrack_matrixMultiply(4, 2, 2, resultSH, inverR, objOut->kalmanGain); //计算卡尔曼增益
    free(inverR);

    //更新状态x = x + K*(kalmanIn - H*x)
    // H(2*4) * x(4*1) = (2*1)
    gtrack_matrixMultiply(2, 4, 1, state2MeasMat, initState, temp2x1);

    // kalmanIn(2*1) - H*x(2*1) = (2*1)
    gtrack_matrixSub(2, 2, kalmanIn, temp2x1, residual); //残差

    // K(4*2)*(input - H*x)(2*1) = (4*1)
    gtrack_matrixMultiply(4, 2, 1, objOut->kalmanGain, residual, modification);//kalman修正

    // x(4*1) + K*(input-H*x)(4*1) = (4*1)
    gtrack_matrixAdd(4, 1, initState, modification, updateState);//后验状态

    //对输出目标进行修改
    objOut->posX = updateState[0];//更新x
    objOut->posY = updateState[1];//更新y
    objOut->dopplerX = updateState[2];//更新vx
    objOut->dopplerY = updateState[3];//更新vy

    //更新协方差矩阵 S = (eye(size(S,1)) - K*H)*S
    //K(4*2) * H(2*4) = (4*4)
    gtrack_matrixMultiply(4, 2, 4, objOut->kalmanGain, state2MeasMat, temp4x4);

    // E(4*4) - K*H(4*4) = (4*4)
    gtrack_matrixSub(4, 4, eyeMatrix, temp4x4, covErr);

    //(E-K*H)(4*4) * S(4*4) = (4*4)
    gtrack_matrixMultiply(4, 4, 4, covErr, objPre->stateCovMat, updateCovMat);
    for (i = 0; i < 16; i++) {
        objOut->stateCovMat[i] = updateCovMat[i];
    }
    //类别
    objOut->classification = objPre->classification;
    //消失检测数
    objOut->noFindCount = objPre->noFindCount;
    //连续检测数
    objOut->continueFindCount = objPre->continueFindCount;
}

void kalmanCopy(kalman_obj_t *dst, kalman_obj_t *source) {
    //将source的信息拷贝进dst 除两个计数器
    int i;
    dst->id = source->id;
    dst->posX = source->posX;
    dst->posY = source->posY;
    dst->dopplerX = source->dopplerX;
    dst->dopplerY = source->dopplerY;
    for (i = 0; i < 8; i++) {
        dst->kalmanGain[i] = source->kalmanGain[i];
    }
    for (i = 0; i < 16; i++) {
        dst->stateCovMat[i] = source->stateCovMat[i];
    }
    dst->noFindCount = source->noFindCount;
    dst->continueFindCount = source->continueFindCount;
    dst->classification = source->classification;
}


void gtrack_matrixMultiply(uint16_t rows, uint16_t m, uint16_t cols, float *A, float *B, float *C) {
    /* C(rows*cols) = A(rows*m)*B(m*cols) */
    uint16_t i, j, k;
    for (i = 0; i < rows; i++) {
        for (j = 0; j < cols; j++) {
            C[i * cols + j] = 0;
            for (k = 0; k < m; k++) {
                C[i * cols + j] += A[i * m + k] * B[k * cols + j];
            }
        }
    }
}

void gtrack_matrixAdd(uint16_t rows, uint16_t cols, float *A, float *B, float *C) {
    /* C(rows*cols) = A(rows*cols) + B(rows*cols) */
    uint16_t i;
    for (i = 0U; i < rows * cols; i++) {
        C[i] = A[i] + B[i];
    }
}

void gtrack_matrixTransposeMultiply(uint16_t rows, uint16_t m, uint16_t cols, float *A, float *B, float *C) {
    /* C(rows*cols) = A(rows*m)*B(cols*m)T */
    uint16_t i, j, k;
    for (i = 0; i < rows; i++) {
        for (j = 0; j < cols; j++) {
            C[i * cols + j] = 0;
            for (k = 0; k < m; k++) {
                C[i * cols + j] += A[i * m + k] * B[k + j * m];
            }
        }
    }
}

void matrixOpp(float A[], int m, int n, float *invmat) {
    int i, j, x, y, k;
    float *SP = NULL, *AB = NULL, *B = NULL, X;
    SP = (float *) malloc(m * n * sizeof(float));
    AB = (float *) malloc(m * n * sizeof(float));
    B = (float *) malloc(m * n * sizeof(float));
    X = surplus(A, m, n);
    X = 1 / X;

    for (i = 0; i < m; i++)
        for (j = 0; j < n; j++) {
            for (k = 0; k < m * n; k++)
                B[k] = A[k];
            {
                for (x = 0; x < n; x++)
                    B[i * n + x] = 0;
                for (y = 0; y < m; y++)
                    B[m * y + j] = 0;
                B[i * n + j] = 1;
                SP[i * n + j] = surplus(B, m, n);
                AB[i * n + j] = X * SP[i * n + j];
            }
        }
    matrixInver(AB, m, n, invmat);
    free(SP);
    free(AB);
    free(B);
}

void matrixInver(float A[], int m, int n, float *invmat) {
    int i, j;
    float *B = invmat;

    for (i = 0; i < n; i++)
        for (j = 0; j < m; j++)
            B[i * m + j] = A[j * n + i];
}

float surplus(float A[], int m, int n) {
    int i, j, k, p, r;
    float X, temp = 1, temp1 = 1, s = 0, s1 = 0;
    if (n == 2) {
        for (i = 0; i < m; i++)
            for (j = 0; j < n; j++)
                if ((i + j) % 2)
                    temp1 *= A[i * n + j];
                else
                    temp *= A[i * n + j];
        X = temp - temp1;
    } else {
        for (k = 0; k < n; k++) {
            for (i = 0, j = k; i < m, j < n; i++, j++)
                temp *= A[i * n + j];
            if (m - i) {
                for (p = m - i, r = m - 1; p > 0; p--, r--)
                    temp *= A[r * n + p - 1];
            }
            s += temp;
            temp = 1;
        }

        for (k = n - 1; k >= 0; k--) {
            for (i = 0, j = k; i < m, j >= 0; i++, j--)
                temp1 *= A[i * n + j];
            if (m - i) {
                for (p = m - 1, r = i; r < m; p--, r++)
                    temp1 *= A[r * n + p];
            }
            s1 += temp1;
            temp1 = 1;
        }
        X = s - s1;
    }
    return X;
}

int matrixInv(float *p, int num, float *invmat) {
    if (p == NULL || invmat == NULL) {
        return 1;
    }
    if (num > 10) {
        return 2;
    }
    matrixOpp(p, num, num, invmat);
    return 0;
}

void gtrack_matrixSub(uint16_t rows, uint16_t cols, float *A, float *B, float *C) {
    /* C(rows*cols) = A(rows*cols) - B(rows*cols) */
    uint16_t i;
    for (i = 0U; i < rows * cols; i++) {
        C[i] = A[i] - B[i];
    }
}

void printfObj(kalman_obj_t A) {
    printf("id = %d ", A.id);
    printf("x = %.2f ", A.posX);
    printf("y = %.2f ", A.posY);
    printf("vx = %.2f ", A.dopplerX);
    printf("vy = %.2f \n", A.dopplerY);
}

int main() {
    int i, j;
    int hisTotalNum;

    kalman_obj_t historyKalmanObj[100];//存储历史kalman滤波结果 //12.5Kb
    hisTotalNum = 0; //初始化历史帧长度

    knn_out_t temp[20];
    knn_out_t *currentObj = temp; //定义输入 //0.39Kb
    int currentNum;

    //begin kalman
    int N = 101;
    for (i = 0; i < N; i++) {
        printf("第 %d 帧\n", i);
        //kalman输入赋值
        currentObj[0].id = data[i][0];
        currentObj[0].x = data[i][1];
        currentObj[0].y = data[i][2];
        currentObj[0].v = 0.5;
        currentObj[0].classifications = 1;

        currentNum = 1;

        //进行滤波
        kalmanFilter(currentObj, currentNum, historyKalmanObj, &hisTotalNum);

        for (j = 0; j < hisTotalNum; j++) {
            printf("滤波结果：%.2f %.2f %.2f %.2f %d\n", historyKalmanObj[j].posX, historyKalmanObj[j].posY,
                   historyKalmanObj[j].dopplerX, historyKalmanObj[j].dopplerY, historyKalmanObj[j].id);
        }
        printf("\n");

    }
    return 0;
}