五点三次平滑法滤波 C 和 matlab代码

 
 

  
  转载http://blog.csdn.net/liyuanbhu/article/details/11119081

 
 
 
 

  
  

 
 
 
 

  
  以五点三次平滑为例。取相邻的5个数据点，可以拟合出一条3次曲线来，然后用3次曲线上相应的位置的数据值作为滤波后结果。简单的说就是 Savitzky-Golay 滤波器 。只不过Savitzky-Golay 滤波器并不特殊考虑边界的几个数据点，而这个算法还特意把边上的几个点的数据拟合结果给推导了出来。

 
 
 
 

  
  首先是线性拟合平滑处理的代码. 分别为三点线性平滑、五点线性平滑和七点线性平滑。

 
 
 
 

  
  void 
  
  linearSmooth3 
  
  ( 
  
  double 
  
  in
  
  [], 
  
  double 
  
  out
  
  [], 
  
  int 
  
  N 
  
  )
 
 
 
 

  
  {
 
 
 
 
    
  
  int 
  
  i
  
  ;
 
 
 
 
    
  
  if 
  
  ( 
  
  N 
  
  < 
  
  3 
  
  )
 
 
 
 
    
  
  {
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  0
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  1
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  in
  
  [
  
  i
  
  ];
 
 
 
 
        
  
  }
 
 
 
 
    
  
  }
 
 
 
 
    
  
  else
 
 
 
 
    
  
  {
 
 
 
 
        
  
  out
  
  [
  
  0
  
  ] 
  
  = 
  
  ( 
  
  5.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  2.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  - 
  
  in
  
  [
  
  2
  
  ] 
  
  ) 
  
  / 
  
  6.0
  
  ;
 
 
 
 


 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  1
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  2
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  ( 
  
  in
  
  [
  
  i 
  
  - 
  
  1
  
  ] 
  
  + 
  
  in
  
  [
  
  i
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  1
  
  ] 
  
  ) 
  
  / 
  
  3.0
  
  ;
 
 
 
 
        
  
  }
 
 
 
 


 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  = 
  
  ( 
  
  5.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  2.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  - 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  ) 
  
  / 
  
  6.0
  
  ;
 
 
 
 
    
  
  }
 
 
 
 

  
  }
 
 
 
 


 
 
 
 

  
  void 
  
  linearSmooth5 
  
  ( 
  
  double 
  
  in
  
  [], 
  
  double 
  
  out
  
  [], 
  
  int 
  
  N 
  
  )
 
 
 
 

  
  {
 
 
 
 
    
  
  int 
  
  i
  
  ;
 
 
 
 
    
  
  if 
  
  ( 
  
  N 
  
  < 
  
  5 
  
  )
 
 
 
 
    
  
  {
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  0
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  1
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  in
  
  [
  
  i
  
  ];
 
 
 
 
        
  
  }
 
 
 
 
    
  
  }
 
 
 
 
    
  
  else
 
 
 
 
    
  
  {
 
 
 
 
        
  
  out
  
  [
  
  0
  
  ] 
  
  = 
  
  ( 
  
  3.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  2.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  + 
  
  in
  
  [
  
  2
  
  ] 
  
  - 
  
  in
  
  [
  
  4
  
  ] 
  
  ) 
  
  / 
  
  5.0
  
  ;
 
 
 
 
        
  
  out
  
  [
  
  1
  
  ] 
  
  = 
  
  ( 
  
  4.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  + 
  
  2 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  + 
  
  in
  
  [
  
  3
  
  ] 
  
  ) 
  
  / 
  
  10.0
  
  ;
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  2
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  3
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  ( 
  
  in
  
  [
  
  i 
  
  - 
  
  2
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  - 
  
  1
  
  ] 
  
  + 
  
  in
  
  [
  
  i
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  1
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  2
  
  ] 
  
  ) 
  
  / 
  
  5.0
  
  ;
 
 
 
 
        
  
  }
 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  = 
  
  ( 
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  2 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  + 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  ) 
  
  / 
  
  10.0
  
  ;
 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  = 
  
  ( 
  
  3.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  2.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  - 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  ) 
  
  / 
  
  5.0
  
  ;
 
 
 
 
    
  
  }
 
 
 
 

  
  }
 
 
 
 


 
 
 
 

  
  void 
  
  linearSmooth7 
  
  ( 
  
  double 
  
  in
  
  [], 
  
  double 
  
  out
  
  [], 
  
  int 
  
  N 
  
  )
 
 
 
 

  
  {
 
 
 
 
    
  
  int 
  
  i
  
  ;
 
 
 
 
    
  
  if 
  
  ( 
  
  N 
  
  < 
  
  7 
  
  )
 
 
 
 
    
  
  {
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  0
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  1
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  in
  
  [
  
  i
  
  ];
 
 
 
 
        
  
  }
 
 
 
 
    
  
  }
 
 
 
 
    
  
  else
 
 
 
 
    
  
  {
 
 
 
 
        
  
  out
  
  [
  
  0
  
  ] 
  
  = 
  
  ( 
  
  13.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  10.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  + 
  
  7.0 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  +
 
 
 
 
                  
  
  in
  
  [
  
  4
  
  ] 
  
  - 
  
  2.0 
  
  * 
  
  in
  
  [
  
  5
  
  ] 
  
  - 
  
  5.0 
  
  * 
  
  in
  
  [
  
  6
  
  ] 
  
  ) 
  
  / 
  
  28.0
  
  ;
 
 
 
 


 
 
 
 
        
  
  out
  
  [
  
  1
  
  ] 
  
  = 
  
  ( 
  
  5.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  + 
  
  3 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  + 
  
  2 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  +
 
 
 
 
                  
  
  in
  
  [
  
  4
  
  ] 
  
  - 
  
  in
  
  [
  
  6
  
  ] 
  
  ) 
  
  / 
  
  14.0
  
  ;
 
 
 
 


 
 
 
 
        
  
  out
  
  [
  
  2
  
  ] 
  
  = 
  
  ( 
  
  7.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  6.0 
  
  * 
  
  in 
  
  [
  
  1
  
  ] 
  
  + 
  
  5.0 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  +
 
 
 
 
                  
  
  3.0 
  
  * 
  
  in
  
  [
  
  4
  
  ] 
  
  + 
  
  2.0 
  
  * 
  
  in
  
  [
  
  5
  
  ] 
  
  + 
  
  in
  
  [
  
  6
  
  ] 
  
  ) 
  
  / 
  
  28.0
  
  ;
 
 
 
 


 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  3
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  4
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  ( 
  
  in
  
  [
  
  i 
  
  - 
  
  3
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  - 
  
  2
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  - 
  
  1
  
  ] 
  
  + 
  
  in
  
  [
  
  i
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  1
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  2
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  3
  
  ] 
  
  ) 
  
  / 
  
  7.0
  
  ;
 
 
 
 
        
  
  }
 
 
 
 


 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  = 
  
  ( 
  
  7.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  6.0 
  
  * 
  
  in 
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  5.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  +
 
 
 
 
                      
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  + 
  
  2.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  6
  
  ] 
  
  + 
  
  in
  
  [
  
  N 
  
  - 
  
  7
  
  ] 
  
  ) 
  
  / 
  
  28.0
  
  ;
 
 
 
 


 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  = 
  
  ( 
  
  5.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  +
 
 
 
 
                      
  
  2.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  + 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  - 
  
  in
  
  [
  
  N 
  
  - 
  
  7
  
  ] 
  
  ) 
  
  / 
  
  14.0
  
  ;
 
 
 
 


 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  = 
  
  ( 
  
  13.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  10.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  7.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  +
 
 
 
 
                      
  
  4 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  + 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  - 
  
  2 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  6
  
  ] 
  
  - 
  
  5 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  7
  
  ] 
  
  ) 
  
  / 
  
  28.0
  
  ;
 
 
 
 
    
  
  }
 
 
 
 

  
  }
 
 

然后是利用二次函数拟合平滑。

 
 

  
  void 
  
  quadraticSmooth5
  
  (
  
  double 
  
  in
  
  [], 
  
  double 
  
  out
  
  [], 
  
  int 
  
  N
  
  )
 
 
 
 

  
  {
 
 
 
 
    
  
  int 
  
  i
  
  ;
 
 
 
 
    
  
  if 
  
  ( 
  
  N 
  
  < 
  
  5 
  
  )
 
 
 
 
    
  
  {
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  0
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  1
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  in
  
  [
  
  i
  
  ];
 
 
 
 
        
  
  }
 
 
 
 
    
  
  }
 
 
 
 
    
  
  else
 
 
 
 
    
  
  {
 
 
 
 
        
  
  out
  
  [
  
  0
  
  ] 
  
  = 
  
  ( 
  
  31.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  9.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  - 
  
  3.0 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  - 
  
  5.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in
  
  [
  
  4
  
  ] 
  
  ) 
  
  / 
  
  35.0
  
  ;
 
 
 
 
        
  
  out
  
  [
  
  1
  
  ] 
  
  = 
  
  ( 
  
  9.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  13.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  + 
  
  12 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  + 
  
  6.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  - 
  
  5.0 
  
  *
  
  in
  
  [
  
  4
  
  ]) 
  
  / 
  
  35.0
  
  ;
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  2
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  3
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  ( 
  
  - 
  
  3.0 
  
  * 
  
  (
  
  in
  
  [
  
  i 
  
  - 
  
  2
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  2
  
  ]) 
  
  +
 
 
 
 
                      
  
  12.0 
  
  * 
  
  (
  
  in
  
  [
  
  i 
  
  - 
  
  1
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  1
  
  ]) 
  
  + 
  
  17 
  
  * 
  
  in
  
  [
  
  i
  
  ] 
  
  ) 
  
  / 
  
  35.0
  
  ;
 
 
 
 
        
  
  }
 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  = 
  
  ( 
  
  9.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  13.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  12.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  + 
  
  6.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  - 
  
  5.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  ) 
  
  / 
  
  35.0
  
  ;
 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  = 
  
  ( 
  
  31.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  9.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  - 
  
  3.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  - 
  
  5.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ]) 
  
  / 
  
  35.0
  
  ;
 
 
 
 
    
  
  }
 
 
 
 

  
  }
 
 
 
 


 
 
 
 


 
 
 
 

  
  void 
  
  quadraticSmooth7
  
  (
  
  double 
  
  in
  
  [], 
  
  double 
  
  out
  
  [], 
  
  int 
  
  N
  
  )
 
 
 
 

  
  {
 
 
 
 
    
  
  int 
  
  i
  
  ;
 
 
 
 
    
  
  if 
  
  ( 
  
  N 
  
  < 
  
  7 
  
  )
 
 
 
 
    
  
  {
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  0
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  1
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  in
  
  [
  
  i
  
  ];
 
 
 
 
        
  
  }
 
 
 
 
    
  
  }
 
 
 
 
    
  
  else
 
 
 
 
    
  
  {
 
 
 
 
        
  
  out
  
  [
  
  0
  
  ] 
  
  = 
  
  ( 
  
  32.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  15.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  - 
  
  4.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  -
 
 
 
 
                  
  
  6.0 
  
  * 
  
  in
  
  [
  
  4
  
  ] 
  
  - 
  
  3.0 
  
  * 
  
  in
  
  [
  
  5
  
  ] 
  
  + 
  
  5.0 
  
  * 
  
  in
  
  [
  
  6
  
  ] 
  
  ) 
  
  / 
  
  42.0
  
  ;
 
 
 
 


 
 
 
 
        
  
  out
  
  [
  
  1
  
  ] 
  
  = 
  
  ( 
  
  5.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  + 
  
  2.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  +
 
 
 
 
                  
  
  in
  
  [
  
  4
  
  ] 
  
  - 
  
  in
  
  [
  
  6
  
  ] 
  
  ) 
  
  / 
  
  14.0
  
  ;
 
 
 
 


 
 
 
 
        
  
  out
  
  [
  
  2
  
  ] 
  
  = 
  
  ( 
  
  1.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in 
  
  [
  
  1
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  +
 
 
 
 
                  
  
  3.0 
  
  * 
  
  in
  
  [
  
  4
  
  ] 
  
  + 
  
  1.0 
  
  * 
  
  in
  
  [
  
  5
  
  ] 
  
  - 
  
  2.0 
  
  * 
  
  in
  
  [
  
  6
  
  ] 
  
  ) 
  
  / 
  
  14.0
  
  ;
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  3
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  4
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  ( 
  
  -
  
  2.0 
  
  * 
  
  (
  
  in
  
  [
  
  i 
  
  - 
  
  3
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  3
  
  ]) 
  
  +
 
 
 
 
                       
  
  3.0 
  
  * 
  
  (
  
  in
  
  [
  
  i 
  
  - 
  
  2
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  2
  
  ]) 
  
  +
 
 
 
 
                      
  
  6.0 
  
  * 
  
  (
  
  in
  
  [
  
  i 
  
  - 
  
  1
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  1
  
  ]) 
  
  + 
  
  7.0 
  
  * 
  
  in
  
  [
  
  i
  
  ] 
  
  ) 
  
  / 
  
  21.0
  
  ;
 
 
 
 
        
  
  }
 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  = 
  
  ( 
  
  1.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in 
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  +
 
 
 
 
                      
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  + 
  
  1.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  6
  
  ] 
  
  - 
  
  2.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  7
  
  ] 
  
  ) 
  
  / 
  
  14.0
  
  ;
 
 
 
 


 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  = 
  
  ( 
  
  5.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  +
 
 
 
 
                      
  
  2.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  + 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  - 
  
  in
  
  [
  
  N 
  
  - 
  
  7
  
  ] 
  
  ) 
  
  / 
  
  14.0
  
  ;
 
 
 
 


 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  = 
  
  ( 
  
  32.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  15.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  3.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  -
 
 
 
 
                      
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  - 
  
  6.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  - 
  
  3.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  6
  
  ] 
  
  + 
  
  5.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  7
  
  ] 
  
  ) 
  
  / 
  
  42.0
  
  ;
 
 
 
 
    
  
  }
 
 
 
 

  
  }
 
 

最后是三次函数拟合平滑。

 
 

  
  /**
 
 
 
 

  
   * 五点三次平滑
 
 
 
 

  
   *
 
 
 
 

  
   */
 
 
 
 

  
  void 
  
  cubicSmooth5 
  
  ( 
  
  double 
  
  in
  
  [], 
  
  double 
  
  out
  
  [], 
  
  int 
  
  N 
  
  )
 
 
 
 

  
  {
 
 
 
 


 
 
 
 
    
  
  int 
  
  i
  
  ;
 
 
 
 
    
  
  if 
  
  ( 
  
  N 
  
  < 
  
  5 
  
  )
 
 
 
 
    
  
  {
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  0
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  1
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  in
  
  [
  
  i
  
  ];
 
 
 
 
    
  
  }
 
 
 
 


 
 
 
 
    
  
  else
 
 
 
 
    
  
  {
 
 
 
 
        
  
  out
  
  [
  
  0
  
  ] 
  
  = 
  
  (
  
  69.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  - 
  
  6.0 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  - 
  
  in
  
  [
  
  4
  
  ]) 
  
  / 
  
  70.0
  
  ;
 
 
 
 
        
  
  out
  
  [
  
  1
  
  ] 
  
  = 
  
  (
  
  2.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  27.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  + 
  
  12.0 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  - 
  
  8.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  + 
  
  2.0 
  
  * 
  
  in
  
  [
  
  4
  
  ]) 
  
  / 
  
  35.0
  
  ;
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  2
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  3
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  (
  
  -
  
  3.0 
  
  * 
  
  (
  
  in
  
  [
  
  i 
  
  - 
  
  2
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  2
  
  ])
  
  + 
  
  12.0 
  
  * 
  
  (
  
  in
  
  [
  
  i 
  
  - 
  
  1
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  1
  
  ]) 
  
  + 
  
  17.0 
  
  * 
  
  in
  
  [
  
  i
  
  ] 
  
  ) 
  
  / 
  
  35.0
  
  ;
 
 
 
 
        
  
  }
 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  = 
  
  (
  
  2.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  - 
  
  8.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  + 
  
  12.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  + 
  
  27.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  2.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ]) 
  
  / 
  
  35.0
  
  ;
 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  = 
  
  (
  
  - 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  - 
  
  6.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  69.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ]) 
  
  / 
  
  70.0
  
  ;
 
 
 
 
    
  
  }
 
 
 
 
    
  
  return
  
  ;
 
 
 
 

  
  }
 
 
 
 


 
 
 
 

  
  void 
  
  cubicSmooth7
  
  (
  
  double 
  
  in
  
  [], 
  
  double 
  
  out
  
  [], 
  
  int 
  
  N
  
  )
 
 
 
 

  
  {
 
 
 
 
    
  
  int 
  
  i
  
  ;
 
 
 
 
    
  
  if 
  
  ( 
  
  N 
  
  < 
  
  7 
  
  )
 
 
 
 
    
  
  {
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  0
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  1
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  in
  
  [
  
  i
  
  ];
 
 
 
 
        
  
  }
 
 
 
 
    
  
  }
 
 
 
 
    
  
  else
 
 
 
 
    
  
  {
 
 
 
 
        
  
  out
  
  [
  
  0
  
  ] 
  
  = 
  
  ( 
  
  39.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  8.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  - 
  
  4.0 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  - 
  
  4.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  +
 
 
 
 
                  
  
  1.0 
  
  * 
  
  in
  
  [
  
  4
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  5
  
  ] 
  
  - 
  
  2.0 
  
  * 
  
  in
  
  [
  
  6
  
  ] 
  
  ) 
  
  / 
  
  42.0
  
  ;
 
 
 
 
        
  
  out
  
  [
  
  1
  
  ] 
  
  = 
  
  ( 
  
  8.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  19.0 
  
  * 
  
  in
  
  [
  
  1
  
  ] 
  
  + 
  
  16.0 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  + 
  
  6.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  -
 
 
 
 
                  
  
  4.0 
  
  * 
  
  in
  
  [
  
  4
  
  ] 
  
  - 
  
  7.0
  
  * 
  
  in
  
  [
  
  5
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  6
  
  ] 
  
  ) 
  
  / 
  
  42.0
  
  ;
 
 
 
 
        
  
  out
  
  [
  
  2
  
  ] 
  
  = 
  
  ( 
  
  -
  
  4.0 
  
  * 
  
  in
  
  [
  
  0
  
  ] 
  
  + 
  
  16.0 
  
  * 
  
  in 
  
  [
  
  1
  
  ] 
  
  + 
  
  19.0 
  
  * 
  
  in
  
  [
  
  2
  
  ] 
  
  + 
  
  12.0 
  
  * 
  
  in
  
  [
  
  3
  
  ] 
  
  +
 
 
 
 
                  
  
  2.0 
  
  * 
  
  in
  
  [
  
  4
  
  ] 
  
  - 
  
  4.0 
  
  * 
  
  in
  
  [
  
  5
  
  ] 
  
  + 
  
  1.0 
  
  * 
  
  in
  
  [
  
  6
  
  ] 
  
  ) 
  
  / 
  
  42.0
  
  ;
 
 
 
 
        
  
  for 
  
  ( 
  
  i 
  
  = 
  
  3
  
  ; 
  
  i 
  
  <= 
  
  N 
  
  - 
  
  4
  
  ; 
  
  i
  
  ++ 
  
  )
 
 
 
 
        
  
  {
 
 
 
 
            
  
  out
  
  [
  
  i
  
  ] 
  
  = 
  
  ( 
  
  -
  
  2.0 
  
  * 
  
  (
  
  in
  
  [
  
  i 
  
  - 
  
  3
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  3
  
  ]) 
  
  +
 
 
 
 
                       
  
  3.0 
  
  * 
  
  (
  
  in
  
  [
  
  i 
  
  - 
  
  2
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  2
  
  ]) 
  
  +
 
 
 
 
                      
  
  6.0 
  
  * 
  
  (
  
  in
  
  [
  
  i 
  
  - 
  
  1
  
  ] 
  
  + 
  
  in
  
  [
  
  i 
  
  + 
  
  1
  
  ]) 
  
  + 
  
  7.0 
  
  * 
  
  in
  
  [
  
  i
  
  ] 
  
  ) 
  
  / 
  
  21.0
  
  ;
 
 
 
 
        
  
  }
 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  = 
  
  ( 
  
  -
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  16.0 
  
  * 
  
  in 
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  19.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  +
 
 
 
 
                      
  
  12.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  + 
  
  2.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  - 
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  6
  
  ] 
  
  + 
  
  1.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  7
  
  ] 
  
  ) 
  
  / 
  
  42.0
  
  ;
 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  = 
  
  ( 
  
  8.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  19.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  + 
  
  16.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  +
 
 
 
 
                      
  
  6.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  - 
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  - 
  
  7.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  6
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  7
  
  ] 
  
  ) 
  
  / 
  
  42.0
  
  ;
 
 
 
 
        
  
  out
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  = 
  
  ( 
  
  39.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  1
  
  ] 
  
  + 
  
  8.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  2
  
  ] 
  
  - 
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  3
  
  ] 
  
  -
 
 
 
 
                      
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  4
  
  ] 
  
  + 
  
  1.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  5
  
  ] 
  
  + 
  
  4.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  6
  
  ] 
  
  - 
  
  2.0 
  
  * 
  
  in
  
  [
  
  N 
  
  - 
  
  7
  
  ] 
  
  ) 
  
  / 
  
  42.0
  
  ;
 
 
 
 
    
  
  }
 
 
 
 

  
  }
 
 

Matlab代码

加高斯白噪

function [Y,NOISE] = noisegen(X,SNR)
% noisegen add white Gaussian noise to a signal.
% [Y, NOISE] = NOISEGEN(X,SNR) adds white Gaussian NOISE to X. The SNR is in dB.
%X是纯信号，SNR是要求的信噪比，Y是带噪信号，NOISE是叠加在信号上的噪声。
NOISE=randn(size(X));
NOISE=NOISE-mean(NOISE);
signal_power = 1/length(X)*sum(X.*X);
noise_variance = signal_power / ( 10^(SNR/10) );
NOISE=sqrt(noise_variance)/std(NOISE)*NOISE;
Y=X+NOISE;

五点三次平滑滤波

function y = smooth5_3(x, m)
% x为需要的数据
% m 为平滑次数
n=length(x);
a=x;
for k=1: m
b(1) = (69*a(1) +4*(a(2) +a(4)) -6*a(3) -a(5)) /70;
b(2) = (2* (a(1) +a(5)) +27*a(2) +12*a(3) -8*a(4)) /35;
for j=3:n-2
b (j) = (-3*(a(j-2) +a(j+2)) +12*(a(j-1) +a(j+1)) +17*a(j)) /35;
end
b (n-1) = (2*(a(n) +a(n-4)) +27*a(n-1) +12*a(n-2) -8*a(n-3)) /35;
b (n) = (69*a(n) +4* (a(n-1) +a(n-3)) -6*a(n-2) -a(n-4)) /70;
a=b;
end
y =a;