#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;
}
扫一扫
04-14
02-23
3572
3572
08-05
3313
3313
06-09
1422
1422
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
