diff --git a/CLOUDS/include/UnscentedKalman.h b/CLOUDS/include/UnscentedKalman.h deleted file mode 100644 index 6eecb5f..0000000 --- a/CLOUDS/include/UnscentedKalman.h +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once - -#include -#include -#include - -template -class UnscentedKalman -{ -public: - void predict(); - void update(const Eigen::Vector measurements); - Eigen::Vector predict_function_f(Eigen::Matrix item); //build this last - Eigen::Vector update_function_h(Eigen::Matrix item); //build this last - void unscented_transform_f(Eigen::Matrix sigmas); //Add spot for Q and R matricies - void unscented_transform_h(Eigen::Matrix sigmas); - const Eigen::Vector get_state() { return state_vector; } - const Eigen::Matrix get_covar() { return covariance_matrix; } - const Eigen::Vector get_state_p() { return state_vector_p; } - const Eigen::Matrix 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 weights_m; - Eigen::Matrix weights_c; - Eigen::Vector state_vector = Eigen::Vector::Ones(); - Eigen::Matrix covariance_matrix = Eigen::Matrix::Identity(); - Eigen::Vector state_vector_p = Eigen::Vector::Ones(); - Eigen::Matrix covariance_matrix_p = Eigen::Matrix::Identity(); - Eigen::Matrix sigma_points = Eigen::Matrix::Ones(); - Eigen::Matrix sigma_points_f = Eigen::Matrix::Ones(); - Eigen::Matrix sigma_points_h = Eigen::Matrix::Ones(); - Eigen::Vector unscented_x_p = Eigen::Vector::Ones(); - Eigen::Matrix unscented_P_p = Eigen::Matrix::Ones(); - Eigen::Vector unscented_x_z = Eigen::Vector::Ones(); - Eigen::Matrix unscented_P_z = Eigen::Matrix::Ones(); - Eigen::Vector measure_vector = Eigen::Vector::Ones(); - Eigen::Matrix Q = Eigen::Matrix::Random(); - Eigen::Matrix R = Eigen::Matrix::Random(); - -}; diff --git a/CLOUDS/src/UnscentedKalman.cpp b/CLOUDS/src/UnscentedKalman.cpp deleted file mode 100644 index 0539dff..0000000 --- a/CLOUDS/src/UnscentedKalman.cpp +++ /dev/null @@ -1,120 +0,0 @@ -// Kalman_Filter_Test.cpp : This file contains the 'main' function. Program execution begins and ends there. -// - -#include -#include -#include "UnscentedKalman.h" -#include "SystemSims.h" - -template -void UnscentedKalman::unscented_transform_f(Eigen::Matrix sigmas) -{ - for (int i = 0; i < state_dim; ++i) - { - unscented_x_p[i] = weights_m.dot(sigmas.col(i)); - } - Eigen::Matrix temp; - for (int i = 0; i < sigma_dim; ++i) - { - Eigen::Matrix Y = sigmas.row(i) - unscented_x_p.transpose(); - auto scalar = (weights_c[i] * (Y * Y.transpose())[0]); - temp.fill(scalar); - unscented_P_p = (unscented_P_p + temp); - } - unscented_P_p += Q; -} - -template -void UnscentedKalman::unscented_transform_h(Eigen::Matrix sigmas) -{ - for (int i = 0; i < measure_dim; ++i) - { - unscented_x_z[i] = weights_m.dot(sigmas.col(i)); - } - Eigen::Matrix temp; - for (int i = 0; i < sigma_dim; ++i) - { - Eigen::Matrix Y = sigmas.row(i) - unscented_x_z.transpose(); - auto scalar = (weights_c[i] * (Y * Y.transpose())[0]); - temp.fill(scalar); - unscented_P_z += temp; - } - unscented_P_z += R; -} - -template -void UnscentedKalman::predict() -{ - weights_m.fill(1 / (2 * (state_dim + lambda_))); - weights_c.fill(1 / (2 * (state_dim + lambda_))); - weights_m(0) = lambda_ / (state_dim + lambda_) + (1 - pow(alpha, 2) + beta); - weights_c(0) = lambda_ / (state_dim + lambda_); - Eigen::LLT> U((state_dim + lambda_) * covariance_matrix); - Eigen::Matrix U_decomposed = U.matrixU(); - sigma_points.row(0) = state_vector; - for (int i = 0; i < state_dim; ++i) - { - sigma_points.row(i + 1) = state_vector.transpose() + U_decomposed.row(i); - sigma_points.row(state_dim + i + 1) = state_vector.transpose() - U_decomposed.row(i); - } - for (int i = 0; i < sigma_dim; ++i) - { - sigma_points_f.row(i) = this->predict_function_f(sigma_points.row(i)); - } - this->unscented_transform_f(sigma_points_f); - state_vector_p = unscented_x_p; - covariance_matrix_p = unscented_P_p; -} - - -template -void UnscentedKalman::update(const Eigen::Vector measurements) -{ - for (int i = 0; i < sigma_dim; ++i) - { - sigma_points_h.row(i) = this->update_function_h(sigma_points_f.row(i)); - } - this->unscented_transform_h(sigma_points_h); - Eigen::Matrix covariance_x_z; - for (int i = 0; i < sigma_dim; ++i) - { - auto inner1 = (sigma_points_h.row(i) - unscented_x_z.transpose()); - auto inner2 = (sigma_points_f.row(i) - state_vector_p.transpose()); - auto comp = inner1.transpose() * inner2; - auto temp = weights_c[i] * comp.transpose(); - covariance_x_z += temp; - } - auto K = covariance_x_z * unscented_P_z.inverse(); - state_vector = state_vector_p + K * (measurements - unscented_x_z); - covariance_matrix = covariance_matrix_p - (K * unscented_P_z * K.transpose()); -} - - -template -Eigen::Vector UnscentedKalman::predict_function_f(Eigen::Matrix item) -{ - Eigen::Matrix F = Eigen::Matrix::Ones(); - return F * item.transpose(); -} - - -template -Eigen::Vector UnscentedKalman::update_function_h(Eigen::Matrix item) -{ - return { 1, 2, 3 }; -} - - - -UnscentedKalman<3, 3> filter; - -Eigen::Vector test2 = Eigen::Matrix::Random(); -Eigen::Vector test3 = Eigen::Matrix::Random(); -Eigen::Vector test4 = Eigen::Matrix::Random(); - -int main() -{ - -} - -