waiwaylee
958ff6b11b
2、TWS点迹关联时增加点航距离门限，限制部分突然关联到很远的点迹的问题。 Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
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(double F[6][6], double Q[6][6], double X[6], double P[6][6], double X_pred[6], double P_pred[6][6]);
|
|
|
|
void kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,double X[4], double P[4][4]);//两点初始化卡尔曼滤波器
|
|
|
|
|
|
double d_cal_track_init(double Z[2],double X[4],double P[4][4],double T); //计算点 临时航迹的d
|
|
|
|
double d_cal_track_init_with_doppler(double Z[2],double X[4],double P[4][4],double T,double vr,double prt,double freq_ind);
|
|
|
|
double d_cal_track_init_EKF(double Z[3],double X[4],double P[4][4],double T,double prt,double freq_ind);
|
|
|
|
void kalman_filter_init_3dots(double Z0[2], double Z1[2],double Z2[2], double T1,double T2, double X[6], double P[6][6]);
|
|
|
|
double d_cal(double Z[2],double X[6],double P[6][6]);
|
|
|
|
double d_cal_with_doppler(double F[6][6], double Q[6][6] ,double Z[2],double X[6],double P[6][6],double vr,double prt,double freq_ind);
|
|
|
|
double d_cal_EKF(double F[6][6], double Q[6][6] ,double Z[3],double X[6],double P[6][6],double prt,double freq_ind);
|
|
|
|
void kalman_filter(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[2],double X_filter[6],double P_filter[6][6],double S_filter[2][2]);
|
|
|
|
void kalman_filter_EKF(double F[6][6], double Q[6][6], double X[6],double P[6][6],double Z[3],
|
|
double X_filter[6],double P_filter[6][6],double S_filter[2][2],
|
|
double prt,double freq_ind );
|
|
|
|
double Bind_speed(double prt,double freq_ind); //根据PRT 和 频率 计算不模糊速度
|
|
|
|
};
|
|
#endif // KALMAN_H
|