#include <stdio.h>
#include "matrice.h"
#include "kalman.h"

/*----------------------------------------------------------------*/
/* nstate = number of components of the filter's state vector     */
/* nobs = number of components of the filter's observation vector */
/* ncom = number of components of the filter's command vector     */
/*----------------------------------------------------------------*/

/*------------------------------------------------------------*
 |  Function state_covar_prediction() :  P=F*P*Ft+J*Cin*Jt+Q  |
 *------------------------------------------------------------*/

void state_covar_prediction()
{
        mat_transpose_S(F,Ft);
  mat_prod_S(F,P,FP);
  mat_prod_S(FP,Ft,FPFt);

        mat_transpose_S(J,Jt);
  mat_prod_S(J,Cin,JCin);
  mat_prod_S(JCin,Jt,JCinJt);

  mat_plus_S(FPFt,JCinJt,FPFtpJCinJt);
  mat_plus_S(FPFtpJCinJt,Q,P);
}

/*-----------------------------------------------------*
 |  Function state_covar(): P=(I-K*H)*P                |
 *-----------------------------------------------------*/

void state_covar()
{
  mat_prod_S(K,H,KH);
  mat_moins_S(I,KH,ImKH);
  mat_prod_S(ImKH,P,ImKHP);
  mat_copy_S(ImKHP,P);
}

/*-----------------------------------------------------*
 |  Function denom()                                   |
 *-----------------------------------------------------*/

void denom()
{
        mat_transpose_S(H,Ht);
  mat_prod_S(H,P,HP);
  mat_prod_S(HP,Ht,HPHt);
        e(Sinv,0,0) = 1.0/(e(HPHt,0,0)+e(R,0,0));
}

/*-----------------------------------------------------*
 |  Function mahalanobis_distance()                    |
 *-----------------------------------------------------*/

double mahalanobis_distance()
{
        return(e(INNOV,0,0)*e(INNOV,0,0)*e(Sinv,0,0));
}

/*--------------------------------------------------------------*
 |  Function kalman_gain() : K = P*Ht*Sinv = P*Ht*inv(H*P*Ht+R) |
 *--------------------------------------------------------------*/

void kalman_gain()
{
        mat_transpose_S(H,Ht);
  mat_prod_S(P,Ht,PHt);
        mat_scal_prod_S(PHt,e(Sinv,0,0));
        mat_copy_S(PHt,K);
}

/*-----------------------------------------------------*
 |  Function state_correction() : X = X+K*innov        |
 *-----------------------------------------------------*/

void state_correction()
{
        mat_copy_S(K,KK); /*--- copy K to leave it unmodified when exiting the function */
        mat_scal_prod_S(KK,e(INNOV,0,0));
        mat_plus_S(X,KK,XpKinnov);
        mat_copy_S(XpKinnov,X);
}

/*-----------------------------------------------------*
 |  Function denom_input_in_obs()                      |
 *-----------------------------------------------------*/

void denom_input_in_obs()
{
        /*--- compute H*P*Ht */
        mat_transpose_S(H,Ht);
  mat_prod_S(H,P,HP);
  mat_prod_S(HP,Ht,HPHt);

        /*--- compute D*Cin*Dt */
        mat_transpose_S(D,Dt);
  mat_prod_S(D,Cin,DCin);
  mat_prod_S(DCin,Dt,DCinDt);

        /*--- compute S=J*Cin*Dt */
        mat_prod_S(J,Cin,JCin);
        mat_prod_S(JCin,Dt,S);

        /*--- compute H*S */
        mat_prod_S(H,S,HS);

        /*--- compute St*Ht = (H*S)t */
        mat_transpose_S(HS,HSt);

        e(Sinv,0,0) = 1.0/(e(HPHt,0,0)+e(DCinDt,0,0)+e(HS,0,0)+e(HSt,0,0)+e(R,0,0));
}

/*-----------------------------------------------------*
 |  Function kalman_gain_input_in_obs()                |
 *-----------------------------------------------------*/

void kalman_gain_input_in_obs()
{
        /*--- compute S=J*Cin*Dt */
        mat_transpose_S(D,Dt);
        mat_prod_S(J,Cin,JCin);
        mat_prod_S(JCin,Dt,S);

        /*--- compute K = (P*Ht+S)*Sinv */
        mat_transpose_S(H,Ht);
  mat_prod_S(P,Ht,PHt);
        mat_plus_S(PHt,S,PHtpSSinv);

        mat_scal_prod_S(PHtpSSinv,e(Sinv,0,0));
        mat_copy_S(PHtpSSinv,K);
}

/*-----------------------------------------------------*
 |  Function state_covar_input_in_obs()                |
 *-----------------------------------------------------*/

void state_covar_input_in_obs()
{
        /*--- compute S=J*Cin*Dt and then St */
        mat_transpose_S(D,Dt);
        mat_prod_S(J,Cin,JCin);
        mat_prod_S(JCin,Dt,S);
        mat_transpose_S(S,St);

        /*--- compute P = P-K*(H*P+St) */
        mat_prod_S(H,P,HP);
        mat_plus_S(HP,St,HPpSt);
        mat_prod_S(K,HPpSt,KHPpSt);
        mat_moins_S(P,KHPpSt,PmKHPpSt);
        mat_copy_S(PmKHPpSt,P);
}