/*
 * --------------------------------------------------------------------------------------
 * Company              : 江苏中信博新能源科技股份有限公司上海子公司 2020-2021 版权所有
 * --------------------------------------------------------------------------------------
 * file Name            : calculate.c 
 * Description			    : calculate-lib
 * --------------------------------------------------------------------------------------
 * Tool Versions        : uVision V5.29.0.0
 * Target Device        : STM32F103|GD32F103
 * --------------------------------------------------------------------------------------
 * Engineer				      : tangtao
 * Revision				      : V0.0
 * Created Date			    : 2021.09.26
 * --------------------------------------------------------------------------------------
 * Engineer				      : 
 * Revision				      : 
 * Modified Date		    : 
 * Additional Comments	: 
 * --------------------------------------------------------------------------------------
 */

/*************************************************
  Function              : KalmanFilter()
  Description           : Kalman 
  Input                 : correct value, angle       
  Output                : angle        
  Return                : angle
  Engineer				      : tangtao
  Revision				      : V0.0
  Modified Date			    : 2021.09.23
  Additional Comments	  : 
*************************************************/
float KalmanFilter(float angle_rate,float accel_angle)
{
    const float delta_t = 0.02;
    const float Q = 0.01;
    const float R = 10;
    static float alpha_prior = 0.0;
    static float beta_prior  = 0.0;
    static float alpha_post = 90.0;
    static float beta_post  = 0.0;
    
    static float p_prior_1 = 0;
    static float p_prior_2 = 0;
    static float p_prior_3 = 0;
    static float p_prior_4 = 0;
    
    static float p_post_1 = 1;
    static float p_post_2 = 0;
    static float p_post_3 = 0;
    static float p_post_4 = 1;
    
    static float K1 = 0;
    static float K2 = 0;
    
    alpha_prior = alpha_post - (delta_t * beta_post) + (delta_t * angle_rate);
    beta_prior  = beta_post;
    
    p_prior_1 = p_post_1 - (delta_t * p_post_3) - (delta_t * p_post_2) + (delta_t * delta_t * p_post_4) + Q;
    p_prior_2 = p_post_2 - (delta_t * p_post_4);
    p_prior_3 = p_post_3 - (delta_t * p_post_4);
    p_prior_4 = p_post_4 + Q;
    
    K1 = p_prior_1 / (p_prior_1 + R);
    K2 = p_prior_3 / (p_prior_1 + R);
    
    alpha_post = alpha_prior + K1 * (accel_angle - alpha_prior);
    beta_post  = beta_prior + K2 * (accel_angle - alpha_prior);
    
    p_post_1 = (1 - K1) * p_prior_1;
    p_post_2 = (1 - K1) * p_prior_2;
    p_post_3 = -1 * K2  * p_prior_1 + p_prior_3;
    p_post_4 = -1 * K2  * p_prior_2 + p_prior_4;
    
    return alpha_post;
}

/*************************************************
  Function              : myln()
  Description           : calculate ln
  Input                 : a    
  Output                : ln a     
  Return                : ln a
  Engineer				      : tangtao
  Revision				      : V0.0
  Modified Date			    : 2021.09.26
  Additional Comments	  : 
*************************************************/
double myln(double a)
{
   int    N = 15;      //取前15+1项来估算
   int    k,nk;
   double x,xx,y;
   x  = (a-1)/(a+1);
   xx = x*x;
   nk = 2*N+1;
   y  = 1.0/nk;
   for(k=N;k>0;k--)
   {
      nk = nk - 2;
      y  = 1.0/nk + xx*y;    
   }
   return 2.0*x*y;
}

/*************************************************
  Function              : Get_Kelvin_Temperature()
  Description           : Get Kelvin Temperature 
  Input                 : Rntc   
  Output                : temperature    
  Return                : temperature
  Engineer				      : tangtao
  Revision				      : V0.0
  Modified Date			    : 2021.09.26
  Additional Comments	  : 
*************************************************/
#define T25 298.15
#define R25 10
#define B	  3950
float Get_Kelvin_Temperature(float Rntc)
{
	float N1,N2,N3,N4;
	N1 = (myln(R25)-myln(Rntc))/B;
	N2 = 1/T25 - N1;
	N3 = 1/N2;
	N4 = N3-273.15;
	
	return N4;
}