Math::Matrix<4, 4> Math::Matrix<4, 4>::rotation(double x, double y, double z)
{
Math::Matrix<4, 4> m_x = Math::Matrix<4, 4>::identite();
Math::Matrix<4, 4> m_y = Math::Matrix<4, 4>::identite();
Math::Matrix<4, 4> m_z = Math::Matrix<4, 4>::identite();
// Convert parameters to radian
x = Math::Utils::DegToRad(x);
y = Math::Utils::DegToRad(y);
z = Math::Utils::DegToRad(z);
// Generate rotation matrix
m_x(1, 1) = +std::cos(x);
m_x(1, 2) = -std::sin(x);
m_x(2, 1) = +std::sin(x);
m_x(2, 2) = +std::cos(x);
m_y(0, 0) = +std::cos(y);
m_y(0, 2) = +std::sin(y);
m_y(2, 0) = -std::sin(y);
m_y(2, 2) = +std::cos(y);
m_z(0, 0) = +std::cos(z);
m_z(0, 1) = -std::sin(z);
m_z(1, 0) = +std::sin(z);
m_z(1, 1) = +std::cos(z);
return m_z * m_y * m_x;
}
// symbolRate > 0
void BaseBandDemodulate::SetSymbolRate(unsigned symbolRate) {
if (symbolRate > 0) {
m_symbolRate = symbolRate;
m_Tsymbol = 1/double(symbolRate);
m_scaleFactor = 1.0/m_z(0.0);
}
} // BaseBandDemodulate::SetSymbolRate(k*symbolRate)
// N > 0
void BaseBandDemodulate::SetSamplesPerSymbol(unsigned N) {
if (N > 0 && m_N != N) {
m_N = N;
m_ReAllocDynamicArrays();
m_scaleFactor = 1.0/m_z(0.0);
}
} // BaseBandDemodulate::SetSamplesPerSymbol(N)
// Lsymbols > 0. See note on SetNyquistRollOff().
void BaseBandDemodulate::SetPulseSymbolSpan(unsigned Lsymbols) {
if (Lsymbols > 0 && m_Lsymbols != Lsymbols) {
m_Lsymbols = Lsymbols;
m_ReAllocDynamicArrays();
m_scaleFactor = 1.0/m_z(0.0);
}
} // BaseBandDemodulate::SetPulseSymbolSpan(Lsymbol)
void testKalmanFilter3D()
{
float F[3][3] = { { 1, 1, 1 },
{ 0, 1, 1 },
{ 0, 0, 1 } };
float H[3] = {1, 0, 0};
// init kalman filter
pkm::Kalman kf;
// track position and velocity, and acceleration (assumes torque is 0)
kf.allocate(3, 1);
// set initial pos/vel/accel to 0
// (x)
kf.setState(pkm::Mat(3, 1, 0.0f));
// create a transition matrix for modeling the physics of the system
// the first row corresponds to using the velocity and position to update the new position:
// x = 1*x + 1*dx*dt + 1*dx^2*d^2t
// the second row uses the velocity and acceleration to update the velocity
// dx = 0*x + 1*dx*dt + 1*dx^2*d^2t
// dx^2 = 0*x + 0*dx*dt + 1*dx^2*d^2t
// (F)
kf.setStateTransitionMatrix(pkm::Mat(3, 3, &F[0][0]));
// How much noise we expect to have in our measurement sensor (fixed gaussian)
// (R)
kf.setMeasurementNoise(0.1f);
// Large initial covariance of starting pos/vel saying how much certainty we have in our state (variable)
// (P)
kf.setStateCovariance(100.0f);
// Unmodeled noise
// (Q)
kf.setProcessNoise(0.1, 0.01);
// Converts our state to a measurement, so position, (what we measure), = x*1 + dx*0 + dx^2*0
// (H)
kf.setMeasurementFunction(pkm::Mat(1, 3, &H[0]));
size_t n_obs = 1000;
float noise = 1.0;
pkm::Mat m_z(1, 2, 0.0f);
m_z[1] = 100.0f;
m_z.interpolate(1, n_obs);
m_z = m_z + pkm::Mat::rand(1, n_obs) * noise;
for (size_t i = 0; i < n_obs; i++) {
kf.update(m_z.colRange(i, i+1));
kf.predict();
std::cout << "obs: " << m_z.colRange(i, i+1)[0] << ", filt: " << kf.getState()[0] << endl;
}
}
//
// BaseBand demodulate incoming baseband digital signal (r(t)) samples
// by application of a matched filter.
//
void BaseBandDemodulate::m_Demodulate() {
double deltat = m_Tsymbol/m_N;
double halfspan = m_Lsymbols/2.0f;
double t0;
unsigned i, j;
for (i=0; i < m_Lsymbols; i++) {
t0 = (i-halfspan)*m_Tsymbol;
for (j=0; j < m_N; j++) {
m_Z[i] +=
int(double(m_baseBandSamples[j])*m_scaleFactor*m_z(t0+j*deltat));
}
}
} // BaseBandDemodulate::m_Modulate()
//
// compute baseband digital signal output samples(gi(t)) from (m_Lsymbols)
// span of input PCM symbols modulated with z(t)
//
void BaseBandModulate::m_Modulate() {
double deltat = m_Tsymbol/m_N;
double halfspan = m_Lsymbols/2.0f;
double t0;
unsigned i, j;
// zero out output signal samples
memset(m_baseBandSamples, 0, m_N*sizeof(int));
for (i=0; i < m_Lsymbols; i++) {
t0 = (i-halfspan)*m_Tsymbol;
for (j=0; j < m_N; j++) {
m_baseBandSamples[j] += int(m_PCMSymbols[i]*m_scaleFactor*m_z(t0+j*deltat));
}
}
} // BaseBandModulate::m_Modulate()
void set_points(const TVector &x, const TVector &y)
{
assert(x.size() == y.size());
assert(x.size()>2);
int n =static_cast<int>(x.size());
m_x.assign(x.begin(), x.end());
m_c0.assign(y.begin(), y.end());
m_c1.resize(n);
m_c2.resize(n);
m_c3.resize(n);
Vector<T, e_host> m_h(n);
Vector<T, e_host> m_l(n);
Vector<T, e_host> m_mu(n);
Vector<T, e_host> m_z(n);
n--;
for(auto i = 0; i < n; i++)
{
m_h[i] = m_x[i+1]-m_x[i];
}
m_l[0] = 1;
m_mu[0] = 0;
m_z[0] = 0;
for(auto i = 1; i < n; i++)
{
m_l[i] = 2*(m_x[i+1]-m_x[i-1])-m_h[i-1]*m_mu[i-1];
m_mu[i] = m_h[i]/m_l[i];
auto alpha = 3*(m_c0[i+1]-m_c0[i])/m_h[i]-3*(m_c0[i]-m_c0[i-1])/m_h[i-1];
m_z[i] = (alpha-m_h[i-1]*m_z[i-1])/m_l[i];
}
m_l[n] = 1;
m_z[n] = 0;
m_c2[n] = 0;
for(auto i =n-1; i >= 0; i--)
{
m_c2[i] = m_z[i] - m_mu[i]*m_c2[i+1];
m_c1[i] = (m_c0[i+1]-m_c0[i])/m_h[i]-m_h[i]*(m_c2[i+1]+2*m_c2[i])/3;
m_c3[i] = (m_c2[i+1]-m_c2[i])/(3*m_h[i]);
}
}
void BaseBandDemodulate::PlotFilter() {
char title[256];
double deltat = m_Tsymbol/m_N;
double halfspan = m_Lsymbols/2.0f;
double t0;
unsigned i, j;
sprintf(title,
"raised cosine recv filter: N=%d, Lsymbols=%d, R=%f",
m_N, m_Lsymbols, m_nyquistRollOff);
for (i=0; i < m_Lsymbols; i++) {
t0 = (i-halfspan)*m_Tsymbol;
for (j=0; j < m_N; j++) {
gd.AppendStreamData(title,
float((t0+j*deltat)/m_Tsymbol),
float(m_scaleFactor*m_z(t0+j*deltat)));
}
}
gd.SetStreamDataPlotOptions(title, "with impulses");
} // BaseBandDemodulate::PlotFilter()
//
// Set/Get Nyquist Roll-off for the root raised cosine filter
// 0.0 <= r <= 1.0
//
// Note: The following function may be helpful for determining a
// value for Lsymbols:
// Lsymbols = -44*R + 33 where 0.2 < R < 0.75
//
void BaseBandDemodulate::SetNyquistRollOff(double r) {
if (0.0f <= r && r <= 1.0f) {
m_nyquistRollOff = r;
m_scaleFactor = 1.0/m_z(0.0);
}
} // BaseBandDemodulate::SetNyquistRollOff()
