Related
I have to apply the 4th order Runge Kutta method (RK) to the coupled equation in handbook of marinecraft hydrodynamics and motion control, page 154, equation 7.33, to determine v and r for every iteration. the RK method works when I use the functions separately, i.e., when I use the functions in separate programs. I have converted the equations from matrix form to linear equations. q1, q2, q3 and q4 are coefficients of v and r
The RK method is given as a function and is called twice to find v and r values. I can obtain the correct range of values of r as when rud increases, the values of v and r increase as well. when rud = rudmax, v and r should remain constant. however, v keeps increasing and doesn't stop. both v and r should be constant when rud = rudmax.
#include <iostream>
#include <fstream>
#include <cmath>
#include <iomanip>
using namespace std;
double dvdt(double r, double v);
double drdt(double v, double r);
double RKv1(double to, double vo, double t, double h);
double RKr1(double to, double ro, double t, double h);
double xo = 0; //initial X position
double yo = 0; //initial Y position
double x1; //final X position
double y11; //final Y position
double ti = 0; //initial time = 0
double tf; //time entered
double dt = 0.01; //time step (iteration time)
double rud = 0; //rudder angle
double m = 3.27385 * pow(10, 11); //mass of submarine (Kg)
double Iz = 2.31448 * pow(10, 12); //moment of inertia of submarine (kg.m^2)
double xg = 10.3; //x position of COG
double yg = 0; // position of COG
double u = 10; // surge velocity
double v, vo = 0; //sway velocity
double r, ro = 0; // yaw rate
double psio = 0; //initial ship heading
double psi1; //final ship heading
double rudmax; // max rudder angle
double au; //surge acceleration
double av; // sway acceleration
double ar; //angular acceleration
double X; //surge force
double Y; //sway force
double N; //turning moment
double h = 1;
//double q1, q2, q3, q4;
//matrices
//maneuvering and hydrodynamic coefficents
double Xu1 = -0.010087715;//-1.0467 * pow(10,-3);
double Yv1 = -0.290828106;//-23.889 * pow(10, -3);
double Yr1 = -0.008070039;//-1.0510 * pow(10, -3);
double Nv1 = -0.002928156;// 1.1531 * pow(10, -3);
double Nr1 = -0.056559574; //-0.50717 * pow(10, -3);
double Xu = -0.007115651;// -2.8763 * pow(10, -3);
double Yv = -0.35098699; //-38.948 * pow(10, -3);
double Yr = 0.091748747;// 2.0031 * pow(10, -3);
double Nv = -0.123433077;//-14.287 * pow(10, -3);
double Nr = -0.056559574;//-4.2267 * pow(10, -3);
double Yrud = -0.587323034;//1.4308 * pow(10, -3);
double Nrud = 0.991135206;// -0.71540 * pow(10, -3);
double a1 = m - Yv1;
double b1 = m * xg - Yr1;
double a2 = m * xg - Yr1;
double b2 = Iz - Nr1;
double det = a1 * b2 - a2 * b1;
double q1 = (-Yv * (Iz - Nr1) / det);
double q2 = ((m - Xu1) * u - Yr) * (-m * xg + Yr1) / det;
double q3 = ((Xu1 - Yv1) * u - Nv) * (-m * xg + Yr1) / det;
double q4 = ((m * xg - Yr1) * u - Nr) * (m - Yr1) / det;
int main()
{
std::ofstream fout; //for .CSV file
fout.open("results2.csv", ios::out | ios::app); //for .CSV file
//cout << det<<"\n";
/*cout << "surge velocity = ";
cin >> */ u = 10;
/*cout << "rudmax = ";
cin >> */rudmax = 30;
/*cout << "final time = ";
cin >>*/ tf = 100;
fout << "rud" << ", " << "ti" << ", " << "r" << ", " << "v" << "\n"; //for .CSV file
while (ti <= tf)
{
v = RKv1(ti, vo, ti + 0.025, h);
r = RKr1(ti, ro, ti + 0.025, h);
fout << rud << ", " << ti << ", " << r << ", " << v << "\n"; //for .CSV file
vo = v;
ro = r;
ti = ti + dt;
if (rudmax > 0)
{
if (rud < rudmax)
{
rud = rud + 0.005;
}
}
else if (rudmax < 0)
{
if (rud > rudmax)
{
rud = rud - 0.005;
}
}
}
}
/*---------------------------------------------------FUNCTIONS---------------------------------------------------*/
//functions for velocity and distance
double dvdt(double r, double v)
{
//return(-Yrud * rud - ((-Yv * (Iz - Nr1) / det) * v + ((m - Xu1) * u - Yr) * ((-m * xg - Yr1) / det )* r));
return(-Yrud * rud - q1 * v + q2 * r);
}
double drdt(double v, double r)
{
//return(-Nrud * rud - (((Xu1 - Yv1) * u - Nv) * ((-m * xg + Yr) / det) * v + ((m * xg - Yr1) * u - Nr) * ((m - Yr1) / det) * r));
return(-Nrud * rud - q3 * v + q4 * r);
}
//RK methods
//correction of rk method
double RKv1(double to, double vo, double t, double h)
{
int n = /*(int)((t - to) / h)*/ 1;
double k1, k2, k3, k4;
double v = vo;
for (int i = 1; i <= n; i++)
{
k1 = h * dvdt(ro, v);
k2 = h * dvdt(ro, v + 0.5 * k1);
k3 = h * dvdt(ro, v + 0.5 * k2);
k4 = h * dvdt(ro, v + k3);
v = v + (1.0 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
to = to + h;
}
return v;
}
double RKr1(double to, double ro, double t, double h)
{
int n = /*(int)((t - to) / h)*/ 1;
double k1, k2, k3, k4;
double r = ro;
for (int i = 1; i <= n; i++)
{
k1 = h * drdt(vo, r);
k2 = h * drdt(vo, r + 0.5 * k1);
k3 = h * drdt(vo, r + 0.5 * k2);
k4 = h * drdt(vo, r + k3);
r = r + (1.0 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
to = to + h;
}
return r;
}
the results are stored in the results2.csv file.
snippet of the result. r (-28.59) remains constant whereas v (-84000) keep increasing
Any help/suggestions are appreciated. thanks!
I'm translating Python's version of 'page_dewarper' (https://mzucker.github.io/2016/08/15/page-dewarping.html) into C++. I'm going to use dlib, which is a fantastic tool, that helped me in a few optimization problems before. In line 748 of Github repo (https://github.com/mzucker/page_dewarp/blob/master/page_dewarp.py) Matt uses optimize function from Scipy, to find the minimal distance between two vectors. I think, my C++ equivalent should be solve_least_squares_lm() or solve_least_squares(). I'll give a concrete example to analyze.
My data:
a) dstpoints is a vector with OpenCV points - std::vector<cv::Point2f> (I have 162 points in this example, they are not changing),
b) ppts is also std::vector<cv::Point2f> and the same size as dstpoints.
std::vector<cv::Point2f> ppts = project_keypoints(params, input);
It is dependent on:
- dlib::column_vector 'input' is 2*162=324 long and is not changing,
- dlib::column_vector 'params' is 189 long and its values should be changed to get the minimal value of variable 'suma', something like this:
double suma = 0.0;
for (int i=0; i<dstpoints_size; i++)
{
suma += pow(dstpoints[i].x - ppts[i].x, 2);
suma += pow(dstpoints[i].y - ppts[i].y, 2);
}
I'm looking for 'params' vector that will give me the smallest value of 'suma' variable. Least squares algorithm seems to be a good option to solve it: http://dlib.net/dlib/optimization/optimization_least_squares_abstract.h.html#solve_least_squares, but I don't know if it is good for my case.
I think, my problem is that for every different 'params' vector I get different 'ppts' vector, not only single value, and I don't know if solve_least_squares function can match my example.
I must calculate residual for every point. I think, my 'list' from aforementioned link should be something like this:
(ppts[i].x - dstpoints[i].x, ppts[i].y - dstpoints[i].y, ppts[i+1].x - dstpoints[i+1].x, ppts[i+1].y - dstpoints[i+1].y, etc.)
, where 'ppts' vector depends on 'params' vector and then this problem can be solved with least squares algorithm. I don't know how to create data_samples with these assumptions, because it requires dlib::input_vector for every sample, as it is shown in example: http://dlib.net/least_squares_ex.cpp.html.
Am I thinking right?
I'm doing the same thing this days. My solution is writing a Powell Class by myself. It works, but really slowly. The program takes 2 minutes in dewarping linguistics_thesis.jpg.
I don't know what cause the program running so slowly. Maybe because of the algorithm or the code has some extra loop. I'm a Chinese student and my school only have java lessons. So it is normal if you find some extra codes in my codes.
Here is my Powell class.
using namespace std;
using namespace cv;
class MyPowell
{
public:
vector<vector<double>> xi;
vector<double> pcom;
vector<double> xicom;
vector<Point2d> dstpoints;
vector<double> myparams;
vector<double> params;
vector<Point> keypoint_index;
Point2d dst_br;
Point2d dims;
int N;
int itmax;
int ncom;
int iter;
double fret, ftol;
int usingAorB;
MyPowell(vector<Point2d> &dstpoints, vector<double> ¶ms, vector<Point> &keypoint_index);
MyPowell(Point2d &dst_br, vector<double> ¶ms, Point2d & dims);
MyPowell();
double obj(vector<double> ¶ms);
void powell(vector<double> &p, vector<vector<double>> &xi, double ftol, double &fret);
double sign(double a);// , double b);
double sqr(double a);
void linmin(vector<double> &p, vector<double> &xit, int n, double &fret);
void mnbrak(double & ax, double & bx, double & cx,
double & fa, double & fb, double & fc);
double f1dim(double x);
double brent(double ax, double bx, double cx, double & xmin, double tol);
vector<double> usePowell();
void erase(vector<double>& pbar, vector<double> &prr, vector<double> &pr);
};
#include"Powell.h"
MyPowell::MyPowell(vector<Point2d> &dstpoints, vector<double>& params, vector<Point> &keypoint_index)
{
this->dstpoints = dstpoints;
this->myparams = params;
this->keypoint_index = keypoint_index;
N = params.size();
itmax = N * N;
usingAorB = 1;
}
MyPowell::MyPowell(Point2d & dst_br, vector<double>& params, Point2d & dims)
{
this->dst_br = dst_br;
this->myparams.push_back(dims.x);
this->myparams.push_back(dims.y);
this->params = params;
this->dims = dims;
N = 2;
itmax = N * 1000;
usingAorB = 2;
}
MyPowell::MyPowell()
{
usingAorB = 3;
}
double MyPowell::obj(vector<double> &myparams)
{
if (1 == usingAorB)
{
vector<Point2d> ppts = Dewarp::projectKeypoints(keypoint_index, myparams);
double total = 0;
for (int i = 0; i < ppts.size(); i++)
{
double x = dstpoints[i].x - ppts[i].x;
double y = dstpoints[i].y - ppts[i].y;
total += (x * x + y * y);
}
return total;
}
else if(2 == usingAorB)
{
dims.x = myparams[0];
dims.y = myparams[1];
//cout << "dims.x " << dims.x << " dims.y " << dims.y << endl;
vector<Point2d> vdims = { dims };
vector<Point2d> proj_br = Dewarp::projectXY(vdims, params);
double total = 0;
double x = dst_br.x - proj_br[0].x;
double y = dst_br.y - proj_br[0].y;
total += (x * x + y * y);
return total;
}
return 0;
}
void MyPowell::powell(vector<double> &x, vector<vector<double>> &direc, double ftol, double &fval)
{
vector<double> x1;
vector<double> x2;
vector<double> direc1;
int myitmax = 20;
if(N>500)
myitmax = 10;
else if (N > 300)
{
myitmax = 15;
}
double fx2, t, fx, dum, delta;
fval = obj(x);
int bigind;
for (int j = 0; j < N; j++)
{
x1.push_back(x[j]);
}
int iter = 0;
while (true)
{
do
{
do
{
iter += 1;
fx = fval;
bigind = 0;
delta = 0.0;
for (int i = 0; i < N; i++)
{
direc1 = direc[i];
fx2 = fval;
linmin(x, direc1, N, fval);
if (fabs(fx2 - fval) > delta)
{
delta = fabs(fx2 - fval);
bigind = i;
}
}
if (2.0 * fabs(fx - fval) <= ftol * (fabs(fx) + fabs(fval)) + 1e-7)
{
erase(direc1, x2, x1);
return;
}
if (iter >= itmax)
{
cout << "powell exceeding maximum iterations" << endl;
return;
}
if (!x2.empty())
{
x2.clear();
}
for (int j = 0; j < N; j++)
{
x2.push_back(2.0*x[j] - x1[j]);
direc1[j] = x[j] - x1[j];
x1[j] = x[j];
}
myitmax--;
cout << fx2 << endl;
fx2 = obj(x2);
if (myitmax < 0)
return;
} while (fx2 >= fx);
dum = fx - 2 * fval + fx2;
t = 2.0*dum*pow((fx - fval - delta), 2) - delta * pow((fx - fx2), 2);
} while (t >= 0.0);
linmin(x, direc1, N, fval);
direc[bigind] = direc1;
}
}
double MyPowell::sign(double a)//, double b)
{
if (a > 0.0)
{
return 1;
}
else
{
if (a < 0.0)
{
return -1;
}
}
return 0;
}
double MyPowell::sqr(double a)
{
return a * a;
}
void MyPowell::linmin(vector<double>& p, vector<double>& xit, int n, double &fret)
{
double tol = 1e-2;
ncom = n;
pcom = p;
xicom = xit;
double ax = 0.0;
double xx = 1.0;
double bx = 0.0;
double fa, fb, fx, xmin;
mnbrak(ax, xx, bx, fa, fx, fb);
fret = brent(ax, xx, bx, xmin, tol);
for (int i = 0; i < n; i++)
{
xit[i] = (xmin * xit[i]);
p[i] += xit[i];
}
}
void MyPowell::mnbrak(double & ax, double & bx, double & cx,
double & fa, double & fb, double & fc)
{
const double GOLD = 1.618034, GLIMIT = 110.0, TINY = 1e-20;
double val, fw, tmp2, tmp1, w, wlim;
double denom;
fa = f1dim(ax);
fb = f1dim(bx);
if (fb > fa)
{
val = ax;
ax = bx;
bx = val;
val = fb;
fb = fa;
fa = val;
}
cx = bx + GOLD * (bx - ax);
fc = f1dim(cx);
int iter = 0;
while (fb >= fc)
{
tmp1 = (bx - ax) * (fb - fc);
tmp2 = (bx - cx) * (fb - fa);
val = tmp2 - tmp1;
if (fabs(val) < TINY)
{
denom = 2.0*TINY;
}
else
{
denom = 2.0*val;
}
w = bx - ((bx - cx)*tmp2 - (bx - ax)*tmp1) / (denom);
wlim = bx + GLIMIT * (cx - bx);
if ((bx - w) * (w - cx) > 0.0)
{
fw = f1dim(w);
if (fw < fc)
{
ax = bx;
fa = fb;
bx = w;
fb = fw;
return;
}
else if (fw > fb)
{
cx = w;
fc = fw;
return;
}
w = cx + GOLD * (cx - bx);
fw = f1dim(w);
}
else
{
if ((cx - w)*(w - wlim) >= 0.0)
{
fw = f1dim(w);
if (fw < fc)
{
bx = cx;
cx = w;
w = cx + GOLD * (cx - bx);
fb = fc;
fc = fw;
fw = f1dim(w);
}
}
else if ((w - wlim)*(wlim - cx) >= 0.0)
{
w = wlim;
fw = f1dim(w);
}
else
{
w = cx + GOLD * (cx - bx);
fw = f1dim(w);
}
}
ax = bx;
bx = cx;
cx = w;
fa = fb;
fb = fc;
fc = fw;
}
}
double MyPowell::f1dim(double x)
{
vector<double> xt;
for (int j = 0; j < ncom; j++)
{
xt.push_back(pcom[j] + x * xicom[j]);
}
return obj(xt);
}
double MyPowell::brent(double ax, double bx, double cx, double & xmin, double tol = 1.48e-8)
{
const double CGOLD = 0.3819660, ZEPS = 1.0e-4;
int itmax = 500;
double a = MIN(ax, cx);
double b = MAX(ax, cx);
double v = bx;
double w = v, x = v;
double deltax = 0.0;
double fx = f1dim(x);
double fv = fx;
double fw = fx;
double rat = 0, u = 0, fu;
int iter;
int done;
double dx_temp, xmid, tol1, tol2, tmp1, tmp2, p;
for (iter = 0; iter < 500; iter++)
{
xmid = 0.5 * (a + b);
tol1 = tol * fabs(x) + ZEPS;
tol2 = 2.0*tol1;
if (fabs(x - xmid) <= (tol2 - 0.5*(b - a)))
break;
done = -1;
if (fabs(deltax) > tol1)
{
tmp1 = (x - w) * (fx - fv);
tmp2 = (x - v) * (fx - fw);
p = (x - v) * tmp2 - (x - w) * tmp1;
tmp2 = 2.0 * (tmp2 - tmp1);
if (tmp2 > 0.0)
p = -p;
tmp2 = fabs(tmp2);
dx_temp = deltax;
deltax = rat;
if ((p > tmp2 * (a - x)) && (p < tmp2 * (b - x)) &&
fabs(p) < fabs(0.5 * tmp2 * dx_temp))
{
rat = p / tmp2;
u = x + rat;
if ((u - a) < tol2 || (b - u) < tol2)
{
rat = fabs(tol1) * sign(xmid - x);
}
done = 0;
}
}
if(done)
{
if (x >= xmid)
{
deltax = a - x;
}
else
{
deltax = b - x;
}
rat = CGOLD * deltax;
}
if (fabs(rat) >= tol1)
{
u = x + rat;
}
else
{
u = x + fabs(tol1) * sign(rat);
}
fu = f1dim(u);
if (fu > fx)
{
if (u < x)
{
a = u;
}
else
{
b = u;
}
if (fu <= fw || w == x)
{
v = w;
w = u;
fv = fw;
fw = fu;
}
else if (fu <= fv || v == x || v == w)
{
v = u;
fv = fu;
}
}
else
{
if (u >= x)
a = x;
else
b = x;
v = w;
w = x;
x = u;
fv = fw;
fw = fx;
fx = fu;
}
}
if(iter > itmax)
cout << "\n Brent exceed maximum iterations.\n\n";
xmin = x;
return fx;
}
vector<double> MyPowell::usePowell()
{
ftol = 1e-4;
vector<vector<double>> xi;
for (int i = 0; i < N; i++)
{
vector<double> xii;
for (int j = 0; j < N; j++)
{
xii.push_back(0);
}
xii[i]=(1.0);
xi.push_back(xii);
}
double fret = 0;
powell(myparams, xi, ftol, fret);
//for (int i = 0; i < xi.size(); i++)
//{
// double a = obj(xi[i]);
// if (fret > a)
// {
// fret = a;
// myparams = xi[i];
// }
//}
cout << "final result" << fret << endl;
return myparams;
}
void MyPowell::erase(vector<double>& pbar, vector<double>& prr, vector<double>& pr)
{
for (int i = 0; i < pbar.size(); i++)
{
pbar[i] = 0;
}
for (int i = 0; i < prr.size(); i++)
{
prr[i] = 0;
}
for (int i = 0; i < pr.size(); i++)
{
pr[i] = 0;
}
}
I used PRAXIS library, because it doesn't need derivative information and is fast.
I modified the code a little to my needs and now it is faster than original version written in Python.
I have read these answers 1, 2 about passing parameter to odeint (Boost).
I tried here to do the same process. but the code give wrong answer.
Here is my try:
This is simplified code of boost calculation of lyapunov exponent of Lorenz oscillator.
#include <iostream>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include "gram_schmidt.hpp"
using namespace std;
using namespace boost::numeric::odeint;
typedef vector<double> dim1;
const double sigma = 10.0;
const double R = 28.0;
const double b = 8.0 / 3.0;
const size_t n = 3;
const size_t num_of_lyap = 3;
const size_t N = n + n * num_of_lyap;
// system with out perturbation
struct lorenz
{
void operator()( const dim1 &x , dim1 &dxdt , double t ) const
{
dxdt[0] = sigma * ( x[1] - x[0] );
dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
dxdt[2] = -b * x[2] + x[0] * x[1];
}
};
// system with perturbation
void lorenz_with_lyap( const dim1 &x , dim1 &dxdt , double t )
{
lorenz()( x , dxdt , t );
for( int l=0 ; l<num_of_lyap ; ++l )
{
const double *pert = &x[3 + l * 3];
double *dpert = &dxdt[3 + l * 3];
dpert[0] = - sigma * pert[0] + 10.0 * pert[1];
dpert[1] = ( R - x[2] ) * pert[0] - pert[1] - x[0] * pert[2];
dpert[2] = x[1] * pert[0] + x[0] * pert[1] - b * pert[2];
}
}
//------------------------------------------------------------------------
int main( int argc , char **argv )
{
const double dt = 0.01;
dim1 x(N);
x[0] = 10.; x[1] = 10.0; x[2] = 5.0; // initial condition
dim1 lyap(3);
runge_kutta4<dim1, double, dim1, double, range_algebra> rk4;
// perform 10000 transient steps
integrate_n_steps(rk4, lorenz(), x, 0.0, dt, 10000);
fill( x.begin()+n , x.end() , 0.0 );
for( size_t i=0 ; i<num_of_lyap ; ++i ) x[n+n*i+i] = 1.0;
fill( lyap.begin() , lyap.end() , 0.0 );
double t = 0.0;
size_t count = 0;
while( true )
{
t = integrate_n_steps( rk4 , lorenz_with_lyap , x , t , dt , 100 );
gram_schmidt< num_of_lyap >( x , lyap , n );
++count;
if( !(count % 1000) )
{
cout << t;
for( size_t i=0 ; i<num_of_lyap ; ++i ) cout << "\t" << lyap[i] / t ;
cout << endl;
}
}
return 0;
}
first I defined these classes
class lorenz
{
public:
void operator()...
and
class lorenz_with_lyap
{
public:
void operator()(const dim1 &x, dim1 &dxdt, double t) const
{
dxdt[0] = sigma * (x[1] - x[0]);
dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
dxdt[2] = -b * x[2] + x[0] * x[1];
for (int l = 0; l < num_of_lyap; ++l)
{
const double *pert = &x[3 + l * 3];
double *dpert = &dxdt[3 + l * 3];
dpert[0] = -sigma * pert[0] + sigma * pert[1];
dpert[1] = (R - x[2]) * pert[0] - pert[1] - x[0] * pert[2];
dpert[2] = x[1] * pert[0] + x[0] * pert[1] - b * pert[2];
}
}
};
then I can change it a little to define a parameter:
class lorenz
{
double sigma;
public:
lorenz(double si) : sigma(si) { }
void operator()...
but defining the lorenz_with_lyap class change the result, and I don't know where I am doing something wrong.
Do you have any idea?
(This is now a homework and I am going to check the result of this code with another method).
Thank you.
After some struggling, I finally corrected that, post it here if somebody probably is interested:
class lorenz
{
public:
void operator()(const dim1 &x, dim1 &dxdt, double t) const
{
dxdt[0] = sigma * (x[1] - x[0]);
dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
dxdt[2] = -b * x[2] + x[0] * x[1];
}
};
class lorenz_with_lyap
{
const double sig;
public:
lorenz_with_lyap(double si) : sig(si) { }
void operator()(const dim1 &x, dim1 &dxdt, double t) const
{
dxdt[0] = sig * (x[1] - x[0]);
dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
dxdt[2] = -b * x[2] + x[0] * x[1];
for (int l = 0; l < num_of_lyap; ++l)
{
const double *pert = &x[3 + l * 3];
double *dpert = &dxdt[3 + l * 3];
dpert[0] = -sig * pert[0] + 10.0 * pert[1];
dpert[1] = (R - x[2]) * pert[0] - pert[1] - x[0] * pert[2];
dpert[2] = x[1] * pert[0] + x[0] * pert[1] - b * pert[2];
}
}
};
//------------------------------------------------------------------------
// in main
lorenz_with_lyap lo(10.0);
t = integrate_n_steps(rk4, lo, x, t, dt, 100);
...
I need some help here. Please excuse the complexity of the code. Basically, I am looking to use the bisection method to find a value "Theta" and each i increment.
I know that all the calculations work fine when I know the Theta, and I have the code run to just simply calculate all the values, but when I introduce a while loop and the bisection method to have the code approximate Theta, I can't seem to get it to run correctly. I am assuming I have my while loop set up incorrectly....
#include <math.h>
#include <iostream>
#include <vector>
#include <iomanip>
#include <algorithm> // std::max
using namespace std;
double FuncM(double theta, double r, double F, double G, double Gprime, double d_t, double sig);
double FuncM(double theta, double r, double F, double G, double Gprime, double d_t, double sig)
{
double eps = 0.0001;
return ((log(max((r + (theta + F - 0.5 * G * Gprime ) * d_t), eps))) / sig);
}
double FuncJSTAR(double m, double x_0, double d_x);
double FuncJSTAR(double m, double x_0, double d_x)
{
return (int(((m - x_0) / d_x)+ 0.5));
}
double FuncCN(double m, double x_0, double j, double d_x);
double FuncCN(double m, double x_0, double j, double d_x)
{
return (m - x_0 - j * d_x);
}
double FuncPup(double d_t, double cn, double d_x);
double FuncPup(double d_t, double cn, double d_x)
{
return (((d_t + pow(cn, 2.0)) / (2.0 * pow(d_x, 2.0))) + (cn / (2.0 * d_x)));
}
double FuncPdn(double d_t, double cn, double d_x);
double FuncPdn(double d_t, double cn, double d_x)
{
return (((d_t + pow(cn, 2.0)) / (2.0 * pow(d_x, 2.0))) - (cn / (2.0 * d_x)));
}
double FuncPmd(double pd, double pu);
double FuncPmd(double pd, double pu)
{
return (1 - pu - pd);
}
int main()
{
const int Maturities = 5;
const double EPS = 0.00001;
double TermStructure[Maturities][2] = {
{0.5 , 0.05},
{1.0 , 0.06},
{1.5 , 0.07},
{2.0 , 0.075},
{3.0 , 0.085} };
//--------------------------------------------------------------------------------------------------------
vector<double> Price(Maturities);
double Initial_Price = 1.00;
for (int i = 0; i < Maturities; i++)
{
Price[i] = Initial_Price * exp(-TermStructure[i][1] * TermStructure[i][0]);
}
//--------------------------------------------------------------------------------------------------------
int j_max = 8;
int j_range = ((j_max * 2) + 1);
//--------------------------------------------------------------------------------------------------------
// Set up vector of possible j values
vector<int> j_value(j_range);
for (int j = 0; j < j_range; j++)
{
j_value[j] = j_max - j;
}
//--------------------------------------------------------------------------------------------------------
double dt = 0.5;
double dx = sqrt(3 * dt);
double sigma = 0.15;
double mean_reversion = 0.2; // "a" value
//--------------------------------------------------------------------------------------------------------
double r0 = TermStructure[0][1]; // Initialise r(0) in case no corresponding dt rate in term structure
//--------------------------------------------------------------------------------------------------------
double x0 = log(r0) / sigma;
//--------------------------------------------------------------------------------------------------------
vector<double> r_j(j_range); // rate at each j
vector<double> F_r(j_range);
vector<double> G_r(j_range);
vector<double> G_prime_r(j_range);
for(int j = 0; j < j_range; j++)
{
if (j == j_max)
{
r_j[j] = r0;
}
else
{
r_j[j] = exp((x0 + j_value[j]*dx) * sigma);
}
F_r[j] = -mean_reversion * r_j[j];
G_r[j] = sigma * r_j[j];
G_prime_r[j] = sigma;
}
//--------------------------------------------------------------------------------------------------------
vector<vector<double>> m((j_range), vector<double>(Maturities));
vector<vector<int>> j_star((j_range), vector<int>(Maturities));
vector<vector<double>> Central_Node((j_range), vector<double>(Maturities));
vector<double> Theta(Maturities - 1);
vector<vector<double>> Pu((j_range), vector<double>(Maturities));
vector<vector<double>> Pd((j_range), vector<double>(Maturities));
vector<vector<double>> Pm((j_range), vector<double>(Maturities));
vector<vector<double>> Q((j_range), vector<double>(Maturities));// = {}; // Arrow Debreu Price. Initialised all array values to 0
vector<double> Q_dt_sum(Maturities);// = {}; // Sum of Arrow Debreu Price at each time step. Initialised all array values to 0
//--------------------------------------------------------------------------------------------------------
double Theta_A, Theta_B, Theta_C;
int JSTART;
int JEND;
int TempStart;
int TempEnd;
int max;
int min;
vector<vector<int>> Up((j_range), vector<int>(Maturities));
vector<vector<int>> Down((j_range), vector<int>(Maturities));
// Theta[0] = 0.0498039349327417;
// Theta[1] = 0.0538710670441647;
// Theta[2] = 0.0181648634139392;
// Theta[3] = 0.0381183886467521;
for(int i = 0; i < (Maturities-1); i++)
{
Theta_A = 0.00;
Theta_B = TermStructure[i][1];
Q_dt_sum[0] = Initial_Price;
Q_dt_sum[i+1] = 0.0;
while (fabs(Theta_A - Theta_B) >= 0.0000001)
{
max = 1;
min = 10;
if (i == 0)
{
JSTART = j_max;
JEND = j_max;
}
else
{
JSTART = TempStart;
JEND = TempEnd;
}
for(int j = JSTART; j >= JEND; j--)
{
Theta_C = (Theta_A + Theta_B) / 2.0; // If Theta C is too low, the associated Price will be higher than Price from initial term structure. (ie P(Theta C) > P(i+2) for Theta C < Theta)
// If P_C > P(i+2), set Theta_B = Theta_C, else if P_C < P(i+2), set Theta_A = Theta_C, Else if P_C = P(i+2), Theta_C = Theta[i]
//cout << Theta_A << " " << Theta_B << " " << Theta_C << endl;
m[j][i] = FuncM(Theta[i], r_j[j], F_r[j], G_r[j], G_prime_r[j], dt, sigma);
j_star[j][i] = FuncJSTAR(m[j][i], x0, dx);
Central_Node[j][i] = FuncCN(m[j][i], x0, j_star[j][i], dx);
Pu[j][i] = FuncPup(dt, Central_Node[j][i], dx);
Pd[j][i] = FuncPdn(dt, Central_Node[j][i], dx);
Pm[j][i] = FuncPmd(Pd[j][i], Pu[j][i]);
for (int p = 0; p < j_range; p++)
{
Q[p][i] = 0; // Clear Q array
}
Q[j_max][0] = Initial_Price;
Q[j_max -(j_star[j][i]+1)][i+1] = Q[j_max - (j_star[j][i]+1)][i+1] + Q[j][i] * Pu[j][i] * exp(-r_j[j] * dt);
Q[j_max -(j_star[j][i] )][i+1] = Q[j_max - (j_star[j][i] )][i+1] + Q[j][i] * Pm[j][i] * exp(-r_j[j] * dt);
Q[j_max -(j_star[j][i]-1)][i+1] = Q[j_max - (j_star[j][i]-1)][i+1] + Q[j][i] * Pd[j][i] * exp(-r_j[j] * dt);
}
for (int j = 0; j < j_range; j++)
{
Up[j][i] = j_star[j][i] + 1;
Down[j][i] = j_star[j][i] - 1;
if (Up[j][i] > max)
{
max = Up[j][i];
}
if ((Down[j][i] < min) && (Down[j][i] > 0))
{
min = Down[j][i];
}
}
TempEnd = j_max - (max);
TempStart = j_max - (min);
for (int j = 0; j < j_range; j++)
{
Q_dt_sum[i+1] = Q_dt_sum[i+1] + Q[j][i] * exp(-r_j[j] * dt);
cout << Q_dt_sum[i+1] << endl;
}
if (Q_dt_sum[i+1] == Price[i+2])
{
Theta[i] = Theta_C;
break;
}
if (Q_dt_sum[i+1] > Price[i+2])
{
Theta_B = Theta_C;
}
else if (Q_dt_sum[i+1] < Price[i+2])
{
Theta_A = Theta_C;
}
}
cout << Theta[i] << endl;
}
return 0;
}
Ok, my bad. I had a value being called incorrectly.
All good.
I am implementing an image analysis algorithm using openCV and c++, but I found out openCV doesnt have any function for Butterworth Bandpass filter officially.
in my project I have to pass a time series of pixels into the Butterworth 5 order filter and the function will return the filtered time series pixels. Butterworth(pixelseries,order, frequency), if you have any idea to help me of how to start please let me know. Thank you
EDIT :
after getting help, finally I come up with the following code. which can calculate the Numerator Coefficients and Denominator Coefficients, but the problem is that some of the numbers is not as same as matlab results. here is my code:
#include <iostream>
#include <stdio.h>
#include <vector>
#include <math.h>
using namespace std;
#define N 10 //The number of images which construct a time series for each pixel
#define PI 3.14159
double *ComputeLP( int FilterOrder )
{
double *NumCoeffs;
int m;
int i;
NumCoeffs = (double *)calloc( FilterOrder+1, sizeof(double) );
if( NumCoeffs == NULL ) return( NULL );
NumCoeffs[0] = 1;
NumCoeffs[1] = FilterOrder;
m = FilterOrder/2;
for( i=2; i <= m; ++i)
{
NumCoeffs[i] =(double) (FilterOrder-i+1)*NumCoeffs[i-1]/i;
NumCoeffs[FilterOrder-i]= NumCoeffs[i];
}
NumCoeffs[FilterOrder-1] = FilterOrder;
NumCoeffs[FilterOrder] = 1;
return NumCoeffs;
}
double *ComputeHP( int FilterOrder )
{
double *NumCoeffs;
int i;
NumCoeffs = ComputeLP(FilterOrder);
if(NumCoeffs == NULL ) return( NULL );
for( i = 0; i <= FilterOrder; ++i)
if( i % 2 ) NumCoeffs[i] = -NumCoeffs[i];
return NumCoeffs;
}
double *TrinomialMultiply( int FilterOrder, double *b, double *c )
{
int i, j;
double *RetVal;
RetVal = (double *)calloc( 4 * FilterOrder, sizeof(double) );
if( RetVal == NULL ) return( NULL );
RetVal[2] = c[0];
RetVal[3] = c[1];
RetVal[0] = b[0];
RetVal[1] = b[1];
for( i = 1; i < FilterOrder; ++i )
{
RetVal[2*(2*i+1)] += c[2*i] * RetVal[2*(2*i-1)] - c[2*i+1] * RetVal[2*(2*i-1)+1];
RetVal[2*(2*i+1)+1] += c[2*i] * RetVal[2*(2*i-1)+1] + c[2*i+1] * RetVal[2*(2*i-1)];
for( j = 2*i; j > 1; --j )
{
RetVal[2*j] += b[2*i] * RetVal[2*(j-1)] - b[2*i+1] * RetVal[2*(j-1)+1] +
c[2*i] * RetVal[2*(j-2)] - c[2*i+1] * RetVal[2*(j-2)+1];
RetVal[2*j+1] += b[2*i] * RetVal[2*(j-1)+1] + b[2*i+1] * RetVal[2*(j-1)] +
c[2*i] * RetVal[2*(j-2)+1] + c[2*i+1] * RetVal[2*(j-2)];
}
RetVal[2] += b[2*i] * RetVal[0] - b[2*i+1] * RetVal[1] + c[2*i];
RetVal[3] += b[2*i] * RetVal[1] + b[2*i+1] * RetVal[0] + c[2*i+1];
RetVal[0] += b[2*i];
RetVal[1] += b[2*i+1];
}
return RetVal;
}
double *ComputeNumCoeffs(int FilterOrder)
{
double *TCoeffs;
double *NumCoeffs;
int i;
NumCoeffs = (double *)calloc( 2*FilterOrder+1, sizeof(double) );
if( NumCoeffs == NULL ) return( NULL );
TCoeffs = ComputeHP(FilterOrder);
if( TCoeffs == NULL ) return( NULL );
for( i = 0; i < FilterOrder; ++i)
{
NumCoeffs[2*i] = TCoeffs[i];
NumCoeffs[2*i+1] = 0.0;
}
NumCoeffs[2*FilterOrder] = TCoeffs[FilterOrder];
free(TCoeffs);
return NumCoeffs;
}
double *ComputeDenCoeffs( int FilterOrder, double Lcutoff, double Ucutoff )
{
int k; // loop variables
double theta; // PI * (Ucutoff - Lcutoff) / 2.0
double cp; // cosine of phi
double st; // sine of theta
double ct; // cosine of theta
double s2t; // sine of 2*theta
double c2t; // cosine 0f 2*theta
double *RCoeffs; // z^-2 coefficients
double *TCoeffs; // z^-1 coefficients
double *DenomCoeffs; // dk coefficients
double PoleAngle; // pole angle
double SinPoleAngle; // sine of pole angle
double CosPoleAngle; // cosine of pole angle
double a; // workspace variables
cp = cos(PI * (Ucutoff + Lcutoff) / 2.0);
theta = PI * (Ucutoff - Lcutoff) / 2.0;
st = sin(theta);
ct = cos(theta);
s2t = 2.0*st*ct; // sine of 2*theta
c2t = 2.0*ct*ct - 1.0; // cosine of 2*theta
RCoeffs = (double *)calloc( 2 * FilterOrder, sizeof(double) );
TCoeffs = (double *)calloc( 2 * FilterOrder, sizeof(double) );
for( k = 0; k < FilterOrder; ++k )
{
PoleAngle = PI * (double)(2*k+1)/(double)(2*FilterOrder);
SinPoleAngle = sin(PoleAngle);
CosPoleAngle = cos(PoleAngle);
a = 1.0 + s2t*SinPoleAngle;
RCoeffs[2*k] = c2t/a;
RCoeffs[2*k+1] = s2t*CosPoleAngle/a;
TCoeffs[2*k] = -2.0*cp*(ct+st*SinPoleAngle)/a;
TCoeffs[2*k+1] = -2.0*cp*st*CosPoleAngle/a;
}
DenomCoeffs = TrinomialMultiply(FilterOrder, TCoeffs, RCoeffs );
free(TCoeffs);
free(RCoeffs);
DenomCoeffs[1] = DenomCoeffs[0];
DenomCoeffs[0] = 1.0;
for( k = 3; k <= 2*FilterOrder; ++k )
DenomCoeffs[k] = DenomCoeffs[2*k-2];
return DenomCoeffs;
}
void filter(int ord, double *a, double *b, int np, double *x, double *y)
{
int i,j;
y[0]=b[0] * x[0];
for (i=1;i<ord+1;i++)
{
y[i]=0.0;
for (j=0;j<i+1;j++)
y[i]=y[i]+b[j]*x[i-j];
for (j=0;j<i;j++)
y[i]=y[i]-a[j+1]*y[i-j-1];
}
for (i=ord+1;i<np+1;i++)
{
y[i]=0.0;
for (j=0;j<ord+1;j++)
y[i]=y[i]+b[j]*x[i-j];
for (j=0;j<ord;j++)
y[i]=y[i]-a[j+1]*y[i-j-1];
}
}
int main(int argc, char *argv[])
{
//Frequency bands is a vector of values - Lower Frequency Band and Higher Frequency Band
//First value is lower cutoff and second value is higher cutoff
double FrequencyBands[2] = {0.25,0.375};//these values are as a ratio of f/fs, where fs is sampling rate, and f is cutoff frequency
//and therefore should lie in the range [0 1]
//Filter Order
int FiltOrd = 5;
//Pixel Time Series
/*int PixelTimeSeries[N];
int outputSeries[N];
*/
//Create the variables for the numerator and denominator coefficients
double *DenC = 0;
double *NumC = 0;
//Pass Numerator Coefficients and Denominator Coefficients arrays into function, will return the same
NumC = ComputeNumCoeffs(FiltOrd);
for(int k = 0; k<11; k++)
{
printf("NumC is: %lf\n", NumC[k]);
}
//is A in matlab function and the numbers are correct
DenC = ComputeDenCoeffs(FiltOrd, FrequencyBands[0], FrequencyBands[1]);
for(int k = 0; k<11; k++)
{
printf("DenC is: %lf\n", DenC[k]);
}
double y[5];
double x[5]={1,2,3,4,5};
filter(5, DenC, NumC, 5, x, y);
return 1;
}
I get this resutls for my code :
B= 1,0,-5,0,10,0,-10,0,5,0,-1
A= 1.000000000000000, -4.945988709743181, 13.556489496973796, -24.700711850327743,
32.994881546824828, -33.180726698160655, 25.546126213403539, -14.802008410165968,
6.285430089797051, -1.772929809750849, 0.277753012228403
but if I want to test the coefficinets in same frequency band in MATLAB, I get the following results:
>> [B, A]=butter(5, [0.25,0.375])
B = 0.0002, 0, -0.0008, 0, 0.0016, 0, -0.0016, 0, 0.0008, 0, -0.0002
A = 1.0000, -4.9460, 13.5565, -24.7007, 32.9948, -33.1806, 25.5461, -14.8020, 6.2854, -1.7729, 0.2778
I have test this website :http://www.exstrom.com/journal/sigproc/ code, but the result is equal as mine, not matlab. anybody knows why? or how can I get the same result as matlab toolbox?
I know this is a post on an old thread, and I would usually leave this as a comment, but I'm apparently not able to do that.
In any case, for people searching for similar code, I thought I would post the link from where this code originates (it also has C code for other types of Butterworth filter coefficients and some other cool signal processing code).
The code is located here:
http://www.exstrom.com/journal/sigproc/
Additionally, I think there is a piece of code which calculates said scaling factor for you already.
/**********************************************************************
sf_bwbp - calculates the scaling factor for a butterworth bandpass filter.
The scaling factor is what the c coefficients must be multiplied by so
that the filter response has a maximum value of 1.
*/
double sf_bwbp( int n, double f1f, double f2f )
{
int k; // loop variables
double ctt; // cotangent of theta
double sfr, sfi; // real and imaginary parts of the scaling factor
double parg; // pole angle
double sparg; // sine of pole angle
double cparg; // cosine of pole angle
double a, b, c; // workspace variables
ctt = 1.0 / tan(M_PI * (f2f - f1f) / 2.0);
sfr = 1.0;
sfi = 0.0;
for( k = 0; k < n; ++k )
{
parg = M_PI * (double)(2*k+1)/(double)(2*n);
sparg = ctt + sin(parg);
cparg = cos(parg);
a = (sfr + sfi)*(sparg - cparg);
b = sfr * sparg;
c = -sfi * cparg;
sfr = b - c;
sfi = a - b - c;
}
return( 1.0 / sfr );
}
I finally found it.
I just need to implement the following code from matlab source code to c++ . "the_mandrill" were right, I need to add the normalizing constant into the coefficient:
kern = exp(-j*w*(0:length(b)-1));
b = real(b*(kern*den(:))/(kern*b(:)));
EDIT:
and here is the final edition, which the whole code will return numbers exactly equal to MATLAB :
double *ComputeNumCoeffs(int FilterOrder,double Lcutoff, double Ucutoff, double *DenC)
{
double *TCoeffs;
double *NumCoeffs;
std::complex<double> *NormalizedKernel;
double Numbers[11]={0,1,2,3,4,5,6,7,8,9,10};
int i;
NumCoeffs = (double *)calloc( 2*FilterOrder+1, sizeof(double) );
if( NumCoeffs == NULL ) return( NULL );
NormalizedKernel = (std::complex<double> *)calloc( 2*FilterOrder+1, sizeof(std::complex<double>) );
if( NormalizedKernel == NULL ) return( NULL );
TCoeffs = ComputeHP(FilterOrder);
if( TCoeffs == NULL ) return( NULL );
for( i = 0; i < FilterOrder; ++i)
{
NumCoeffs[2*i] = TCoeffs[i];
NumCoeffs[2*i+1] = 0.0;
}
NumCoeffs[2*FilterOrder] = TCoeffs[FilterOrder];
double cp[2];
double Bw, Wn;
cp[0] = 2*2.0*tan(PI * Lcutoff/ 2.0);
cp[1] = 2*2.0*tan(PI * Ucutoff / 2.0);
Bw = cp[1] - cp[0];
//center frequency
Wn = sqrt(cp[0]*cp[1]);
Wn = 2*atan2(Wn,4);
double kern;
const std::complex<double> result = std::complex<double>(-1,0);
for(int k = 0; k<11; k++)
{
NormalizedKernel[k] = std::exp(-sqrt(result)*Wn*Numbers[k]);
}
double b=0;
double den=0;
for(int d = 0; d<11; d++)
{
b+=real(NormalizedKernel[d]*NumCoeffs[d]);
den+=real(NormalizedKernel[d]*DenC[d]);
}
for(int c = 0; c<11; c++)
{
NumCoeffs[c]=(NumCoeffs[c]*den)/b;
}
free(TCoeffs);
return NumCoeffs;
}
There are code which could be found online implementing butterworth filter. If you use the source code to try to get result matching MATLAB results, there will be the same problem.Basically the result you got from the code hasn't been normalized, and in the source code there is a variable sff in bwhp.c. If you set that to 1, the problem will be easily solved.
I recommend you to use this source code and
the source code and usage could be found here
I added the final edition of function ComputeNumCoeffs to the program and fix "FilterOrder" (k<11 to k<2*FiltOrd+1). Maybe it will save someone's time.
f1=0.5Gz, f2=10Gz, fs=127Gz/2
In MatLab
a={1.000000000000000,-3.329746259105707, 4.180522138699884,-2.365540522960743,0.514875789136976};
b={0.041065495448784, 0.000000000000000,-0.082130990897568, 0.000000000000000,0.041065495448784};
Program:
#include <iostream>
#include <stdio.h>
#include <vector>
#include <math.h>
#include <complex>
using namespace std;
#define N 10 //The number of images which construct a time series for each pixel
#define PI 3.1415926535897932384626433832795
double *ComputeLP(int FilterOrder)
{
double *NumCoeffs;
int m;
int i;
NumCoeffs = (double *)calloc(FilterOrder+1, sizeof(double));
if(NumCoeffs == NULL) return(NULL);
NumCoeffs[0] = 1;
NumCoeffs[1] = FilterOrder;
m = FilterOrder/2;
for(i=2; i <= m; ++i)
{
NumCoeffs[i] =(double) (FilterOrder-i+1)*NumCoeffs[i-1]/i;
NumCoeffs[FilterOrder-i]= NumCoeffs[i];
}
NumCoeffs[FilterOrder-1] = FilterOrder;
NumCoeffs[FilterOrder] = 1;
return NumCoeffs;
}
double *ComputeHP(int FilterOrder)
{
double *NumCoeffs;
int i;
NumCoeffs = ComputeLP(FilterOrder);
if(NumCoeffs == NULL) return(NULL);
for(i = 0; i <= FilterOrder; ++i)
if(i % 2) NumCoeffs[i] = -NumCoeffs[i];
return NumCoeffs;
}
double *TrinomialMultiply(int FilterOrder, double *b, double *c)
{
int i, j;
double *RetVal;
RetVal = (double *)calloc(4 * FilterOrder, sizeof(double));
if(RetVal == NULL) return(NULL);
RetVal[2] = c[0];
RetVal[3] = c[1];
RetVal[0] = b[0];
RetVal[1] = b[1];
for(i = 1; i < FilterOrder; ++i)
{
RetVal[2*(2*i+1)] += c[2*i] * RetVal[2*(2*i-1)] - c[2*i+1] * RetVal[2*(2*i-1)+1];
RetVal[2*(2*i+1)+1] += c[2*i] * RetVal[2*(2*i-1)+1] + c[2*i+1] * RetVal[2*(2*i-1)];
for(j = 2*i; j > 1; --j)
{
RetVal[2*j] += b[2*i] * RetVal[2*(j-1)] - b[2*i+1] * RetVal[2*(j-1)+1] +
c[2*i] * RetVal[2*(j-2)] - c[2*i+1] * RetVal[2*(j-2)+1];
RetVal[2*j+1] += b[2*i] * RetVal[2*(j-1)+1] + b[2*i+1] * RetVal[2*(j-1)] +
c[2*i] * RetVal[2*(j-2)+1] + c[2*i+1] * RetVal[2*(j-2)];
}
RetVal[2] += b[2*i] * RetVal[0] - b[2*i+1] * RetVal[1] + c[2*i];
RetVal[3] += b[2*i] * RetVal[1] + b[2*i+1] * RetVal[0] + c[2*i+1];
RetVal[0] += b[2*i];
RetVal[1] += b[2*i+1];
}
return RetVal;
}
double *ComputeNumCoeffs(int FilterOrder,double Lcutoff, double Ucutoff, double *DenC)
{
double *TCoeffs;
double *NumCoeffs;
std::complex<double> *NormalizedKernel;
double Numbers[11]={0,1,2,3,4,5,6,7,8,9,10};
int i;
NumCoeffs = (double *)calloc(2*FilterOrder+1, sizeof(double));
if(NumCoeffs == NULL) return(NULL);
NormalizedKernel = (std::complex<double> *)calloc(2*FilterOrder+1, sizeof(std::complex<double>));
if(NormalizedKernel == NULL) return(NULL);
TCoeffs = ComputeHP(FilterOrder);
if(TCoeffs == NULL) return(NULL);
for(i = 0; i < FilterOrder; ++i)
{
NumCoeffs[2*i] = TCoeffs[i];
NumCoeffs[2*i+1] = 0.0;
}
NumCoeffs[2*FilterOrder] = TCoeffs[FilterOrder];
double cp[2];
//double Bw;
double Wn;
cp[0] = 2*2.0*tan(PI * Lcutoff/ 2.0);
cp[1] = 2*2.0*tan(PI * Ucutoff/2.0);
//Bw = cp[1] - cp[0];
//center frequency
Wn = sqrt(cp[0]*cp[1]);
Wn = 2*atan2(Wn,4);
//double kern;
const std::complex<double> result = std::complex<double>(-1,0);
for(int k = 0; k<2*FilterOrder+1; k++)
{
NormalizedKernel[k] = std::exp(-sqrt(result)*Wn*Numbers[k]);
}
double b=0;
double den=0;
for(int d = 0; d<2*FilterOrder+1; d++)
{
b+=real(NormalizedKernel[d]*NumCoeffs[d]);
den+=real(NormalizedKernel[d]*DenC[d]);
}
for(int c = 0; c<2*FilterOrder+1; c++)
{
NumCoeffs[c]=(NumCoeffs[c]*den)/b;
}
free(TCoeffs);
return NumCoeffs;
}
double *ComputeDenCoeffs(int FilterOrder, double Lcutoff, double Ucutoff)
{
int k; // loop variables
double theta; // PI * (Ucutoff - Lcutoff)/2.0
double cp; // cosine of phi
double st; // sine of theta
double ct; // cosine of theta
double s2t; // sine of 2*theta
double c2t; // cosine 0f 2*theta
double *RCoeffs; // z^-2 coefficients
double *TCoeffs; // z^-1 coefficients
double *DenomCoeffs; // dk coefficients
double PoleAngle; // pole angle
double SinPoleAngle; // sine of pole angle
double CosPoleAngle; // cosine of pole angle
double a; // workspace variables
cp = cos(PI * (Ucutoff + Lcutoff)/2.0);
theta = PI * (Ucutoff - Lcutoff)/2.0;
st = sin(theta);
ct = cos(theta);
s2t = 2.0*st*ct; // sine of 2*theta
c2t = 2.0*ct*ct - 1.0; // cosine of 2*theta
RCoeffs = (double *)calloc(2 * FilterOrder, sizeof(double));
TCoeffs = (double *)calloc(2 * FilterOrder, sizeof(double));
for(k = 0; k < FilterOrder; ++k)
{
PoleAngle = PI * (double)(2*k+1)/(double)(2*FilterOrder);
SinPoleAngle = sin(PoleAngle);
CosPoleAngle = cos(PoleAngle);
a = 1.0 + s2t*SinPoleAngle;
RCoeffs[2*k] = c2t/a;
RCoeffs[2*k+1] = s2t*CosPoleAngle/a;
TCoeffs[2*k] = -2.0*cp*(ct+st*SinPoleAngle)/a;
TCoeffs[2*k+1] = -2.0*cp*st*CosPoleAngle/a;
}
DenomCoeffs = TrinomialMultiply(FilterOrder, TCoeffs, RCoeffs);
free(TCoeffs);
free(RCoeffs);
DenomCoeffs[1] = DenomCoeffs[0];
DenomCoeffs[0] = 1.0;
for(k = 3; k <= 2*FilterOrder; ++k)
DenomCoeffs[k] = DenomCoeffs[2*k-2];
return DenomCoeffs;
}
void filter(int ord, double *a, double *b, int np, double *x, double *y)
{
int i,j;
y[0]=b[0] * x[0];
for (i=1;i<ord+1;i++)
{
y[i]=0.0;
for (j=0;j<i+1;j++)
y[i]=y[i]+b[j]*x[i-j];
for (j=0;j<i;j++)
y[i]=y[i]-a[j+1]*y[i-j-1];
}
for (i=ord+1;i<np+1;i++)
{
y[i]=0.0;
for (j=0;j<ord+1;j++)
y[i]=y[i]+b[j]*x[i-j];
for (j=0;j<ord;j++)
y[i]=y[i]-a[j+1]*y[i-j-1];
}
}
int main(int argc, char *argv[])
{
(void)argc;
(void)argv;
//Frequency bands is a vector of values - Lower Frequency Band and Higher Frequency Band
//First value is lower cutoff and second value is higher cutoff
//f1 = 0.5Gz f2=10Gz
//fs=127Gz
//Kotelnikov/2=Nyquist (127/2)
double FrequencyBands[2] = {0.5/(127.0/2.0),10.0/(127.0/2.0)};//these values are as a ratio of f/fs, where fs is sampling rate, and f is cutoff frequency
//and therefore should lie in the range [0 1]
//Filter Order
int FiltOrd = 2;//5;
//Pixel Time Series
/*int PixelTimeSeries[N];
int outputSeries[N];
*/
//Create the variables for the numerator and denominator coefficients
double *DenC = 0;
double *NumC = 0;
//Pass Numerator Coefficients and Denominator Coefficients arrays into function, will return the same
printf("\n");
//is A in matlab function and the numbers are correct
DenC = ComputeDenCoeffs(FiltOrd, FrequencyBands[0], FrequencyBands[1]);
for(int k = 0; k<2*FiltOrd+1; k++)
{
printf("DenC is: %lf\n", DenC[k]);
}
printf("\n");
NumC = ComputeNumCoeffs(FiltOrd,FrequencyBands[0],FrequencyBands[1],DenC);
for(int k = 0; k<2*FiltOrd+1; k++)
{
printf("NumC is: %lf\n", NumC[k]);
}
double y[5];
double x[5]={1,2,3,4,5};
filter(5, DenC, NumC, 5, x, y);
return 1;
}