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()