Smoothing Derivative in C++?

[CroCo]

With the method in this link
http://www.cplusplus.com/forum/general/103788/
I've got the derivative of stream experimental data which represents position(x, y, z). I got x',y',z'. However, from the results that I've got, it seems that the data is corrupted by noise, so I need to smooth them. Fortunately, the device gives me the actual velocities, so I'm able to compare the method with the actual velocities.

Velocities is being captured by the device
http://s21.postimg.org/tcg21xzdj/actual_Velocities.png

Velocities by the aforementioned method
http://s21.postimg.org/7wjc1qzbr/cvelocities.png

filtered Velocities of the aforementioned method by using Matlab ( using Savitzky-Golay method)
http://s22.postimg.org/z9t4a9q5d/filtered.png

I found this method Savitzky-Golay for smoothing the derivative. I'm trying to understand the method and implement it in C++. Also, each position is a function of time x(t_n), y(t_n), z(t_n) and the sampling rate is 1kHz (1msec). The unit of position is millimeter.

Any suggestions will be appreciated.

[andywestken]

Not an alg I've come across.

But Google found this (googling: "Savitzky-Golay for smoothing c++")

sgsmooth.C
http://www.ks.uiuc.edu/Research/vmd/mailing_list/vmd-l/att-13397/sgsmooth.C

Andy

Also see:

how to use Savitzky-Golay smooth coefficient to calculate derivatives
http://stackoverflow.com/questions/3571222/how-to-use-savitzky-golay-smooth-coefficient-to-calculate-derivatives

Etc.

[CroCo]

Thank Andy,
I implemented what in the following link
http://en.wikipedia.org/wiki/Savitzky–Golay_filter_for_smoothing_and_differentiation

But without any good results. After filtering the data five times this is what I'v got
red represents smoothed data and the blue is raw data.

http://s9.postimg.org/5duffykan/Untitled.png

This is my code

#include <iostream>
#include <fstream>
#include <vector>

double sg_filter(const double raw_data[5])
{
    const int i = 2;
    double result(0);
    
    result = (1.0/35.0) * ( - 3.0*raw_data[i-2]  
                            + 12.0*raw_data[i-1]
                            + 17.0*raw_data[i]  
                            + 12.0*raw_data[i+1] 
                            - 3.0*raw_data[i+2] );
    return result;
}

int main()
{
    std::ifstream xdata;
    std::ofstream xdot;
    double temp[5] = {0};
    std::vector<double> x, xd;
    
    
    xdata.open("smoothed4.txt");
    xdot.open("smoothed5.txt");
    
    if (xdata.fail())
    {
        std::cout << "Error in open xdata file";
    }
    if (xdot.fail())
    {
        std::cout << "Error in Opening xdot file";
    }
    
    double line;
    while ( xdata >> line )
    {
        x.push_back(line);
    }
    
    int xdata_counter = 0;
    
    while ( xdata_counter < (x.size() - 2) )
    {
        
        temp[2] = x[xdata_counter];
        temp[3] = x[xdata_counter+1];
        temp[4] = x[xdata_counter+2];
        
        xdot << sg_filter(temp) << std::endl;
        temp[0] = temp[1];
        temp[1] = temp[2];
        
        xdata_counter++;
    }


    
    
    return 0;
}

[andywestken]

Sorry, but diagnosing this problem would be too time consuming for me. You might have to track down a forum where people who work with signal processing live and ask the question there.

Unless someone who knows the field just happens to pass by here!?

Andy

[CroCo]

@Andy,
Thanks again for being helpful. I got it to work with good results. This is what I did

#include <iostream>
#include <vector>
#include <fstream>
#include "sg_filer.h"

double sg_filter(const double raw_data[25])
{
    const int i = 12;
    double result(0);
    
    result = (1.0/5175.0) * (- 253.0*raw_data[i-12]
                             - 138.0*raw_data[i-11]
                             -  33.0*raw_data[i-10]
                             +  62.0*raw_data[i-9]
                             + 147.0*raw_data[i-8]
                             + 222.0*raw_data[i-7]
                             + 287.0*raw_data[i-6]
                             + 343.0*raw_data[i-5]
                             + 387.0*raw_data[i-4]
                             + 422.0*raw_data[i-3]
                             + 447.0*raw_data[i-2]  
                             + 462.0*raw_data[i-1]
                             
                             + 467.0*raw_data[i]
                             
                             + 462.0*raw_data[i+1] 
                             + 447.0*raw_data[i+2]
                             + 422.0*raw_data[i+3] 
                             + 387.0*raw_data[i+4]
                             + 343.0*raw_data[i+5]
                             + 287.0*raw_data[i+6]
                             + 222.0*raw_data[i+7]
                             + 147.0*raw_data[i+8]
                             +  62.0*raw_data[i+9]
                             -  33.0*raw_data[i+10]
                             - 138.0*raw_data[i+11]
                             - 253.0*raw_data[i+12]);
    
    return result;
}

int main()
{
    
    std::ifstream raw_data;
    std::ofstream smoothed_data;
    std::vector<double> x;
    double temp[25] = {0};
    
    
    raw_data.open("data.txt");
    smoothed_data.open("smoothed.txt");
    
    if (raw_data.fail())
    {
        std::cout << "Error in open xdata file";
    }
    if (smoothed_data.fail())
    {
        std::cout << "Error in Opening xdot file";
    }
    
    double line;
    while ( raw_data >> line )
    {
        x.push_back(line);
    }
    
    int xdata_counter = 0;
    
    while ( xdata_counter < (x.size() - 25) )
    {
        
        for ( int i = 0; i < 25; ++i)
        {
            temp[i] = x[xdata_counter+i];
        }
        
        smoothed_data << sg_filter(temp) << std::endl;
        
        
        xdata_counter++;
    }    
    
    return 0;
}
    

And this is the smoothed data (in blue) and the raw data (in the red)
http://s18.postimg.org/jmxd0e621/Untitled1.png

It is evident that the data at the beginning are affected severely I don't know why. Also the peaks are not smoothed enough.