/**
* @file ALGO_Kalman_ATY.h
*
* @param Project ALGO_Algorithm_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2023 MZ-ATY
*       - This code follows:
*           - MZ-ATY Various Contents Joint Statement -
*               <a href="https://mengze.top/MZ-ATY_VCJS">
*                        https://mengze.top/MZ-ATY_VCJS</a>
*           - CC 4.0 BY-NC-SA -
*               <a href="https://creativecommons.org/licenses/by-nc-sa/4.0/">
*                        https://creativecommons.org/licenses/by-nc-sa/4.0/</a>
*       - Your use will be deemed to have accepted the terms of this statement.
*
* @brief Familiar functions of Kalman
*
* @version
*       - 1_01_220605 > ATY
*           -# Preliminary version, first Release
*       - 1_02_230108 > ATY
*           -# Finish and test 2D
********************************************************************************
*/

#ifndef __ALGO_Kalman_ATY_H
#define __ALGO_Kalman_ATY_H

#include "INCLUDE_ATY.h"

/******************************* For user *************************************/
// #define __DEBUG_ALGO_Kalman_ATY
// #define Kalman_2D
#define Kalman1D_TYPE 1

/******************************************************************************/

#if !Kalman1D_TYPE      // use below instead
typedef struct _ALGO_Kalman_S
{
    float L_P;      // estimate uncertainty (covariance) matrix of last state, not zero !
    float P;        // predicted estimate uncertainty (covariance) matrix for this state
    float O;        // kalman filter out
    float G;        // kalman gain
    float Q;        // process noise (covariance) matrix
    float R;        // measurement noise covariance matrix (Measurement Uncertainty)
}ALGO_Kalman1D_S;
#else
typedef struct _ALGO_Kalman1D_S
{
    float X;        // state
    float G;        // kalman gain
    float A;        // x(n)=A*x(n-1)+u(n),u(n)~N(0,q)
    float H;        // z(n)=H*x(n)+w(n),w(n)~N(0,r)
    float P;        // estimated error convariance
    float Q;        // process(predict) noise convariance
    float R;        // measure noise convariance
} ALGO_Kalman1D_S;
#endif /* 01 */

float ALGO_KalmanFilter1D(ALGO_Kalman1D_S* kfp, float input);


#ifdef Kalman_2D
typedef struct _ALGO_Kalman2D_S
{
    float X[2];     // state: [0]-angle [1]-diffrence of angle, 2x1
    float G[2];     // kalman gain, 2x1
    float A[2][2];  // X(n)=A*X(n-1)+U(n),U(n)~N(0,q), 2x2
    float H[2];     // Z(n)=H*X(n)+W(n),W(n)~N(0,r), 1x2
    float P[2][2];  // estimated error convariance,2x2 [p0 p1; p2 p3]
    float Q[2][2];  // process(predict) noise convariance,2x1 [q0,0; 0,q1]
    float R;        // measure noise convariance
} ALGO_Kalman2D_S;

float ALGO_KalmanFilter2D(ALGO_Kalman2D_S* kfp, float input);
#endif /* Kalman_2D */


#ifdef __DEBUG_ALGO_Kalman_ATY
extern ALGO_Kalman1D_S ALGO_kfp1D;
extern ALGO_Kalman2D_S ALGO_kfp2D;
extern float testSignalSigned[120];
extern float testSignalUnsigned[120];
extern const float code testSignalFloat[620];
extern uint16_t code testSignalUint16[5000];
void ALGO_Kalman_Test(void);
#endif /* __DEBUG_ALGO_Kalman_ATY */








#endif /* __ALGO_Kalman_ATY_H */

/******************************** End Of File *********************************/