Welcome to Keen Software House Forums! Log in or Sign up to interact with the KSH community.
  1. You are currently browsing our forum as a guest. Create your own forum account to access all forum functionality.

Weird Vector Bug

Discussion in 'Programming Questions and Suggestions' started by aeshalo, Jan 24, 2017.

Thread Status:
This last post in this thread was made more than 31 days old.
  1. aeshalo Trainee Engineer

    Messages:
    2
    Hi, I've been coding a missile autopilot and have run into a very strange bug in my code. I've tried to fix it on my own by i just can't figure out where the problem is.

    It helps to have an understanding of what my code does as the bug is so strange...

    My code works by first taking a unit vector of the direction from the missile to it's target (in the missile's frame of reference).
    Code:
    Vector3D LocalTgtDir = Vector3D.Normalize(Vector3D.Transform( EstPos , (MatrixD.Invert(myTransform))));
    where EstPos (Vector3D) is the position of the target in world space and myTransform (Matrix D) is my camera's WorldMatrix.
    It also calculates an intercept direction, so if the target is moving it calculates lead and the correct direction to intercept.
    Code:
    Vector3D LocalIntDir = Vector3D.Multiply( (Vector3D.Transform( EstPos , (MatrixD.Invert(myTransform))) - Vector3D.Multiply(info.Velocity,intTime)),(1/(estVel*intTime)));
    where intTime is the time to intercept (double) and estVel is the estiated missile velocity (set as 100(double))
    This then feeds into:
    Code:
    double yaw = Math.Sign(LocalTgtDir.X)*Math.Acos(Vector2D.Dot(new Vector2D(0,-1), Vector2D.Normalize(new Vector2D( LocalTgtDir.X, LocalTgtDir.Z )) ) );
    which calculates the yaw angle between the missile's forward direction (which is 0,-1 in the camera's reference frame) and the target direction.
    Then i use this angle as the input to a PI controller that set's the missile's gyro override.
    The bug makes itself visible in the 'integralYaw' term, which is defined like this:
    Code:
    integralYaw = (integralYaw + (yaw*10*Runtime.TimeSinceLastRun.TotalSeconds));
    this, obviously sums the current yaw value with all the previous ones.

    Now. The Bug. The bug is, that when i use LocalTgtDir to calculate yaw, integralYaw behaves normally, yet when i use LocalIntDir, integalYaw always shows NaN. This is insane, as i've tested it with a stationary target where LocalTgtDir and LocalIntDir have been IDENTICAL to 5d.p. yet it still shows NAN for one and works fine for the other. Also, yaw always shows the correct value no matter which vector i give it, but somehow when passing the EXACT SAME value of yaw in, one time it works normally (with LocalTgtDir) and one time i get NaN (with LocalIntDir).
    How the hell does the same value, yaw, which should just be a double work sometimes but not work other times?
    Furthermore, when i set integralYaw to:
    Code:
    integralYaw = (yaw*10*Runtime.TimeSinceLastRun.TotalSeconds);
    i.e. i remove the summing with previous values bit, it works normally with both vectors!!

    So yeah. I am very confused. Any help would be appreciated.
    I've included a screenshot of an LCD screen i use for debugging where you can see the weirdness.
    For this screenshot: yaw uses LocalIntDir while pitch uses LocalTgtDir.

    as you can see LocalTgtDir (shown as TGT DirVect) and LocalIntDir (shown as Local Intercept Vector) are Identical
    and both yaw and pitch are real numbers (not broken) YET the yaw controller shows NaN whereas the pitch controller shows the proper numbers.


    Full Code included for completeness:
    Code:
    private bool firstRun = true;
    //private bool inTube = false;
    private int lastScan = 0;
    private MyDetectedEntityInfo info;
    private IMyThrust thruster;
    private IMyTextPanel screen;
    private IMyCameraBlock camera;
    private IMyTimerBlock timer;
    private List<IMyTerminalBlock> gyros = new List<IMyTerminalBlock>();
    private int i = 0;
    private int ticks = 0;
    private Vector3D myPosition;
    private MatrixD myTransform;
    private double integralPitch = 0;
    private double integralYaw = 0;
    private double propGain = 1;
    private double intGain = 1;
    private double estVel = 100;
    public void Main(string argument) {
    
        if (firstRun == true) { //find blocks
            IMyBlockGroup group = GridTerminalSystem.GetBlockGroupWithName ("Gyros") as IMyBlockGroup;
            group.GetBlocks(gyros);
            thruster = GridTerminalSystem.GetBlockWithName("LrgT") as IMyThrust;
            screen = GridTerminalSystem.GetBlockWithName("Screen") as IMyTextPanel;
            camera = GridTerminalSystem.GetBlockWithName("Camera 3") as IMyCameraBlock;
            timer = GridTerminalSystem.GetBlockWithName("Timer Block") as IMyTimerBlock;    
          
            foreach (IMyGyro gyro in gyros) { //set gyro overrides
                if (gyro.GyroOverride == false) {
                    gyro.GetActionWithName("Override").Apply(gyro);
                    }
                }
            camera.EnableRaycast = true;
            //thruster.SetValueFloat("Override",100f); //fire the forward booster (disabled for test)
            firstRun = false;
            }
          
        if (ticks < 10) {//only activate every 10 ticks (i use a single tick timer clock)
            ticks++;
            timer.GetActionWithName("TriggerNow").Apply(timer);
            return;
            }
        ticks = 0;
        lastScan++;					//this deals with the fact that raycasts might take more than one tick to charge
        myPosition = camera.GetPosition();
        myTransform = camera.WorldMatrix;
        Vector3D myForDir = (Vector3D.Transform(new Vector3D(0,0,-1),myTransform) - myPosition );
        Vector3D DistVect = new Vector3D(0,0,0);
        Vector3D EstPos = (info.Position +
            (Vector3D.Multiply(info.Velocity, lastScan*10*Runtime.TimeSinceLastRun.TotalSeconds )));
        if (((int)info.Type == 3) || ((int)info.Type == 2)) {
            if (camera.CanScan(1.02*(EstPos - myPosition).Length())) {
                info = camera.Raycast(EstPos*1.02);
                lastScan = 0;
                EstPos = (info.Position);
                }
            DistVect = (EstPos - myPosition);
            }
        else {
            info = camera.Raycast(10000,0,0);
            lastScan = 0;
        }
    
      
      
        double intTime = 	//this is some complex math for calculating the right intercept time
                ( 		//i had some issues with there being so many brackets...
                    (
                        (-2*info.Velocity.Dot(DistVect)) -
                        Math.Sqrt(
                            Math.Pow(2*info.Velocity.Dot(DistVect),2)
                            - 4*(info.Velocity.Dot(info.Velocity)-Math.Pow(estVel,2))*DistVect.Dot(DistVect)
                        ) 
                    )
                /  ( 
                        2*(info.Velocity.Dot(info.Velocity)-Math.Pow(estVel,2)) //denominator item
                    )
                );
    		//here we go:
        Vector3D LocalIntDir = Vector3D.Multiply( (Vector3D.Transform( EstPos , (MatrixD.Invert(myTransform)))
        - Vector3D.Multiply(info.Velocity,intTime)),(1/(estVel*intTime)));
        Vector3D LocalTgtDir = Vector3D.Normalize(Vector3D.Transform( EstPos , (MatrixD.Invert(myTransform))));
        		//here are the angles
        double angle = Math.Acos((new Vector3D(0,0,-1).Dot(LocalIntDir))); //total angle
        double yaw = Math.Sign(LocalIntDir.X)*Math.Acos(Vector2D.Dot(new Vector2D(0,-1), 
            Vector2D.Normalize(new Vector2D( LocalIntDir.X, LocalIntDir.Z )) ) );
        double pitch = Math.Sign(LocalTgtDir.Y)*Math.Acos(Vector3D.Dot(
            Vector3D.Normalize(new Vector3D( (float)LocalTgtDir.X, 0, (float)LocalTgtDir.Z )) ,
            LocalTgtDir) );
    
        if((int)info.Type == 0) { //if there's no target there's no offset
            LocalTgtDir = new Vector3D(0,0,0);
            LocalIntDir = new Vector3D(0,0,0);
            angle = 0;                                                                                                                                                                
            yaw = 0;
            pitch = 0;
            }
        i++;
    
    
        double yawSignal;
        double pitchSignal;
        if(((int)info.Type == 3) || ((int)info.Type == 2)) {  //got a target? if so: activate PI controller
          
            integralYaw = (integralYaw + (yaw*10*Runtime.TimeSinceLastRun.TotalSeconds));
            if (Math.Abs(integralYaw) > 5) {integralYaw = Math.Sign(integralYaw)*(double)5;}
            yawSignal = (propGain*yaw + intGain*integralYaw);
            integralPitch += (pitch*10*Runtime.TimeSinceLastRun.TotalSeconds);
            //if (Math.Abs(integralPitch) > 5) {integralPitch = Math.Sign(integralPitch)*5;}
            pitchSignal = -(propGain*pitch + intGain*integralPitch);
            foreach (IMyGyro gyro in gyros) {
                gyro.SetValueFloat("Yaw",(float)yawSignal);
                gyro.SetValueFloat("Pitch",(float)pitchSignal);
            }
        }    
        else {   //otherwise just set it to 0
            integralYaw = 0;
            integralPitch = 0;
            yawSignal = 0;
            pitchSignal = 0;
          
        }
    
    	//write a whole bunch of stuff to the debug LCD
    
        screen.WritePublicText("Name: " + info.Name + "\n");
        screen.WritePublicText("Iter Since Last Scan: " + lastScan + "\n", true);
        screen.WritePublicText("Type: " + info.Type + "\n", true);
        screen.WritePublicText("Tgt Position: " + EstPos.ToString("0.0") + "\n", true);
        screen.WritePublicText("Velocity: " + info.Velocity.ToString("0.0") + "\n", true);
        screen.WritePublicText("Distance(Vect,Abs): " +
            RoundVector(DistVect,3) + ", " + Math.Round(DistVect.Length(),4) + "\n", true);
        screen.WritePublicText("Error Angle (Glbl / Yaw / Pitch): " + Math.Round((angle*180/Math.PI),2) + " / " +
            Math.Round((yaw*180/Math.PI),2) + " / " + Math.Round((pitch*180/Math.PI),2) + "\n", true);
        screen.WritePublicText("Relationship: " + info.Relationship + "\n", true);
        screen.WritePublicText("Missile Position: " + RoundVector(myPosition,3) + "\n", true);
        screen.WritePublicText("Missile Forward in Ws: " + RoundVector(myForDir,3) + "\n", true);
        screen.WritePublicText("TGT Pos in LclRF: " +  RoundVector(
            Vector3D.Multiply(LocalTgtDir, DistVect.Length()),3)  + "\n", true);
        screen.WritePublicText("TGT DirVect in LclRF: " + LocalTgtDir + "\n", true);
        screen.WritePublicText("Local Intercept Vector: "  +
            LocalIntDir + "  /  " +  Math.Round(LocalIntDir.Length(),3) + "\n", true);
        screen.WritePublicText("Time to intercept at V = " + estVel + ":" + Math.Round(intTime,3) + "\n", true);
        screen.WritePublicText("Time direct to T = "  + DistVect.Length()/estVel + "\n", true);
        screen.WritePublicText("Iteration: " + i + "\n", true);
      
        screen.WritePublicText("Yaw P/I/T: " + Math.Round(propGain*yaw, 3) + " / " +
            Math.Round(integralYaw,3) + " / " + Math.Round(yawSignal,3) + "\n", true);
        screen.WritePublicText("DebugYaw: " + yaw*10*Runtime.TimeSinceLastRun.TotalSeconds + "\n", true);
          screen.WritePublicText("Pitch P/I/T: " + Math.Round(propGain*pitch , 3) + " / " +
            Math.Round(integralPitch , 3) + " / " + Math.Round(pitchSignal , 3) + "\n", true);
    screen.ShowPublicTextOnScreen();
    
        timer.GetActionWithName("TriggerNow").Apply(timer);
    
    
    }
    
    public Vector3D RoundVector(Vector3D input, int places) {  // a useful function for displaying things
    var output = new Vector3D();
    output.X = Math.Round(input.X,places);
    output.Y = Math.Round(input.Y,places);
    output.Z = Math.Round(input.Z,places);
    return output;
    }
    
    public MatrixD RoundMatrix(MatrixD input, int places) {  //ditto
    var output = new MatrixD();
    output.M11 = Math.Round(input.M11,places);
    output.M12 = Math.Round(input.M12,places);
    output.M13 = Math.Round(input.M13,places);
    output.M21 = Math.Round(input.M21,places);
    output.M22 = Math.Round(input.M22,places);
    output.M23 = Math.Round(input.M23,places);
    output.M31 = Math.Round(input.M31,places);
    output.M32 = Math.Round(input.M32,places);
    output.M33 = Math.Round(input.M33,places);
    return output;
    }
     
  2. JoeTheDestroyer Junior Engineer

    Messages:
    573
    NaN is infectious, it's required to propagate through expressions. In particular NaN plus anything is always NaN, so when you're doing a running sum, if even a single input value is NaN, the whole result will end up that way.

    Removing the sum looks ok because that single NaN value probably flashes by too fast for you to catch it.

    I see two likely culprits. The first is Acos(). If the result of your dot product is even slightly outside of [-1,1] (due to numerical precision issues, for example) then it will return NaN. This is one of several reasons not to use Acos().

    Either clip the values to the [-1,1] range or (better) use Atan2 instead.

    Also note that the Normalize() set of functions aren't completely safe, they don't handle zero-length vectors properly.

    The culprit might also be here, can intTime ever be 0? Say when you first initialize tracking?


    ---

    As a side note, calculating yaw/pitch is not really the right thing to be doing here. The SE UI lies when it calls those 3 values on gyros "Yaw", "Pitch" and "Roll". More discussion here.
     
  3. aeshalo Trainee Engineer

    Messages:
    2
    ah! that worked thanks. i think the first value that was calculated before the target was identified so the first value into intergalYaw was an NaN. i fixed it be re-arranging my code a bit so that wouldn't happen.

    As for calculating yaw/pitch it works so i'm not going to question it too much. i'm sure there's a more elegant method involving rotation matrices but i'm unfamiliar with it.
     
Thread Status:
This last post in this thread was made more than 31 days old.