시스템개발툴킷

제품화 분야 > 시스템개발툴킷 > XDL TIP

시스템개발툴킷

[NXDL] 타겟지점, 센서의 고도, 센서자세를 이용하여 센서의 위치 추정

Author
pixoneer
Date
2016-11-19 13:35
Views
4992
              //  타겟지점,  센서의 고도, 센서자세를 이용하여 역으로 센서의 위치 추정한다.

                // 1. 타겟 지점에 대한 XGeoPoint를 생성하고 이로부터 ECEF값을 얻는다.
                XVertex3d vTarget;
                XGeoPoint geoTarget = XGeoPoint.FromDegree(127.059026, 36.070813, 24);
                vTarget = geoTarget.ecr;

                // 2. 센서의 yaw, pitch, roll값을 설정한다.
                XAngle yaw, pitch, roll;            
                yaw = XAngle.FromDegree(70.57);
                pitch = XAngle.FromDegree(61.20);
                roll = XAngle.FromDegree(0);

                // 3. 타겟이 있는 지점에 대한 ECEF좌표계상의 NED(North, East, Down)벡터를 생성한다.
                XVertex3d vN, vE, vD;
                vN = new XVertex3d();
                vE = new XVertex3d();
                vD = new XVertex3d();
                Xfn.GetFrameNED(vTarget, ref vN, ref vE, ref vD);

                // 4. NED  벡터와 센서의 자세 정보를 입력하여 ECEF공간상에서 센서의 지향 벡터를 얻는다.
                XVertex3d vS = Xfn.GetSensorPointingVector(vE, vN, vD, yaw, pitch, roll);
                // 5. 센서의 고도값을 이용하여 센서까지의 Slant거리값을 계산. 
                double height = 14882;      
                double slant = height / Xfn.DotProduct(vS, vD);

               // 6. 센서의 위치 추정
                XVertex3d vPos = new XVertex3d();  
                vPos = vTarget - slant * vS;

               // 7. ECEF 좌표를 위경도 좌표로 변환
                XGeoPoint vSource = XGeoPoint.FromEcr(ref vPos);
                Console.WriteLine("{0} {1} {2}", vSource.lond.ToString(), vSource.latd.ToString(), vSource.hgt.ToString());
                // 35.982355, 126.75321, 14882
Total Reply 0Count
« [NXDL] ECEF좌표계로부터 Lat/Lon(위경도)좌표계로 변환하기
[NCC] XSpatialReference클래스에 좌표 시스템 설정하기 »
List