44 lines
1.7 KiB
C++
44 lines
1.7 KiB
C++
#ifndef KALMAN_H
|
|
#define KALMAN_H
|
|
#include "parameters.h"
|
|
#include "struct.h"
|
|
#include <QVector>
|
|
#include <Eigen/Dense>
|
|
using namespace Eigen;
|
|
using namespace std;
|
|
|
|
|
|
class kalman
|
|
{
|
|
public:
|
|
|
|
void kalman_pred(float F[6][6], float Q[6][6], float X[6], float P[6][6], float X_pred[6], float P_pred[6][6]);
|
|
|
|
void kalman_filter_init_2dots(float Z0[2], float Z1[2], float T,float X[4], float P[4][4]);//两点初始化卡尔曼滤波器
|
|
|
|
|
|
float d_cal_track_init(float Z[2],float X[4],float P[4][4],float T); //计算点 临时航迹的d
|
|
|
|
float d_cal_track_init_with_doppler(float Z[2],float X[4],float P[4][4],float T,float vr,float prt,float freq_ind);
|
|
|
|
float d_cal_track_init_EKF(float Z[3],float X[4],float P[4][4],float T,float prt,float freq_ind);
|
|
|
|
void kalman_filter_init_3dots(float Z0[2], float Z1[2],float Z2[2], float T1,float T2, float X[6], float P[6][6]);
|
|
|
|
float d_cal(float Z[2],float X[6],float P[6][6]);
|
|
|
|
float d_cal_with_doppler(float F[6][6], float Q[6][6] ,float Z[2],float X[6],float P[6][6],float vr,float prt,float freq_ind);
|
|
|
|
float d_cal_EKF(float F[6][6], float Q[6][6] ,float Z[3],float X[6],float P[6][6],float prt,float freq_ind);
|
|
|
|
void kalman_filter(float F[6][6], float Q[6][6], float X[6],float P[6][6],float Z[2],float X_filter[6],float P_filter[6][6],float S_filter[2][2]);
|
|
|
|
void kalman_filter_EKF(float F[6][6], float Q[6][6], float X[6],float P[6][6],float Z[3],
|
|
float X_filter[6],float P_filter[6][6],float S_filter[2][2],
|
|
float prt,float freq_ind );
|
|
|
|
float Bind_speed(float prt,float freq_ind); //根据PRT 和 频率 计算不模糊速度
|
|
|
|
};
|
|
#endif // KALMAN_H
|