45 lines
2.9 KiB
C++
45 lines
2.9 KiB
C++
#pragma once
|
|
|
|
#include <Eigen/Dense>
|
|
#include <Eigen/src/Cholesky/LLT.h>
|
|
#include <Eigen/src/Core/Matrix.h>
|
|
|
|
template<int state_dim, int measure_dim>
|
|
class UnscentedKalman
|
|
{
|
|
public:
|
|
void predict();
|
|
void update(const Eigen::Vector<double, measure_dim> measurements);
|
|
Eigen::Vector<double, state_dim> predict_function_f(Eigen::Matrix<double, 1, state_dim> item); //build this last
|
|
Eigen::Vector<double, measure_dim> update_function_h(Eigen::Matrix<double, 1, state_dim> item); //build this last
|
|
void unscented_transform_f(Eigen::Matrix<double, 2 * state_dim + 1, state_dim> sigmas); //Add spot for Q and R matricies
|
|
void unscented_transform_h(Eigen::Matrix<double, 2 * state_dim + 1, measure_dim> sigmas);
|
|
const Eigen::Vector<double, state_dim> get_state() { return state_vector; }
|
|
const Eigen::Matrix<double, state_dim, state_dim> get_covar() { return covariance_matrix; }
|
|
const Eigen::Vector<double, state_dim> get_state_p() { return state_vector_p; }
|
|
const Eigen::Matrix<double, state_dim, state_dim> get_covar_p() { return covariance_matrix_p; }
|
|
private:
|
|
static const int sigma_dim = 2 * state_dim + 1;
|
|
const double kappa = 3 - state_dim;
|
|
const double alpha = 1;
|
|
const double beta = 2;
|
|
const double lambda_ = pow(alpha, 2) * (state_dim - kappa) - state_dim;
|
|
Eigen::Matrix<double, 1, sigma_dim> weights_m;
|
|
Eigen::Matrix<double, 1, sigma_dim> weights_c;
|
|
Eigen::Vector<double, state_dim> state_vector = Eigen::Vector<double, state_dim>::Ones();
|
|
Eigen::Matrix<double, state_dim, state_dim> covariance_matrix = Eigen::Matrix<double, state_dim, state_dim>::Identity();
|
|
Eigen::Vector<double, state_dim> state_vector_p = Eigen::Vector<double, state_dim>::Ones();
|
|
Eigen::Matrix<double, state_dim, state_dim> covariance_matrix_p = Eigen::Matrix<double, state_dim, state_dim>::Identity();
|
|
Eigen::Matrix<double, sigma_dim, state_dim> sigma_points = Eigen::Matrix<double, sigma_dim, state_dim>::Ones();
|
|
Eigen::Matrix<double, sigma_dim, state_dim> sigma_points_f = Eigen::Matrix<double, sigma_dim, state_dim>::Ones();
|
|
Eigen::Matrix<double, sigma_dim, measure_dim> sigma_points_h = Eigen::Matrix<double, sigma_dim, measure_dim>::Ones();
|
|
Eigen::Vector<double, state_dim> unscented_x_p = Eigen::Vector<double, state_dim>::Ones();
|
|
Eigen::Matrix<double, state_dim, state_dim> unscented_P_p = Eigen::Matrix<double, state_dim, state_dim>::Ones();
|
|
Eigen::Vector<double, measure_dim> unscented_x_z = Eigen::Vector<double, measure_dim>::Ones();
|
|
Eigen::Matrix<double, measure_dim, measure_dim> unscented_P_z = Eigen::Matrix<double, measure_dim, measure_dim>::Ones();
|
|
Eigen::Vector<double, measure_dim> measure_vector = Eigen::Vector<double, measure_dim>::Ones();
|
|
Eigen::Matrix<double, state_dim, state_dim> Q = Eigen::Matrix<double, state_dim, state_dim>::Random();
|
|
Eigen::Matrix<double, measure_dim, measure_dim> R = Eigen::Matrix<double, measure_dim, measure_dim>::Random();
|
|
|
|
};
|