コード例 #1
0
ファイル: lloyd_algorithm.cpp プロジェクト: freehawkzk/cgal
        void operator () (const Point &a,
                          const Point &b,
                          const Point &c) {
            Vector ex(1, 0, 0),
                   ey(0, 1, 0),
                   ez(0, 0, 1);

            Vector va = CGAL::ORIGIN - a;
            Vector vb = CGAL::ORIGIN - b;
            Vector vc = CGAL::ORIGIN - c;

            // Updating the volume...
            Vector nhat = CGAL::cross_product(b - a, c - a);
            vol += nhat * va;

            // ... and the centroid
            // X
            cx += (nhat * ex) * (
                CGAL::square(ex * (va + vb)) +
                CGAL::square(ex * (vb + vc)) +
                CGAL::square(ex * (vc + va)) );

            // Y
            cy += (nhat * ey) * (
                CGAL::square(ey * (va + vb)) +
                CGAL::square(ey * (vb + vc)) +
                CGAL::square(ey * (vc + va)) );

            // Z
            cz += (nhat * ez) * (
                CGAL::square(ez * (va + vb)) +
                CGAL::square(ez * (vb + vc)) +
                CGAL::square(ez * (vc + va)) );
        }
コード例 #2
0
ファイル: Unit1.cpp プロジェクト: MChudak/builder_nbody
void __fastcall TForm1::Button1Click(TObject *Sender)
{
        static int wyb;
        resettablicy();
        wyb = ComboBox1->ItemIndex;
        if (wyb==1){
                Ilosc = 2;
                L[0].x = ex(-1.0);
                L[0].y = ey(0.0);
                L[0].vx= 0.0;
                L[0].vy= -75.0;

                L[1].x =  ex(1.0);
                L[1].y =  ey(0.0);
                L[1].vx=  0.0;
                L[1].vy=  75.0;
        }
}
コード例 #3
0
ファイル: radg3d.cpp プロジェクト: aprodi/mcradia
void radTg3d::CheckAxesExchangeForSubdInLabFrame(double* SubdivArray, char& ConversionToPolyhedronsIsNeeded)
{
	ConversionToPolyhedronsIsNeeded = 0;

	radTrans ResTransf;
	short SomethingFound = 0;
	FindResTransfWithMultOne(ResTransf, SomethingFound);
	if(SomethingFound) 
	{
		const double ZeroTol = 1.E-13;
		TVector3d ex(1.,0.,0.), ey(0.,1.,0.), ez(0.,0.,1.);
		TVector3d ex1 = ResTransf.TrBiPoint_inv(ex);
		TVector3d ey1 = ResTransf.TrBiPoint_inv(ey);
		TVector3d ez1 = ResTransf.TrBiPoint_inv(ez);

		if(PracticallyEqualOrAnti(ex, ex1, ZeroTol))
		{
			if(!(PracticallyEqualOrAnti(ey, ey1, ZeroTol) || PracticallyEqualOrAnti(ey, ez1, ZeroTol)))
				ConversionToPolyhedronsIsNeeded = 1;
		}
		else if(PracticallyEqualOrAnti(ex, ey1, ZeroTol))
		{
			if(!(PracticallyEqualOrAnti(ey, ex1, ZeroTol) || PracticallyEqualOrAnti(ey, ez1, ZeroTol)))
				ConversionToPolyhedronsIsNeeded = 1;
		}
		else if(PracticallyEqualOrAnti(ex, ez1, ZeroTol))
		{
			if(!(PracticallyEqualOrAnti(ey, ex1, ZeroTol) || PracticallyEqualOrAnti(ey, ey1, ZeroTol)))
				ConversionToPolyhedronsIsNeeded = 1;
		}
		else ConversionToPolyhedronsIsNeeded = 1;

		if(!ConversionToPolyhedronsIsNeeded)
		{
			double &kx = SubdivArray[0], &ky = SubdivArray[2],  &kz = SubdivArray[4];
			double &qx = SubdivArray[1], &qy = SubdivArray[3],  &qz = SubdivArray[5];

			TVector3d kx_ex1 = kx*ex1, ky_ey1 = ky*ey1, kz_ez1 = kz*ez1;
			TVector3d k1Tot = kx_ex1 + ky_ey1 + kz_ez1;
			TVector3d qx_ex1 = qx*ex1, qy_ey1 = qy*ey1, qz_ez1 = qz*ez1;
			TVector3d q1Tot = qx_ex1 + qy_ey1 + qz_ez1;

			kx = ex*k1Tot; qx = ex*q1Tot;
			if(kx < 0.) { kx = -kx; qx = -1./qx;}
			ky = ey*k1Tot; qy = ey*q1Tot;
			if(ky < 0.) { ky = -ky; qy = -1./qy;}
			kz = ez*k1Tot; qz = ez*q1Tot;
			if(kz < 0.) { kz = -kz; qz = -1./qz;}
		}
	}
}
コード例 #4
0
ファイル: base.cpp プロジェクト: EthanRutherford/geneticcars
Vector3D Matrix3D::solve3(Vector3D b)
{
	Vector3D ex(m[0][0], m[1][0], m[2][0]);
	Vector3D ey(m[0][1], m[1][1], m[2][1]);
	Vector3D ez(m[0][2], m[1][2], m[2][2]);
	double det = dot(ex, cross(ey, ez));
	if (det != 0)
		det = 1 / det;
	Vector3D x;
	x.x = det * dot(b, cross(ey, ez));
	x.y = det * dot(ex, cross(b, ez));
	x.z = det * dot(ex, cross(ey, b));
	return x;
}
コード例 #5
0
void player_bullets_draw(player_bullet *b, int liczba, BITMAP *bufor)
{
	for (int i = 0; i < liczba; i++)
	{
		clear_to_color(b[i].obraz_rot, makecol(255, 0, 255));
		rotate_sprite(b[i].obraz_rot, b[i].obraz,
				(b[i].obraz_rot->w - b[i].obraz->w) / 2,
				(b[i].obraz_rot->h - b[i].obraz->h) / 2,
				itofix((int) (256.0 * b[i].rot / (2.0 * pi))));

		masked_blit(b[i].obraz_rot, bufor, 0, 0,
				ex(b[i].h * sin(b[i].a)) / 2 - b[i].obraz_rot->w / 2,
				ey(b[i].h * cos(b[i].a)) / 2 - b[i].obraz_rot->h / 2,
				b[i].obraz_rot->w, b[i].obraz_rot->h);
	}
}
コード例 #6
0
/*!
  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features.

  \param s_star : Desired 3D point visual feature.

  \param select : The error can be computed for a selection of a
  subset of the possible 3D point coordinate features.
  - To compute the error for all the three coordinates use
    vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3 
    dimension column vector.
  - To compute the error for only one of the coordinate
    feature \f$(X,Y, or Z)\f$ use one of the
    corresponding function selectX(), selectY() or selectZ(). In
    that case the error vector is a 1 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature.

  The code below shows how to use this method to manipulate the \f$
  Z \f$ subset:

  \code
  // Creation of the current feature s
  vpFeaturePoint3D s;
  s.set_Z(0.8); // Initialization of the current Z feature

  // Creation of the desired feature s*. 
  vpFeatureTranslation s_star; 
  s_star.set_Z(1); // Initialization of the current Z* feature to Z*=1 meter 

  // Compute the interaction matrix for the Z coordinate feature
  vpMatrix L_Z = s.interaction( vpFeaturePoint3D::selectZ() );

  // Compute the error vector (s-s*) for the Z feature
  s.error(s_star, vpFeaturePoint3D::selectZ());
  \endcode

  To manipulate the subset features \f$s=(Y, Z)\f$,
  the code becomes:
  \code
  // Compute the interaction matrix for the Y, Z feature coordinates
  vpMatrix L_YZ = s.interaction( vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ() );

  // Compute the error vector e = (s-s*) for the Y, Z feature coordinates
  vpColVector e = s.error(s_star, vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ());
  \endcode

*/
vpColVector
vpFeaturePoint3D::error(const vpBasicFeature &s_star,
			const unsigned int select)
{
  vpColVector e(0) ;

  try{
    if (vpFeaturePoint3D::selectX() & select )
    {
      vpColVector ex(1) ;
      ex[0] = s[0] - s_star[0] ;

      e = vpMatrix::stackMatrices(e,ex) ;
    }

    if (vpFeaturePoint3D::selectY() & select )
    {
      vpColVector ey(1) ;
      ey[0] = s[1] - s_star[1] ;
      e =  vpMatrix::stackMatrices(e,ey) ;
    }

    if (vpFeaturePoint3D::selectZ() & select )
    {
      vpColVector ez(1) ;
      ez[0] = s[2] - s_star[2] ;
      e =  vpMatrix::stackMatrices(e,ez) ;
    }
  }
  catch(vpMatrixException me)
  {
    vpERROR_TRACE("caught a Matrix related error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("caught another error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }

  return e ;

}
コード例 #7
0
/*!
  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features.

  - With the feature type cdMc:
  Since this visual feature \f$ s \f$ represent the 3D translation from the desired
  camera frame to the current one \f$^{c^*}t_{c} \f$, the desired
  visual feature \f$ s^* \f$ should be zero. Thus, the error is here
  equal to the current visual feature \f$ s \f$.

  - With the feature type cMo:
  In this case the desired feature is not necessary equal to zero. Thus, the error is here equal to \f$ s-s^* \f$.

  \param s_star : Desired visual feature.

  \param select : The error can be computed for a selection of a
  subset of the possible translation features.
  - To compute the error for all the three translation vector coordinates use
    vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3 
    dimension column vector.
  - To compute the error for only one of the translation vector coordinate
    feature \f$(t_x, t_y, t_z)\f$ use one of the
    corresponding function selectTx(), selectTy() or selectTz(). In
    that case the error vector is a 1 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature.

  \exception vpFeatureException::badInitializationError : If the
  desired visual feature \f$ s^* \f$ is not equal to zero in the case of the feature type is cdMc or cMcd.

  The code below shows how to use this method to manipulate the \f$
  t_z \f$ subset in the case of the cdMc feature type. It can be used also with the cMo feature type. In that case just change vpFeatureTranslation::cdMc by vpFeatureTranslation::cMo during the declaration of the two vpFeatureTranslation features.

  \code
  // Creation of the current feature s
  vpFeatureTranslation s(vpFeatureTranslation::cdMc);
  s.set_TUz(0.3); // Initialization of the feature

  // Creation of the desired feature s*. By default this feature is 
  // initialized to zero
  vpFeatureTranslation s_star(vpFeatureTranslation::cdMc); 

  // Compute the interaction matrix for the t_z translation feature
  vpMatrix L_z = s.interaction( vpFeatureTranslation::selectTz() );

  // Compute the error vector (s-s*) for the t_z feature
  s.error(s_star, vpFeatureTranslation::selectTz());
  \endcode

  To manipulate the subset features \f$s=(t_y, t_z)\f$,
  the code becomes:
  \code
  // Compute the interaction matrix for the t_y, t_z features
  vpMatrix L_yz = s.interaction( vpFeatureTranslation::selectTy() | vpFeatureTranslation::selectTz() );

  // Compute the error vector e = (s-s*) for the t_y, t_z feature
  vpColVector e = s.error(s_star, vpFeatureTranslation::selectTy() | vpFeatureTranslation::selectTz());
  \endcode

*/
vpColVector
vpFeatureTranslation::error(const vpBasicFeature &s_star,
			    const unsigned int select)
{
  vpColVector e(0) ;

  if(translation == cdMc || translation == cMcd)
  {
    if (s_star.get_s().sumSquare() > 1e-6)
    {
      vpERROR_TRACE("s* should be zero ! ") ;
      throw(vpFeatureException(vpFeatureException::badInitializationError,
			       "s* should be zero !")) ;
    }
  }


  if (vpFeatureTranslation::selectTx() & select )
    {
      vpColVector ex(1) ;
      ex[0] = s[0]-s_star[0]  ;
      e = vpMatrix::stackMatrices(e,ex) ;
    }

  if (vpFeatureTranslation::selectTy() & select )
    {
      vpColVector ey(1) ;
      ey[0] = s[1]-s_star[1] ;
      e = vpMatrix::stackMatrices(e,ey) ;
    }

  if (vpFeatureTranslation::selectTz() & select )
    {
      vpColVector ez(1) ;
      ez[0] = s[2]-s_star[2] ;
      e = vpMatrix::stackMatrices(e,ez) ;
    }

  return e ;
}
コード例 #8
0
ファイル: stacje.c プロジェクト: MChudak/space-shooter
void draw_all_stations(station *stacje, int n, BITMAP *bufor)
{
	for (int i = 0; i < n; i++)
	{
		if (!stacje[i].zywa)
			continue;

		clear_to_color(stacje[i].budynek_rot, makecol(255, 0, 255));

		rotate_sprite(stacje[i].budynek_rot, stacje[i].budynek,
				(stacje[i].budynek_rot->w - stacje[i].budynek->w) / 2,
				(stacje[i].budynek_rot->h - stacje[i].budynek->h) / 2,
				itofix((int) (256.0 * stacje[i].a / (2.0 * pi))));

		masked_blit(stacje[i].budynek_rot, bufor, 0, 0,
				ex(stacje[i].h * sin(stacje[i].a)) / 2
						- stacje[i].budynek_rot->w / 2,
				ey(stacje[i].h * cos(stacje[i].a)) / 2
						- stacje[i].budynek_rot->h / 2,
				stacje[i].budynek_rot->w, stacje[i].budynek_rot->h);
	}
}
コード例 #9
0
ファイル: scriptwidget.cpp プロジェクト: dukie/Little-World
void ScriptWidget::timerEvent(QTimerEvent *)
{
    QPointF player = m_scene->camera().pos();
    QPointF entity = m_entity->pos();

    QScriptValue px(m_engine, player.x());
    QScriptValue py(m_engine, player.y());
    QScriptValue ex(m_engine, entity.x());
    QScriptValue ey(m_engine, entity.y());
    QScriptValue time(m_engine, m_time.elapsed());

    m_engine->globalObject().setProperty("player_x", px);
    m_engine->globalObject().setProperty("player_y", py);
    m_engine->globalObject().setProperty("my_x", ex);
    m_engine->globalObject().setProperty("my_y", ey);
    m_engine->globalObject().setProperty("time", time);

    m_engine->evaluate(m_source);
    if (m_engine->hasUncaughtException()) {
        QString text = m_engine->uncaughtException().toString();
        m_statusView->setText(text);
    }
}
コード例 #10
0
ファイル: vpFeatureThetaU.cpp プロジェクト: 976717326/visp
/*!
  
  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features.

  Since this visual feature \f$ s \f$ represent either the rotation
  from the desired camera frame to the current camera frame
  \f$^{c^*}R_{c} \f$, or the rotation from the current camera frame to
  the desired camera frame \f$^{c}R_{c^*} \f$, the desired visual
  feature \f$ s^* \f$ should be zero. Thus, the error is here equal to
  the current visual feature \f$ s \f$.

  \param s_star : Desired visual visual feature that should be equal to zero.

  \param select : The error can be computed for a selection of a
  subset of the possible \f$ \theta u \f$ features.
  - To compute the error for all the three \f$ \theta u \f$ features use
    vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3 
    dimension column vector.
  - To compute the error for only one of the \f$ \theta u \f$ component 
    feature (\f$ \theta u_x, \theta u_y, \theta u_z\f$) use one of the
    corresponding function selectTUx(), selectTUy() or selectTUz(). In
    that case the error vector is a 1 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature.

  \exception vpFeatureException::badInitializationError : If the
  desired visual feature \f$ s^* \f$ is not equal to zero.

  The code below shows how to use this method to manipulate the
  \f$\theta u_z \f$ subset:

  \code
  // Creation of the current feature s
  vpFeatureThetaU s(vpFeatureThetaU::cdRc);
  s.set_TUz(0.3);

  // Creation of the desired feature s^*. By default this feature is 
  // initialized to zero
  vpFeatureThetaU s_star(vpFeatureThetaU::cdRc); 

  // Compute the interaction matrix for the ThetaU_z feature
  vpMatrix L_z = s.interaction( vpFeatureThetaU::selectTUz() );

  // Compute the error vector (s-s*) for the ThetaU_z feature
  s.error(s_star, vpFeatureThetaU::selectTUz());
  \endcode

  To manipulate the subset features \f$s=(\theta u_y, \theta u_z)\f$,
  the code becomes:
  \code
  // Compute the interaction matrix for the ThetaU_y, ThetaU_z features
  vpMatrix L_yz = s.interaction( vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz() );

  // Compute the error vector e = (s-s*) for the ThetaU_y, ThetaU_z feature
  vpColVector e = s.error(s_star, vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz());
  \endcode

*/
vpColVector
vpFeatureThetaU::error(const vpBasicFeature &s_star,
		       const unsigned int select)
{

  if (fabs(s_star.get_s().sumSquare()) > 1e-6)
    {
      vpERROR_TRACE("s* should be zero ! ") ;
      throw(vpFeatureException(vpFeatureException::badInitializationError,
			       "s* should be zero !")) ;
    }

  vpColVector e(0) ;


  if (vpFeatureThetaU::selectTUx() & select )
    {
      vpColVector ex(1) ;
      ex[0] = s[0]  ;
      e = vpColVector::stack(e,ex) ;
    }

  if (vpFeatureThetaU::selectTUy() & select )
    {
      vpColVector ey(1) ;
      ey[0] = s[1] ;
      e = vpColVector::stack(e,ey) ;
    }

  if (vpFeatureThetaU::selectTUz() & select )
    {
      vpColVector ez(1) ;
      ez[0] = s[2] ;
      e = vpColVector::stack(e,ez) ;
    }
  return e ;
}
コード例 #11
0
ファイル: vpFeaturePoint3D.cpp プロジェクト: 976717326/visp
/*!
  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features.

  \param s_star : Desired 3D point visual feature.

  \param select : The error can be computed for a selection of a
  subset of the possible 3D point coordinate features.
  - To compute the error for all the three coordinates use
    vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3 
    dimension column vector.
  - To compute the error for only one of the coordinate
    feature \f$(X,Y, or Z)\f$ use one of the
    corresponding function selectX(), selectY() or selectZ(). In
    that case the error vector is a 1 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature.

  The code below shows how to use this method to manipulate the \f$
  Z \f$ subset:

  \code
  // Creation of the current feature s
  vpFeaturePoint3D s;
  s.set_Z(0.8); // Initialization of the current Z feature

  // Creation of the desired feature s*. 
  vpFeatureTranslation s_star; 
  s_star.set_Z(1); // Initialization of the current Z* feature to Z*=1 meter 

  // Compute the interaction matrix for the Z coordinate feature
  vpMatrix L_Z = s.interaction( vpFeaturePoint3D::selectZ() );

  // Compute the error vector (s-s*) for the Z feature
  s.error(s_star, vpFeaturePoint3D::selectZ());
  \endcode

  To manipulate the subset features \f$s=(Y, Z)\f$,
  the code becomes:
  \code
  // Compute the interaction matrix for the Y, Z feature coordinates
  vpMatrix L_YZ = s.interaction( vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ() );

  // Compute the error vector e = (s-s*) for the Y, Z feature coordinates
  vpColVector e = s.error(s_star, vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ());
  \endcode

*/
vpColVector
vpFeaturePoint3D::error(const vpBasicFeature &s_star,
			const unsigned int select)
{
  vpColVector e(0) ;

  try{
    if (vpFeaturePoint3D::selectX() & select )
    {
      vpColVector ex(1) ;
      ex[0] = s[0] - s_star[0] ;

      e = vpColVector::stack(e,ex) ;
    }

    if (vpFeaturePoint3D::selectY() & select )
    {
      vpColVector ey(1) ;
      ey[0] = s[1] - s_star[1] ;
      e = vpColVector::stack(e,ey) ;
    }

    if (vpFeaturePoint3D::selectZ() & select )
    {
      vpColVector ez(1) ;
      ez[0] = s[2] - s_star[2] ;
      e = vpColVector::stack(e,ez) ;
    }
  }
  catch(...) {
    throw ;
  }

  return e ;

}
コード例 #12
0
ファイル: rgbdsensor.cpp プロジェクト: steppobeck/rgbd-calib
glm::mat4
RGBDSensor::guess_eye_d_to_world(const ChessboardSampling& cbs, const Checkerboard& cb){


  // find slowest ChessboardPose
  const double time         = cbs.searchSlowestTime(cbs.searchStartIR());
  bool valid_pose;
  glm::mat4 chessboard_pose = cb.pose_offset * cbs.interpolatePose(time,valid_pose);
  if(!valid_pose){
    std::cerr << "ERROR: could not interpolate valid pose" << std::endl;
    exit(0);
  }


  // find pose of that board in kinect camera space
  bool valid_ir;
  ChessboardViewIR cbvir(cbs.interpolateIR(time, valid_ir));
  if(!valid_ir){
    std::cerr << "ERROR: could not interpolate valid ChessboardIR" << std::endl;
    exit(0);
  }


  std::vector<glm::vec3> exs;
  std::vector<glm::vec3> eys;

  const float u = cbvir.corners[0].x;
  const float v = cbvir.corners[0].y;
  const float d = cbvir.corners[0].z;
  std::cerr << "calc origin at " << u << ", " << v << ", " << d << std::endl;
  glm::vec3 origin = calc_pos_d(u,v,d);

  for(unsigned i = 1; i < (CB_WIDTH * CB_HEIGHT); ++i){

    const float u = cbvir.corners[i].x;
    const float v = cbvir.corners[i].y;
    const float d = cbvir.corners[i].z;

    glm::vec3 corner = calc_pos_d(u,v,d);
    
    if(i < CB_WIDTH){
      eys.push_back(glm::normalize(corner - origin));
    }
    else if(i % CB_WIDTH == 0){
      exs.push_back(glm::normalize(corner - origin));
    }
  }

  glm::vec3 ex(calcMean(exs));
  glm::vec3 ey(calcMean(eys));
  
  ex = glm::normalize(ex);
  ey = glm::normalize(ey);
  
  glm::vec3 ez = glm::cross(ex,ey);
  ey           = glm::cross(ez,ex);

  std::cerr << "origin: " << origin << std::endl;
  std::cerr << "ex: " << ex << std::endl;
  std::cerr << "ey: " << ey << std::endl;
  std::cerr << "ez: " << ez << std::endl;

  glm::mat4 eye_d_to_world;
  eye_d_to_world[0][0] = ex[0];
  eye_d_to_world[0][1] = ex[1];
  eye_d_to_world[0][2] = ex[2];

  eye_d_to_world[1][0] = ey[0];
  eye_d_to_world[1][1] = ey[1];
  eye_d_to_world[1][2] = ey[2];

  eye_d_to_world[2][0] = ez[0];
  eye_d_to_world[2][1] = ez[1];
  eye_d_to_world[2][2] = ez[2];

  eye_d_to_world[3][0] = origin[0];
  eye_d_to_world[3][1] = origin[1];
  eye_d_to_world[3][2] = origin[2];

  eye_d_to_world = glm::inverse(eye_d_to_world);
  
  return chessboard_pose * eye_d_to_world;

}
コード例 #13
0
ファイル: magcal.cpp プロジェクト: wellrun/rise_sdvp
void MagCal::calcMagComp()
{
    /*
     * Inspired by
     * http://davidegironi.blogspot.it/2013/01/magnetometer-calibration-helper-01-for.html#.UriTqkMjulM
     *
     * Ellipsoid fit from:
     * http://www.mathworks.com/matlabcentral/fileexchange/24693-ellipsoid-fit
     *
     * To use Eigen to convert matlab code, have a look at Eigen/AsciiQuickReference.txt
     */

    if (mMagSamples.size() < 9) {
        QMessageBox::warning(this, "Magnetometer compensation",
                             "Too few points.");
        return;
    }

    int samples = mMagSamples.size();
    Eigen::VectorXd ex(samples);
    Eigen::VectorXd ey(samples);
    Eigen::VectorXd ez(samples);

    for (int i = 0;i < samples;i++) {
        ex(i) = mMagSamples.at(i).at(0);
        ey(i) = mMagSamples.at(i).at(1);
        ez(i) = mMagSamples.at(i).at(2);
    }

    Eigen::MatrixXd eD(samples, 9);

    for (int i = 0;i < samples;i++) {
        eD(i, 0) = ex(i) * ex(i);
        eD(i, 1) = ey(i) * ey(i);
        eD(i, 2) = ez(i) * ez(i);
        eD(i, 3) = 2.0 * ex(i) * ey(i);
        eD(i, 4) = 2.0 * ex(i) * ez(i);
        eD(i, 5) = 2.0 * ey(i) * ez(i);
        eD(i, 6) = 2.0 * ex(i);
        eD(i, 7) = 2.0 * ey(i);
        eD(i, 8) = 2.0 * ez(i);
    }

    Eigen::MatrixXd etmp1 = eD.transpose() * eD;
    Eigen::MatrixXd etmp2 = eD.transpose() * Eigen::MatrixXd::Ones(samples, 1);
    Eigen::VectorXd eV = etmp1.lu().solve(etmp2);

    Eigen::MatrixXd eA(4, 4);
    eA(0,0)=eV(0);   eA(0,1)=eV(3);   eA(0,2)=eV(4);   eA(0,3)=eV(6);
    eA(1,0)=eV(3);   eA(1,1)=eV(1);   eA(1,2)=eV(5);   eA(1,3)=eV(7);
    eA(2,0)=eV(4);   eA(2,1)=eV(5);   eA(2,2)=eV(2);   eA(2,3)=eV(8);
    eA(3,0)=eV(6);   eA(3,1)=eV(7);   eA(3,2)=eV(8);   eA(3,3)=-1.0;

    Eigen::MatrixXd eCenter = -eA.topLeftCorner(3, 3).lu().solve(eV.segment(6, 3));
    Eigen::MatrixXd eT = Eigen::MatrixXd::Identity(4, 4);
    eT(3, 0) = eCenter(0);
    eT(3, 1) = eCenter(1);
    eT(3, 2) = eCenter(2);

    Eigen::MatrixXd eR = eT * eA * eT.transpose();

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eEv(eR.topLeftCorner(3, 3) * (-1.0 / eR(3, 3)));
    Eigen::MatrixXd eVecs = eEv.eigenvectors();
    Eigen::MatrixXd eVals = eEv.eigenvalues();

    Eigen::MatrixXd eRadii(3, 1);
    eRadii(0) = sqrt(1.0 / eVals(0));
    eRadii(1) = sqrt(1.0 / eVals(1));
    eRadii(2) = sqrt(1.0 / eVals(2));

    Eigen::MatrixXd eScale = eRadii.asDiagonal().inverse() * eRadii.minCoeff();
    Eigen::MatrixXd eComp = eVecs * eScale * eVecs.transpose();

    mMagComp.resize(9);
    mMagComp[0] = eComp(0, 0);
    mMagComp[1] = eComp(0, 1);
    mMagComp[2] = eComp(0, 2);

    mMagComp[3] = eComp(1, 0);
    mMagComp[4] = eComp(1, 1);
    mMagComp[5] = eComp(1, 2);

    mMagComp[6] = eComp(2, 0);
    mMagComp[7] = eComp(2, 1);
    mMagComp[8] = eComp(2, 2);

    mMagCompCenter.resize(3);
    mMagCompCenter[0] = eCenter(0, 0);
    mMagCompCenter[1] = eCenter(1, 0);
    mMagCompCenter[2] = eCenter(2, 0);

    QVector<double> magX, magY, magZ;

    for (int i = 0;i < mMagSamples.size();i++) {
        double mx = mMagSamples.at(i).at(0);
        double my = mMagSamples.at(i).at(1);
        double mz = mMagSamples.at(i).at(2);

        mx -= mMagCompCenter.at(0);
        my -= mMagCompCenter.at(1);
        mz -= mMagCompCenter.at(2);

        magX.append(mx * mMagComp.at(0) + my * mMagComp.at(1) + mz * mMagComp.at(2));
        magY.append(mx * mMagComp.at(3) + my * mMagComp.at(4) + mz * mMagComp.at(5));
        magZ.append(mx * mMagComp.at(6) + my * mMagComp.at(7) + mz * mMagComp.at(8));
    }

    ui->magSampXyPlot->graph(1)->setData(magX, magY);
    ui->magSampXzPlot->graph(1)->setData(magX, magZ);
    ui->magSampYzPlot->graph(1)->setData(magY, magZ);

    updateMagPlots();
}
コード例 #14
0
ファイル: rgbdsensor.cpp プロジェクト: steppobeck/rgbd-calib
glm::mat4
RGBDSensor::guess_eye_d_to_world_static(const ChessboardSampling& cbs, const Checkerboard& cb){

  if(cbs.getIRs().empty() || cbs.getPoses().empty()){
    std::cerr << "ERROR in RGBDSensor::guess_eye_d_to_world_static: no Chessboard found" << std::endl;
    exit(0);
  }

  glm::mat4 chessboard_pose = cb.pose_offset * cbs.getPoses()[0].mat;
  ChessboardViewIR cbvir(cbs.getIRs()[0]);

  std::vector<glm::vec3> exs;
  std::vector<glm::vec3> eys;

  const float u = cbvir.corners[0].x;
  const float v = cbvir.corners[0].y;
  const float d = cbvir.corners[0].z;
  std::cerr << "calc origin at " << u << ", " << v << ", " << d << std::endl;
  glm::vec3 origin = calc_pos_d(u,v,d);

  for(unsigned i = 1; i < (CB_WIDTH * CB_HEIGHT); ++i){

    const float u = cbvir.corners[i].x;
    const float v = cbvir.corners[i].y;
    const float d = cbvir.corners[i].z;

    glm::vec3 corner = calc_pos_d(u,v,d);
    
    if(i < CB_WIDTH){
      eys.push_back(glm::normalize(corner - origin));
    }
    else if(i % CB_WIDTH == 0){
      exs.push_back(glm::normalize(corner - origin));
    }
  }

  glm::vec3 ex(calcMean(exs));
  glm::vec3 ey(calcMean(eys));
  
  ex = glm::normalize(ex);
  ey = glm::normalize(ey);
  
  glm::vec3 ez = glm::cross(ex,ey);
  ey           = glm::cross(ez,ex);

  std::cerr << "origin: " << origin << std::endl;
  std::cerr << "ex: " << ex << std::endl;
  std::cerr << "ey: " << ey << std::endl;
  std::cerr << "ez: " << ez << std::endl;

  glm::mat4 eye_d_to_world;
  eye_d_to_world[0][0] = ex[0];
  eye_d_to_world[0][1] = ex[1];
  eye_d_to_world[0][2] = ex[2];

  eye_d_to_world[1][0] = ey[0];
  eye_d_to_world[1][1] = ey[1];
  eye_d_to_world[1][2] = ey[2];

  eye_d_to_world[2][0] = ez[0];
  eye_d_to_world[2][1] = ez[1];
  eye_d_to_world[2][2] = ez[2];

  eye_d_to_world[3][0] = origin[0];
  eye_d_to_world[3][1] = origin[1];
  eye_d_to_world[3][2] = origin[2];

  eye_d_to_world = glm::inverse(eye_d_to_world);
  
  return chessboard_pose * eye_d_to_world;

}
コード例 #15
0
void LCGWSpinBBHNR1::ConvertSpinDirToSSB(int iSpin)
{
	double thS, phS, th, ph;
	double iota, stheta, nez;
	double thBS, phBS;
	double phi0(0.);
	LCVector ex(MT), ey(MT), spin(MT), nW(MT), ez(MT);
	
	
	if(iSpin==1){
		thS = thS1;
		phS = phS1;
	}
	if(iSpin==2){
		thS = thS2;
		phS = phS2;
	}
	
	th = M_PI/2. - Beta;
	ph = Lambda;
	
	
	//! **** Compute \f$ \theta_L \f$ and \f$ \phi_L \f$ :
	//! ** Declaration of coordinates of ^k, ^theta, ^phi and ^Ln in SSB frame and cos / sin of some angles
	double x_k, y_k, z_k;
	double x_th, y_th, z_th;
	double x_ph, y_ph, z_ph;
	// double LnBx, LnBy, LnBz;
	double sPhS, cPhS, sPsi, cPsi, sThd, cThd; 
	
	sPhS = sin(phiS);
	cPhS = cos(phiS);
	sPsi = sin(2.*M_PI-NRPolarization);
	cPsi = cos(2.*M_PI-NRPolarization);
	sThd = sin(Thd);
	cThd = cos(Thd);
	
	x_k = - sThS * cPhS;
	y_k = - sThS * sPhS;
	z_k = - cThS;
	
	x_th = cThS * cPhS;
	y_th = cThS * sPhS;
	z_th = - sThS;
    
	x_ph = -y_k * z_th + z_k * y_th;
	y_ph = -z_k * x_th + x_k * z_th;
	z_ph = -x_k * y_th + y_k * x_th;
    
	
    
	
	
	//! ** Following convention of Sofiane for the angle psi (^p = cos psi ^theta + sin psi ^phi) :
	//	 LnBx = (- sThd * sPsi * x_th + sThd * cPsi * x_ph + cThd * x_k  );
	//	 LnBy = (- sThd * sPsi * y_th + sThd * cPsi * y_ph + cThd * y_k  );
	//	 LnBz = (- sThd * sPsi * z_th + sThd * cPsi * z_ph + cThd * z_k  );
	
	
	/*  
	 // new
	 
	 LnBx = (- sThd * sPsi * x_th - sThd * cPsi * x_ph + cThd * x_k  );
	 LnBy = (- sThd * sPsi * y_th - sThd * cPsi * y_ph + cThd * y_k  );
	 LnBz = (- sThd * sPsi * z_th - sThd * cPsi * z_ph + cThd * z_k  );
	 */
    
    /*
	 //! ** Following convention of Antoine for the angle psi (^p = cos psi ^theta - sin psi ^phi) :
	 LnBx = -( sThd * cPsi * x_th - sThd * sPsi * x_ph - cThd * x_k  );
	 LnBy = -( sThd * cPsi * y_th - sThd * sPsi * y_ph - cThd * y_k  );
	 LnBz = -( sThd * cPsi * z_th - sThd * sPsi * z_ph - cThd * z_k  );   
	 */
  	
	// with all angles
	
	
    LnB.x( -LnN.y()*cPsi*x_th+LnN.y()*sPsi*x_ph+LnN.x()*cThd*sPsi*x_th+LnN.x()*cThd*cPsi*x_ph-LnN.z()*sThd*sPsi*x_th-LnN.z()*sThd*cPsi*x_ph+x_k*LnN.x()*sThd+x_k*LnN.z()*cThd );
    LnB.y( -LnN.y()*cPsi*y_th+LnN.y()*sPsi*y_ph+LnN.x()*cThd*sPsi*y_th+LnN.x()*cThd*cPsi*y_ph-LnN.z()*sThd*sPsi*y_th-LnN.z()*sThd*cPsi*y_ph+y_k*LnN.x()*sThd+y_k*LnN.z()*cThd );
    LnB.z( -LnN.y()*cPsi*z_th+LnN.y()*sPsi*z_ph+LnN.x()*cThd*sPsi*z_th+LnN.x()*cThd*cPsi*z_ph-LnN.z()*sThd*sPsi*z_th-LnN.z()*sThd*cPsi*z_ph+z_k*LnN.x()*sThd+z_k*LnN.z()*cThd );
    
    
    double n_LnB = LnB.norm();
    
	//	thBL =   atan2(sqrt(LnBx*LnBx + LnBy*LnBy),LnBz);  // acos(LnBz/n_LnB); corrected by Sofiane
    thBL =  acos(LnB.z()/n_LnB);
    
	//    if(thBL<0.)
	//        thBL +-M_PI;
    
	//	phBL =M_PI*(1-LnBy/fabs(LnBy)) + LnBy/fabs(LnBy) * acos(LnBx / n_LnB / sqrt(1. - LnBz*LnBz / (n_LnB*n_LnB)) );  //  atan2(LnBy,LnBx); corrected by Sofiane
	
    phBL = LnB.ph();
    
    
    if(phBL<0.)
  		phBL += 2.*M_PI;
	
	//  double up = cThS*sin(thBL)*cos(phiS-phBL) - cos(thBL)*sThS;   /: commented by Sofiane
	//  double down = sin(thBL)*sin(phiS - phBL);    // commented by Sofiane
    
    ConvertLS1S2dirNR2SSBSofVer(Phd, thetaS, phiS, thetaJ, phiJ, LnN, S1N, S2N, AmpL, AmpS1, AmpS2, LnB, S1B, S2B);  // see how to use it in more elegant way
    
    
    double up =  cos(Phd - phiS);    // introduced by Sofiane
    double down = cos(thetaS)*sin(Phd - phiS);  // introduced by Sofiane
    
    PSIN =atan2(up,down);
    
    if(PSIN<0.)  
        PSIN +=2*M_PI;
	
	
	iota = acos(sin(th)*sin(thBL)*cos(ph - phBL) + cos(th)*cos(thBL)); 
	
    
	stheta = fabs(sin(iota));
	if (iota == 0.0)
		Cout << "WARNING in LCGWSpinBBHNR1::ConvertSpinDirToSSB : degenerate case need to consider separately: L colinear with n !" << Endl;
	nW.p[0] = sin(th)*cos(ph) ;
	nW.p[1] = sin(th)*sin(ph) ;
	nW.p[2] = cos(th) ;
	ez.p[0] = sin(thBL)*cos(phBL) ;  
	ez.p[1] = sin(thBL)*sin(phBL) ;
	ez.p[2] = cos(thBL) ;
	ey.p[0] = (nW.p[1]*ez.p[2] - nW.p[2]*ez.p[1])/stheta ;
	ey.p[1] = (nW.p[2]*ez.p[0] - nW.p[0]*ez.p[2])/stheta ;
	ey.p[2] = (nW.p[0]*ez.p[1] - nW.p[1]*ez.p[0])/stheta ;
	
	nez = nW.p[0]*ez.p[0] + nW.p[1]*ez.p[1] + nW.p[2]*ez.p[2] ;
	ex.p[0] = (ez.p[0]*nez - nW.p[0])/stheta ;
	ex.p[1] = (ez.p[1]*nez - nW.p[1])/stheta ;
	ex.p[2] = (ez.p[2]*nez - nW.p[2])/stheta ;
	
	//! ** if it is eccentric orbit, we need to rotate ex, ey by -phi0
	
	for(int i=0; i<3; i++)
		spin.p[i] = sin(thS)*cos(phS)*( cos(phi0)*ex.p[i] - sin(phi0)*ey.p[i] ) + sin(thS)*sin(phS)*( sin(phi0)*ex.p[i] + cos(phi0)*ey.p[i] ) + cos(thS)*ez.p[i] ;
	
	thBS = acos(spin.p[2]);
	phBS = atan2(spin.p[1], spin.p[0]);
	if (phBS < 0.)
		phBS = phBS + 2.*M_PI;
	
	if(iSpin==1){
		thBS1 = thBS;
		phBS1 = phBS;
	}
	if(iSpin==2){
		thBS2 = thBS;
		phBS2 = phBS;
	}	
	
	S1B.x( sin(thBS1)*cos(phBS1) );
	S1B.y( sin(thBS1)*sin(phBS1) );
	S1B.z( cos(thBS1) );
	
	S2B.x( sin(thBS2)*cos(phBS2) );
	S2B.y( sin(thBS2)*sin(phBS2) );
	S2B.z( cos(thBS2) );
	
	
}
コード例 #16
0
   void QuatOpsTest::testQuatDiv()
   {
      // test matrix div version against quat div version, 
      // make sure they all work the same.
      // note: this test depends on the xforms functions working
      {
         const float eps = 0.0001f;
         gmtl::Matrix44f q3( gmtl::makeRot<gmtl::Matrix44f>( gmtl::AxisAnglef( gmtl::Math::deg2Rad( 90.0f ), 0,1,0 ) ) ), 
                           q4( gmtl::makeRot<gmtl::Matrix44f>( gmtl::AxisAnglef( gmtl::Math::deg2Rad( 90.0f ), 0,0,1 ) ) ), q6;
         gmtl::Vec3f sx( 6,0,0 ), sy( 0,4,0 ), sz( 0,0,9 ), ex( 0,0,6 ), ey( -4,0,0 ), ez( 0,-9,0 ), tx, ty, tz;

         gmtl::invert( q3 ); // there is no matrix div, so do this to simulate it.

         // make sure the mult() func works
         gmtl::mult( q6, q4, q3 );
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );

         // make sure the operator* works too
         q6 = q4 * q3;
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );

         // make sure the operator*= works too
         q6 = q4;
         q6 *= q3;
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );
      }
      {
         const float eps = 0.0001f;
         gmtl::Quat<float> q3( gmtl::makeRot<gmtl::Quatf>( gmtl::AxisAnglef( gmtl::Math::deg2Rad( 90.0f ), 0,1,0 ) ) ), 
                           q4( gmtl::makeRot<gmtl::Quatf>( gmtl::AxisAnglef( gmtl::Math::deg2Rad( 90.0f ), 0,0,1 ) ) ), q5, q6;
         gmtl::Vec3f sx( 6,0,0 ), sy( 0,4,0 ), sz( 0,0,9 ), ex( 0,0,6 ), ey( -4,0,0 ), ez( 0,-9,0 ), tx, ty, tz;

         // make sure the mult() func works
         gmtl::div( q6, q4, q3 ); 
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );

         // make sure the operator* works too
         q6 = q4 / q3; 
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );

         // make sure the operator*= works too
         q6 = q4;
         q6 /= q3;
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );
      }
   }
コード例 #17
0
void Filter::Kalman(Joint joint, double &dx, double &dy)
{
	Kalmans[Kalman_count++] = joint;
	Kalman_count = Kalman_count % Kalman_limit;
	Kalman_num++;
	if (Kalman_num > Kalman_limit)
	{
		Kalman_num = Kalman_limit;
	}
	if (Kalman_num < Kalman_limit)
	{
		dx = joint.Position.X;
		dy = joint.Position.Y;
		return;
	}
	else
	{
		//X, Y
		int haha;
		haha = 1;
		double x[5], y[5];
		int pos = Kalman_count;
		for (int i = 0; i < 5; i++)
		{
			x[i] = Kalmans[pos].Position.X;
			y[i] = Kalmans[pos].Position.Y;
			pos++;
			pos = pos%Kalman_limit;
		}
		//求系数Ax, Ay
		double Ax[5] = {
			/*a0*/ x[0],
			/*a1*/ 4 * (x[1] - x[0]) - 3 * x[2] + 4 * x[3] / 3 - x[4] / 4,
			/*a2*/ 11 * x[4] / 24 - 7 * x[3] / 3 + 19 * x[2] / 4 - 13 * (x[1] - x[0]) / 3,
			/*a3*/ x[4] / 3 - 7 * x[3] / 6 + x[2] - (x[1] - x[0]) / 2,
			/*a4*/ (x[4] - 4 * x[3] + 6 * x[2] + 4 * (x[1] - x[0])) / 24
		};
		double Ay[5] = {
			/*a0*/ y[0],
			/*a1*/ 4 * (y[1] - y[0]) - 3 * y[2] + 4 * y[3] / 3 - y[4] / 4,
			/*a2*/ 11 * y[4] / 24 - 7 * y[3] / 3 + 19 * y[2] / 4 - 13 * (y[1] - y[0]) / 3,
			/*a3*/ y[4] / 3 - 7 * y[3] / 6 + y[2] - (y[1] - y[0]) / 2,
			/*a4*/ (y[4] - 4 * y[3] + 6 * y[2] + 4 * (y[1] - y[0])) / 24
		};

		//求转换矩阵Fx, Fy
		Matrix Fx(4, 4,
							 new double[16]{
			  1, 1, -0.5, (Ax[1] + 6 * Ax[3] - 4 * Ax[4]) / (24 * Ax[4]),
				0, 1, 1, 0.5,
				0, 0, 1, 1,
				0, 0, 0, 1});
		Matrix Fy(4, 4,
							 new double[16]{
			1, 1, -0.5, (Ay[1] + 6 * Ay[3] - 4 * Ay[4]) / (24 * Ay[4]),
				0, 1, 1, 0.5,
				0, 0, 1, 1,
				0, 0, 0, 1});
		//求ε(t|t-1)
		Matrix ex(4, 4), ey(4, 4);
		ex = Fx*Kalman_ex*(!Fx);
		ey = Fy*Kalman_ey*(!Fy);
		//cout << "ex" << endl; ex.print();
		//cout << "ey" << endl; ey.print();

		Matrix Bx(4, 1), By(4, 1);
		//cout << "!Kalman_C" << endl; (!Kalman_C).print();
		//cout << "Kalman_vx" << endl; Kalman_vx.print();
		//cout << "Kalman_C" << endl; Kalman_C.print();
		//cout << "!Kalman_C" << endl; (!Kalman_C).print();
		//cout << "Kalman_C*ex" << endl; (Kalman_C*ex).print();
		//cout << "Kalman_C*ex*(!Kalman_C)" << endl; (Kalman_C*ex*(!Kalman_C)).print();
		//cout << "(~(Kalman_vx + Kalman_C*ex*(!Kalman_C)))" << endl;
		//(~(Kalman_vx + Kalman_C*ex*(!Kalman_C))).print();
		Bx = ex*(!Kalman_C)*(~(Kalman_vx + Kalman_C*ex*(!Kalman_C)));
		//cout << "Bx" << endl; Bx.print();
		By = ey*(!Kalman_C)*(~(Kalman_vy + Kalman_C*ey*(!Kalman_C)));
		
		Matrix I4(4, 4);
		I4.SetIdentity();
		Kalman_Sx = (I4 - Bx*Kalman_C)*(Fx*Kalman_Sx + Kalman_Gx) +
			Bx*Matrix(1, 1, new double[1] {joint.Position.X});
		//cout << "Kalman_Sx" << endl; Kalman_Sx.print();
		Kalman_Sy = (I4 - By*Kalman_C)*(Fy*Kalman_Sy + Kalman_Gy) +
			By*Matrix(1, 1, new double[1] {joint.Position.Y});

		Kalman_ex = ex - Bx*Kalman_C*ex;
		Kalman_ey = ey - By*Kalman_C*ey;

		dx = Kalman_Sx.at(0, 0);
		dy = Kalman_Sy.at(0, 0);
	}
}