コード例 #1
0
        public CalBuffer Calculation()
        {
            CalBuffer result = new CalBuffer();
            double    obj    = 0;
            double    talpha = Alpha / MagData.Count;

            for (var i = 0; i < Times; i++)
            {
                obj = 0;
                //Debug.WriteLine("Position Before: (" + OwnPosition[0] + ", " + OwnPosition[1] + ", " + OwnPosition[2] + ")");
                for (var j = 0; j < MagData.Count; j++)
                {
                    CalBuffer buffer = PartCalculation(OwnPosition, MagData[j]);
                    obj += buffer.Cost;
                    for (var k = 0; k < 3; k++)
                    {
                        //      Debug.WriteLine("Test: " + OwnPosition[k]);
                        OwnPosition[k] -= talpha * buffer.Pos[k];
                        //      Debug.WriteLine("Test: " + OwnPosition[k]);
                    }
                }
                //Debug.WriteLine("Position now: (" + OwnPosition[0] + ", " + OwnPosition[1] + ", " + OwnPosition[2] + ")");
                //Debug.WriteLine("Obj now: " + obj);
            }
            result.Cost = obj;
            result.Pos  = OwnPosition;
            return(result);
        }
コード例 #2
0
        public void GetPosition()
        {
            //PositionManager.OwnPosition = Position;
            //PositionManager.MagData = MeasureData;
            // May have problem here
            PositionManager = new PositionCal(Position, MeasureData);
            CalBuffer buffer = PositionManager.Calculation();

            Position = buffer.Pos;
            OnPropertyChanged("Position");
        }
コード例 #3
0
        public CalBuffer PartCalculation(double[] cal, CalUnit magInfo)
        {
            CalBuffer result = new CalBuffer();

            double[] dif = new double[3];
            double   a1, a2;

            for (var i = 0; i < 3; i++)
            {
                dif[i] = cal[i] - magInfo.Position[i];
            }

            a1          = Math.Pow(dif[0], 2) / 16 + Math.Pow(dif[1], 2) / 16 + Math.Pow(dif[1], 2) / 25;
            a2          = Co * Math.Pow(a1, -1.5) - magInfo.Signal;
            result.Cost = Math.Pow(a2, 2);
            double gradCo = -3 * Co * Math.Pow(a1, -2.5) * a2;

            double[] grad = new double[3];
            grad[0]    = gradCo * dif[0] / 8;
            grad[1]    = gradCo * dif[1] / 8;
            grad[2]    = gradCo * dif[2] * 2 / 25;
            result.Pos = grad;
            return(result);
        }