标签:matrix back efi ack for can cti error correct
#ifndef KALMANOFLIDAR_H
#define KALMANOFLIDAR_H
#include <Eigen\Dense>
#include <Eigen\Core>
#include <vector>
#include <iostream>
using namespace std;
using namespace Eigen;
class kalmanFilter
{
public:
vector<VectorXd> measurements;
kalmanFilter();
~kalmanFilter();
VectorXd measusre_data(VectorXd single_meas);
void test();
private:
VectorXd Measurement_update_0(vector<VectorXd> measurements, int i=0)
{
VectorXd z = measurements[i];
return z;
}
VectorXd Cal_measurement_perdcition_error_1_1(VectorXd &x_predict_i, VectorXd &z, MatrixXd &H)//H is the relation of input and output,output can be meausured, input need prediction
{
VectorXd y = z - H * x_predict_i;
return y;
}
VectorXd Cal_kalmen_gain_1_2(MatrixXd &P_predict_i, MatrixXd &H, MatrixXd &Rnoise)
{
VectorXd S = H * P_predict_i*H.transpose() + Rnoise;
VectorXd k = P_predict_i * H.transpose()*S.inverse();
return k;
}
VectorXd Correctpredcit_x_2_1(VectorXd &y, VectorXd &x_predict_i, VectorXd &k)
{
VectorXd x_predict_calibration = x_predict_i + k * y;
return x_predict_calibration;
}
MatrixXd Correctpredcit_P_2_2(MatrixXd &P_predict_i, VectorXd &k, MatrixXd &I,MatrixXd &H)
{
MatrixXd P_predict_calibration_i = (I - k * H)*P_predict_i;
return P_predict_calibration_i;
}
VectorXd Generate_NewPredic_xVector_3_1(VectorXd &x_predict_calibration, VectorXd &unosie, MatrixXd &F)//F is the relation of this time and next time
{
VectorXd x_predict_iplus1 = F * x_predict_calibration + unosie;
return x_predict_iplus1;
}
MatrixXd Generate_NewPredic_PMatrix_3_2(MatrixXd &P_predict_calibration_i, MatrixXd &F, MatrixXd &Qnoise)//F is the relation of this time and next time
{
MatrixXd P_predict_iplus1= F * P_predict_calibration_i*F.transpose() + Qnoise;
return P_predict_iplus1;
}
void filter(VectorXd &x, MatrixXd &P, VectorXd &unosie, MatrixXd &F, MatrixXd &H, MatrixXd &Rnoise, MatrixXd &I, MatrixXd &Qnoise, int N);
};
-------------------------------------------------------------------------------------------------------------------------------------------------------------------
#include "kalmanoflidar.h"
using namespace std;
using namespace Eigen;
kalmanFilter::kalmanFilter()
{
}
kalmanFilter::~kalmanFilter()
{
}
VectorXd kalmanFilter::measusre_data(VectorXd single_meas)
{
measurements.push_back(single_meas);
return single_meas;
}
void kalmanFilter::filter(VectorXd &x, MatrixXd &P, VectorXd &unosie, MatrixXd &F, MatrixXd &H, MatrixXd &Rnoise, MatrixXd &I, MatrixXd &Qnoise,int N)
{
VectorXd x_predict_i = x;
MatrixXd P_predict_i = P;
for (int i = 0;i < N;i++)
{
VectorXd z = Measurement_update_0(measurements, i);
VectorXd y = Cal_measurement_perdcition_error_1_1(x_predict_i, z, H);//H is the relation of input and output
VectorXd k = Cal_kalmen_gain_1_2(P_predict_i, H, Rnoise);
VectorXd x_predict_calibration_i = Correctpredcit_x_2_1(y, x_predict_i, k);//F is the relation of this time and next time
MatrixXd P_predict_calibration_i = Correctpredcit_P_2_2(P_predict_i, k, I,H);
VectorXd x_predict_iplus1 = Generate_NewPredic_xVector_3_1(x_predict_calibration_i, unosie, F);//F is the relation of this time and next time
MatrixXd P_predict_iplus1 = Generate_NewPredic_PMatrix_3_2(P_predict_calibration_i, F, Qnoise);//F is the relation of this time and next time
x_predict_i = x_predict_iplus1;
P_predict_i = P_predict_iplus1;
}
}
void kalmanFilter::test()
{
VectorXd x;
MatrixXd P;
VectorXd u;
MatrixXd F;
MatrixXd H;
MatrixXd R;
MatrixXd I;
MatrixXd Q;
printf("hello world\n");
VectorXd my_vector(2);
my_vector << 10, 20;
MatrixXd my_matrix(2, 2);
my_matrix << 1, 2, 3, 4;
cout << my_matrix << endl;
cout << my_vector << endl;
x = VectorXd(2);
x << 0, 0;
P = MatrixXd(2, 2);
P << 1000, 1000, -1000, 1000;
u = VectorXd(2);
u << 0, 0;
F = MatrixXd(2, 2);
F << 1, 1, 0, 1;
H = MatrixXd(1, 2);
H << 1, 0;
R = MatrixXd(1, 1);
R << 1;
I = MatrixXd::Identity(2, 2);
Q = MatrixXd(2, 2);
Q << 0, 0, 0, 0;
//create a list of measurements
VectorXd single_meas(1);
single_meas << 1;
measurements.push_back(single_meas);
single_meas << 5;
measurements.push_back(single_meas);
single_meas << 9;
measurements.push_back(single_meas);
single_meas << 13;
measurements.push_back(single_meas);
single_meas << 17;
measurements.push_back(single_meas);
single_meas << 21;
measurements.push_back(single_meas);
single_meas << 25;
measurements.push_back(single_meas);
single_meas << 29;
measurements.push_back(single_meas);
filter(x, P,u,F,H,R,I, Q,8);
;
}
#endif // KALMANOFLIDAR_H
标签:matrix back efi ack for can cti error correct
原文地址:https://www.cnblogs.com/gaoxianzhi/p/9456652.html