//
//  main.cpp
//  Kalman_filter
//
//  Created by Guillaume Frèche on 30/09/2019.
//  Copyright © 2019 Guillaume Frèche. All rights reserved.
//

#include <iostream>
//#include <exception>
#include <random>
#include <chrono>

#include "KalmanFilter.hpp"

int main(int argc, const char * argv[]) {
    using namespace std::chrono;
    auto start = high_resolution_clock::now();
    
    // Time step
    double dt = 1.;
    
    // Kalman filter's model matrices
    // Matrix F
    vector<vector<double>> matF = { {1, dt}, {0, 1} };
    // Matrix Rv
    double sigma_a = 0.2;
    vector<vector<double>> matRv = { {sigma_a*sigma_a*pow(dt, 4.)/4, sigma_a*sigma_a*pow(dt, 3.)/2},
        {sigma_a*sigma_a*pow(dt, 3.)/2, dt*dt*sigma_a*sigma_a} };
    // Matrix H
    vector<vector<double>> matH = { {1, 0} };
    // Matrix Ru
    double sigma_noise = 30;
    vector<vector<double>> matRu = { {sigma_noise*sigma_noise} };
    
    // Construct Kalman filter
    KalmanFilter filter = KalmanFilter(2,1, Matrix(matF), Matrix(matRv), Matrix(matH), Matrix(matRu));
    
    int niter = 500;
    
    // Generate random acceleration
    random_device rd;
    mt19937 generator(rd());
    normal_distribution<double> dist_accel(0, sigma_a);
    vector<double> accel;
    for (int i=0; i<niter; i++)
        accel.push_back(dist_accel(generator));
    
    // Generate true position and speed
    vector<double> true_pos = {0};
    double sigma_speed = 2;
    normal_distribution<double> dist_speed(0, sigma_speed);
    vector<double> true_speed = {dist_speed(generator)};
    for (int i=0; i<niter; i++){
        true_pos.push_back((*true_pos.end())+((*true_speed.end())*dt)+(accel[i]*dt*dt/2));
        true_speed.push_back((*true_speed.end())+(accel[i]*dt));
    }
    
    // Generate noisy observations
    normal_distribution<double> dist_obs(0, sigma_noise);
    vector<double> obs_pos;
    for (int i=0; i<true_pos.size(); i++){
        obs_pos.push_back(true_pos[i]+dist_obs(generator));
    }
    
    // Apply Kalman filter
    Matrix estim_state = filter.estimate_state(obs_pos);
    
    auto stop = high_resolution_clock::now();
    auto duration = duration_cast<microseconds>(stop - start);
    cout << "Program duration: " << ((double) duration.count())/double(1000000) << " sec " << endl;
    
    return 0;
}

//
//  Matrix.hpp
//  Kalman_filter
//
//  Created by Guillaume Frèche on 30/09/2019.
//  Copyright © 2019 Guillaume Frèche. All rights reserved.
//

#ifndef Matrix_hpp
#define Matrix_hpp

#include <iostream>
#include <vector>
#include <cmath>

using namespace std;

class Matrix{
private:
    vector<vector<double>> entries;
    int nb_row;
    int nb_col;
    
    void remove_row(int i);
    void remove_col(int j);
    // Sub matrix obtained by removing row i and column j
    Matrix sub_matrix(int i, int j) const;
    
public:
    // Empty constructor
    Matrix();
    // Copy constructor
    Matrix(const Matrix& mat);
    // Constructor converting a vector of vector into a matrix
    Matrix(const vector<vector<double>>& array);
    
    // Instantiate an all zero matrix
    static Matrix zeros(const int m, const int n);
    // Instantiate identity matrix
    static Matrix identity(const int m);
    
    // Display matrix entries
    void print();
    
    // Operators overload
    Matrix operator+(const Matrix& mat);
    Matrix operator-(const Matrix& mat);
    Matrix operator*(const Matrix& mat);
    Matrix operator*(const double val);
    Matrix operator/(const Matrix& mat);
    Matrix operator/(const double val);
    
    // Transpose matrix
    Matrix transpose() const;
    // Inverse matrix
    Matrix inverse();
    
    // Determinant
    double determinant() const;
    
    // Convert a 1x1 matrix into a double
    double to_double();

    vector<double> get_row(int i);
    vector<double> get_column(int j);
    void set_column(int j, vector<double> col);
};

#endif /* Matrix_hpp */

//
//  Matrix.cpp
//  Kalman_filter
//
//  Created by Guillaume Frèche on 30/09/2019.
//  Copyright © 2019 Guillaume Frèche. All rights reserved.
//

#include "Matrix.hpp"

// Empty constructor
Matrix::Matrix(){
    entries = {};
    nb_row = 0;
    nb_col = 0;
}

// Copy constructor
Matrix::Matrix(const Matrix& mat){
    entries = mat.entries;
    nb_row = mat.nb_row;
    nb_col = mat.nb_col;
}

// Constructor converting a vector of vector into a matrix
Matrix::Matrix(const vector<vector<double>>& array){
    entries = array;
    nb_row = (int) array.size();
    nb_col = (int) array[0].size();
}

// Instantiate an all zero matrix
Matrix Matrix::zeros(const int m, const int n){
    Matrix res;
    if((m>0)&&(n>0)){
        res.entries = vector<vector<double>>(m, vector<double>(n, 0));
        res.nb_row = m;
        res.nb_col = n;
    }
    else{
        throw invalid_argument ("Arguments must be a positive integer.");
    }
    return res;
}

// Instantiate identity matrix
Matrix Matrix::identity(const int m){
    Matrix res;
    if (m>0){
        res = zeros(m,m);
        for (int i=0; i<m; i++){
            res.entries[i][i] = 1;
        }
    }
    else{
        throw invalid_argument ("Argument must be a positive integer.");
    }
    return res;
}

// Display matrix entries
void Matrix::print(){
    for (int i=0; i < nb_row; i++){
        for (int j=0; j < nb_col; j++){
            cout << entries[i][j] << " ";
        }
        cout << endl;
    }
}

Matrix Matrix::operator+(const Matrix& mat){
    Matrix res;
    if ((nb_row!=mat.nb_row)||(nb_col!=mat.nb_col)){
        throw invalid_argument ( "Matrices should have the same dimensions." );
    }
    else{
        res = Matrix::zeros(nb_row, nb_col);
        for (int i=0; i< nb_row; i++){
            for (int j=0; j< nb_col; j++){
                res.entries[i][j] = entries[i][j] + mat.entries[i][j];
            }
        }
    }
    return res;
}

Matrix Matrix::operator-(const Matrix& mat){
    Matrix res;
    if ((nb_row!=mat.nb_row)||(nb_col!=mat.nb_col)){
        throw invalid_argument ( "Matrices should have the same dimensions " );
    }
    else{
        res = Matrix::zeros(nb_row, nb_col);
        for (int i=0; i< nb_row; i++){
            for (int j=0; j< nb_col; j++){
                res.entries[i][j] = entries[i][j] - mat.entries[i][j];
            }
        }
    }
    return res;
}

Matrix Matrix::operator*(const Matrix& mat){
    Matrix res;
    //pair<int, int> curr_shape = get_shape();
    //pair<int, int> mat_shape = mat.get_shape();
    
    if (nb_col!=mat.nb_row){
        throw invalid_argument ( "Matrices should have the same common dimension " );
    }
    else{
        res = Matrix::zeros(nb_row, mat.nb_col);
        for (int i=0; i< nb_row; i++){
            for (int j=0; j< mat.nb_col; j++){
                double temp = 0;
                for (int k=0; k < nb_col; k++){
                    temp += entries[i][k]*mat.entries[k][j];
                }
                res.entries[i][j] = temp;
            }
        }
    }
    return res;
}

Matrix Matrix::operator*(const double val){
    Matrix res = Matrix(entries);
    for (int i=0; i< nb_row; i++){
        for (int j=0; j< nb_col; j++){
            res.entries[i][j] = entries[i][j]*val;
        }
    }
    return res;
}

Matrix Matrix::operator/(const Matrix& mat){
    Matrix res;
    if ((nb_row!=mat.nb_row)||(nb_col!=mat.nb_col)){
        throw invalid_argument ( "Matrices should have the same dimensions " );
    }
    else{
        res = Matrix::zeros(nb_row, nb_col);
        Matrix mat_transp = mat.transpose();
        double det_mat = mat.determinant();
        for (int i=0; i < nb_row; i++){
            for (int j=0; j< nb_col; j++){
                Matrix temp_mat = Matrix(mat_transp);
                temp_mat.set_column(j, get_row(i));
                res.entries[i][j] = temp_mat.determinant()/det_mat;
            }
        }
    }
    return res;
}

Matrix Matrix::operator/(const double val){
    Matrix res = Matrix(entries);
    //pair<int,int> res_shape = res.get_shape();
    for (int i=0; i<nb_row; i++){
        for (int j=0; j<nb_col; j++){
            res.entries[i][j] = entries[i][j] / val;
            //res.set_entry(i, j, res.get_entry(i, j)/val);
        }
    }
    return res;
}

Matrix Matrix::transpose() const{
    Matrix res = Matrix::zeros(nb_col, nb_row);
    for(int i=0; i< nb_col; i++){
        for(int j=0; j<nb_row; j++){
            res.entries[i][j] = entries[j][i];
        }
    }
    return res;
}

Matrix Matrix::inverse(){
    //pair<int,int> curr_shape = get_shape();
    return Matrix::identity(nb_row)/Matrix(entries);
}

double Matrix::determinant() const{
    double res;
    if (nb_row!=nb_col){
        throw invalid_argument ( " Determinant only works for square matrices " );
    }
    else{
        if (nb_row==1){
            res = entries[0][0];
        }
        else{
            res = 0;
            for (int i=0; i<nb_row; i++){
                Matrix sub_mat = sub_matrix(i, 0);
                res += pow(-1, (double) i) * entries[i][0] * (sub_mat.determinant());
            }
        }
    }
    return res;
}

// Convert a 1x1 matrix into a double
double Matrix::to_double(){
    int res;
    if ((nb_row!=1)||(nb_col!=1)){
        throw invalid_argument ( "Matrix must be of shape 1x1" );
    }
    else{
        res = entries[0][0];
    }
    return res;
}

void Matrix::remove_row(int i){
    entries.erase(entries.begin()+i);
}

void Matrix::remove_col(int j){
    for (int i=0; i < entries.size();i++){
        entries[i].erase(entries[i].begin()+j);
    }
}

Matrix Matrix::sub_matrix(int i, int j) const{
    Matrix res = Matrix(entries);
    res.remove_row(i);
    res.remove_col(j);
    res.nb_row--;
    res.nb_col--;
    return res;
}

vector<double> Matrix::get_row(int i){
    return entries[i];
}

vector<double> Matrix::get_column(int j){
    vector<double> res;
    for (int i=0; i< nb_row; i++)
        res.push_back(entries[i][j]);
    return res;
}

void Matrix::set_column(int j, vector<double> col){
    for (int i=0; i<nb_row; i++)
        entries[i][j] = col[i];
}

//
//  KalmanFilter.hpp
//  Kalman_filter
//
//  Created by Guillaume Frèche on 30/09/2019.
//  Copyright © 2019 Guillaume Frèche. All rights reserved.
//

#ifndef KalmanFilter_hpp
#define KalmanFilter_hpp

#include <iostream>
#include "Matrix.hpp"

class KalmanFilter{
private:
    int state_dim;
    int obs_dim;
    Matrix F;
    Matrix Rv;
    Matrix H;
    Matrix Ru;
    
public:
    // Kalman filter constructor
    KalmanFilter(int m, int n, Matrix matF, Matrix matRv, Matrix matH, Matrix matRu) :
    state_dim(m), obs_dim(n), F(matF), Rv(matRv), H(matH), Ru(matRu) {}
    // Kalman filter state estimator
    Matrix estimate_state(vector<double> observ);
};

#endif /* KalmanFilter_hpp */

//
//  KalmanFilter.cpp
//  Kalman_filter
//
//  Created by Guillaume Frèche on 30/09/2019.
//  Copyright © 2019 Guillaume Frèche. All rights reserved.
//

#include "KalmanFilter.hpp"

Matrix KalmanFilter::estimate_state(vector<double> observ){
    Matrix estim_state = Matrix::zeros(state_dim, (int) observ.size());
    Matrix prior_innov_cov;
    Matrix posterior_innov_cov = Matrix::zeros(state_dim, state_dim);
    Matrix kalman_gain;
    Matrix prior_estim_state;
    Matrix posterior_estim_state;
    
    
    for (int k=1; k<observ.size(); k++){
        // Prior innovation covariance matrix update
        prior_innov_cov = (F*posterior_innov_cov*(F.transpose())) + Rv;
        // Kalman gain update
        double denom = ((H*prior_innov_cov*(H.transpose()))+Ru).to_double();
        kalman_gain = (prior_innov_cov*(H.transpose()))/denom;
        // posterior innovation covariance matrix update
        posterior_innov_cov = (Matrix::identity(state_dim)-(kalman_gain*H))*prior_innov_cov;
        
        // Previous posterior state estimate
        vector<vector<double>> temp_state;
        temp_state.push_back(estim_state.get_column(k));
        posterior_estim_state = Matrix(temp_state).transpose();
        // New prior state estimate
        prior_estim_state = F*posterior_estim_state;
        // New posterior state estimate
        posterior_estim_state = prior_estim_state + (kalman_gain*(observ[k]-((H*prior_estim_state).to_double())));
        
        // Store the new state estimate
        estim_state.set_column(k, posterior_estim_state.get_column(0));
    }
    
    return estim_state;
}