Disabled ThunderSVM loading and added better interpolation.

Examples/OpenSeeVRMDriver.cs

@@ -39,6 +39,9 @@ public class OpenSeeVRMDriver : MonoBehaviour {
     public float blinkSmoothing = 0.75f;
     [Tooltip("With tracked eye blinking, the eyes will close when looking down. Enable this to compensate this by modifying thresholds.")]
     public bool lookDownCompensation = false;
+    [Tooltip("This is an angle value that can be used to adjust the angle from which a face is considered to be looking downwards.")]
+    [Range(-10, 20)]
+    public float lookDownAdjustment = 0f;
     [Tooltip("When enabled, auto blinking will be enabled while looking down.")]
     public bool autoBlinkLookingDown = false;
     [Tooltip("When enabled, the blink state for both eyes is permanently linked together.")]
@@ -161,21 +164,20 @@ public class OpenSeeVRMDriver : MonoBehaviour {
     private float turnLeftBoundaryAngle = -30f;
     private float turnRightBoundaryAngle = 20f;
     private float turnDownBoundaryAngle = 205f;
+    private float wasLookingDown = 0f;
     
     private double lastGaze = 0f;
     private float lastLookUpDown = 0f;
     private float lastLookLeftRight = 0f;
     private float currentLookUpDown = 0f;
     private float currentLookLeftRight = 0f;
-    private int interpolationCount = 0;
+    private TimeInterpolate gazeInterpolate;
-    private int interpolationState = 0;
     
     private float[] lastVisemeValues;
     private double lastMouth = 0f;
     private float[] lastMouthStates;
     private float[] currentMouthStates;
-    private int mouthInterpolationCount = 0;
+    private TimeInterpolate mouthInterpolate;
-    private int mouthInterpolationState = 0;
     
     private double startedSilVisemes = -10;
     private double silVisemeHybridThreshold = 0.3;
@@ -186,8 +188,7 @@ public class OpenSeeVRMDriver : MonoBehaviour {
     private float lastBlinkRight = 0f;
     private float currentBlinkLeft = 0f;
     private float currentBlinkRight = 0f;
-    private int blinkInterpolationCount = 0;
+    private TimeInterpolate blinkInterpolate;
-    private int blinkInterpolationState = 0;
     
     private bool overridden = false;
     private OpenSeeVRMExpression toggledExpression = null;
@@ -209,11 +210,39 @@ public class OpenSeeVRMDriver : MonoBehaviour {
     private float lastBrowUpDown = 0f;
     private float[] lastBrowStates;
     private float[] currentBrowStates;
-    private int browInterpolationCount = 0;
+    private TimeInterpolate browInterpolate;
-    private int browInterpolationState = 0;
     private BlendShapeKey[] browClips;
     
     private float lastAudioTime = -1f;
+    
+    private class TimeInterpolate {
+        private float interpolateT = 0f;
+        private float updateT = 0f;
+        private double lastT = 0f;
+        private double currentT = 0f;
+        private float clamp = 1f;
+        private int gotData = 0;
+        
+        public TimeInterpolate() {}
+        public TimeInterpolate(float clamp) {
+            this.clamp = clamp;
+        }
+        
+        public void UpdateTime(double nowT) {
+            updateT = Time.time;
+            lastT = currentT;
+            currentT = nowT;
+            if (gotData < 2)
+                gotData++;
+        }
+        
+        public float Interpolate() {
+            interpolateT = Time.time;
+            if (interpolateT == updateT || gotData < 2)
+                return 0f;
+            return Mathf.Min((interpolateT - updateT) / (float)(currentT - lastT), clamp);
+        }
+    }
 
     private Dictionary<OVRLipSync.Viseme, float[]> catsData = null;
     void InitCatsData() {
@@ -313,8 +342,32 @@ public class OpenSeeVRMDriver : MonoBehaviour {
         if (openSeeData == null || openSeeData.features == null)
             return;
         
+        if (lastMouthStates == null)
+            lastMouthStates = new float[]{0f, 0f, 0f, 0f, 0f};
+        if (currentMouthStates == null)
+            currentMouthStates = new float[]{0f, 0f, 0f, 0f, 0f};
+        
+        if (mouthInterpolate == null)
+            mouthInterpolate = new TimeInterpolate();
+        float t = mouthInterpolate.Interpolate();
+        
+        if (currentExpression != null && !currentExpressionEnableVisemes)
+            return;
+        
+        float expressionFactor = 1f;
+        if (currentExpression != null)
+            expressionFactor = currentExpressionVisemeFactor;
+
+        for (int i = 0; i < 5; i++) {
+            float interpolated = Mathf.Lerp(lastMouthStates[i], currentMouthStates[i], t);
+            float result = interpolated * expressionFactor * visemeFactor;
+            if (result > 0f)
+                vrmBlendShapeProxy.AccumulateValue(visemePresetMap[i], result);
+        }
+
         if (lastMouth < openSeeData.time) {
             lastMouth = openSeeData.time;
+            mouthInterpolate.UpdateTime(lastMouth);
             float open = openSeeData.features.MouthOpen;
             float wide = openSeeData.features.MouthWide;
             float[] mouthStates = new float[]{0f, 0f, 0f, 0f, 0f};
@@ -369,37 +422,11 @@ public class OpenSeeVRMDriver : MonoBehaviour {
                 }
             } while (false);
             
-            if (mouthInterpolationState == 2) {
+            for (int i = 0; i < 5; i++) {
-                for (int i = 0; i < 5; i++) {
+                lastMouthStates[i] = Mathf.Lerp(lastMouthStates[i], currentMouthStates[i], t);
-                    mouthStates[i] = Mathf.Lerp(currentMouthStates[i], mouthStates[i], 1f - mouthSmoothing);
+                mouthStates[i] = Mathf.Lerp(lastMouthStates[i], mouthStates[i], 1f - mouthSmoothing);
-                }
             }
-            
-            lastMouthStates = currentMouthStates;
             currentMouthStates = mouthStates;
-            if (mouthInterpolationState < 2)
-                mouthInterpolationState++;
-            mouthInterpolationCount = 0;
-        }
-        
-        if (mouthInterpolationState < 2)
-            return;
-        
-        float t = Mathf.Clamp((float)mouthInterpolationCount / openSeeIKTarget.averageInterpolations, 0f, 0.985f);
-        mouthInterpolationCount++;
-        
-        if (currentExpression != null && !currentExpressionEnableVisemes)
-            return;
-        
-        float expressionFactor = 1f;
-        if (currentExpression != null)
-            expressionFactor = currentExpressionVisemeFactor;
-
-        for (int i = 0; i < 5; i++) {
-            float interpolated = Mathf.Lerp(lastMouthStates[i], currentMouthStates[i], t);
-            float result = interpolated * expressionFactor * visemeFactor;
-            if (result > 0f)
-                vrmBlendShapeProxy.AccumulateValue(visemePresetMap[i], result);
         }
     }
     
@@ -411,6 +438,29 @@ public class OpenSeeVRMDriver : MonoBehaviour {
         if ((browClips == null && (faceMesh == null || faceType < 0)) || openSeeData == null) {
             return;
         }
+        if (browInterpolate == null)
+            browInterpolate = new TimeInterpolate();
+
+        float t = browInterpolate.Interpolate();
+        float strength = eyebrowStrength;
+        if (currentExpression != null)
+            strength *= currentExpressionEyebrowWeight;
+        
+        if (browUpIndex > -1 && strength > 0f)
+            faceMesh.SetBlendShapeWeight(browUpIndex, strength * Mathf.Lerp(lastBrowStates[0], currentBrowStates[0], t));
+        if (browDownIndex > -1 && strength > 0f)
+            faceMesh.SetBlendShapeWeight(browDownIndex, strength * Mathf.Lerp(lastBrowStates[1], currentBrowStates[1], t));
+        if (browAngryIndex > -1 && strength > 0f)
+            faceMesh.SetBlendShapeWeight(browAngryIndex, strength * Mathf.Lerp(lastBrowStates[2], currentBrowStates[2], t));
+        if (browSorrowIndex > -1 && strength > 0f)
+            faceMesh.SetBlendShapeWeight(browSorrowIndex, strength * Mathf.Lerp(lastBrowStates[3], currentBrowStates[3], t));
+        if (browClips != null && strength > 0f) {
+            if (lastBrowUpDown > 0f)
+                vrmBlendShapeProxy.AccumulateValue(browClips[0], strength * Mathf.Lerp(lastBrowStates[0], currentBrowStates[0], t));
+            else if (lastBrowUpDown < 0f)
+                vrmBlendShapeProxy.AccumulateValue(browClips[1], strength * Mathf.Lerp(lastBrowStates[1], currentBrowStates[1], t));
+        }
+
         if (lastBrows < openSeeData.time) {
             lastBrows = openSeeData.time;
             float upDownStrength = (openSeeData.features.EyebrowUpDownLeft + openSeeData.features.EyebrowUpDownRight) / 2f;
@@ -448,15 +498,12 @@ public class OpenSeeVRMDriver : MonoBehaviour {
             }
             if (eyebrowIsMoving > 0)
                 eyebrowIsMoving--;
-            upDownStrength = Mathf.Lerp(lastBrowUpDown, upDownStrength, 1f - eyebrowSmoothing);
+            upDownStrength = Mathf.LerpUnclamped(lastBrowUpDown, upDownStrength, 1f - eyebrowSmoothing);
             lastBrowUpDown = upDownStrength;
             
-            if (browInterpolationState < 2)
-                browInterpolationState++;
-            browInterpolationCount = 0;
-            
             for (int i = 0; i < 4; i++)
-                lastBrowStates[i] = currentBrowStates[i];
+                lastBrowStates[i] = Mathf.Lerp(lastBrowStates[i], currentBrowStates[i], t);
+            browInterpolate.UpdateTime(openSeeData.time);
             
             if (browClips != null) {
                 currentBrowStates[0] = upDownStrength;
@@ -498,28 +545,6 @@ public class OpenSeeVRMDriver : MonoBehaviour {
                 }
             }
         }
-        
-        float t = Mathf.Clamp((float)browInterpolationCount / openSeeIKTarget.averageInterpolations, 0f, 0.985f);
-        browInterpolationCount++;
-        
-        float strength = eyebrowStrength;
-        if (currentExpression != null)
-            strength *= currentExpressionEyebrowWeight;
-        
-        if (browUpIndex > -1 && strength > 0f)
-            faceMesh.SetBlendShapeWeight(browUpIndex, strength * Mathf.Lerp(lastBrowStates[0], currentBrowStates[0], t));
-        if (browDownIndex > -1 && strength > 0f)
-            faceMesh.SetBlendShapeWeight(browDownIndex, strength * Mathf.Lerp(lastBrowStates[1], currentBrowStates[1], t));
-        if (browAngryIndex > -1 && strength > 0f)
-            faceMesh.SetBlendShapeWeight(browAngryIndex, strength * Mathf.Lerp(lastBrowStates[2], currentBrowStates[2], t));
-        if (browSorrowIndex > -1 && strength > 0f)
-            faceMesh.SetBlendShapeWeight(browSorrowIndex, strength * Mathf.Lerp(lastBrowStates[3], currentBrowStates[3], t));
-        if (browClips != null && strength > 0f) {
-            if (lastBrowUpDown > 0f)
-                vrmBlendShapeProxy.AccumulateValue(browClips[0], strength * Mathf.Lerp(lastBrowStates[0], currentBrowStates[0], t));
-            else if (lastBrowUpDown < 0f)
-                vrmBlendShapeProxy.AccumulateValue(browClips[1], strength * Mathf.Lerp(lastBrowStates[1], currentBrowStates[1], t));
-        }
     }
     
     void FindFaceMesh() {
@@ -924,38 +949,18 @@ public class OpenSeeVRMDriver : MonoBehaviour {
     }
     
     void UpdateGaze() {
-        if (openSeeData == null || vrmBlendShapeProxy == null || openSeeIKTarget == null || !(openSeeIKTarget.averageInterpolations > 0f))
+        if (openSeeData == null || vrmBlendShapeProxy == null || openSeeIKTarget == null)
             return;
         
         float lookUpDown = currentLookUpDown;
         float lookLeftRight = currentLookLeftRight;
-
-        if (lastGaze < openSeeData.time) {
-            GetLookParameters(ref lookLeftRight, ref lookUpDown, false, 66, 36, 39, 37, 38, 41, 40);
-            GetLookParameters(ref lookLeftRight, ref lookUpDown, true,  67, 42, 45, 43, 44, 47, 46);
-            
-            lookLeftRight = Mathf.Lerp(currentLookLeftRight, lookLeftRight, 1f - gazeSmoothing);
-            lookUpDown = Mathf.Lerp(currentLookUpDown, lookUpDown, 1f - gazeSmoothing);
-
-            if (Mathf.Abs(lookLeftRight - currentLookLeftRight) + Mathf.Abs(lookUpDown - currentLookUpDown) > gazeStabilizer) {
-                lastLookLeftRight = currentLookLeftRight;
-                lastLookUpDown = currentLookUpDown;
-                currentLookLeftRight = lookLeftRight;
-                currentLookUpDown = lookUpDown;
-                lastGaze = openSeeData.time;
-                if (interpolationState < 2)
-                    interpolationState++;
-                interpolationCount = 0;
-            }
-        }
         
-        if (interpolationState < 2)
+        if (gazeInterpolate == null)
-            return;
+            gazeInterpolate = new TimeInterpolate();
-        
+        float t = gazeInterpolate.Interpolate();
-        float t = Mathf.Clamp((float)interpolationCount / openSeeIKTarget.averageInterpolations, 0f, 0.985f);
+
         lookLeftRight = Mathf.Lerp(lastLookLeftRight, currentLookLeftRight, t);
         lookUpDown = Mathf.Lerp(lastLookUpDown, currentLookUpDown, t);
-        interpolationCount++;
         
         if (!openSeeIKTarget.mirrorMotion)
             lookLeftRight = -lookLeftRight;
@@ -989,6 +994,23 @@ public class OpenSeeVRMDriver : MonoBehaviour {
                 leftEye.localRotation = rotation;
             }
         }
+
+        if (lastGaze < openSeeData.time) {
+            GetLookParameters(ref lookLeftRight, ref lookUpDown, false, 66, 36, 39, 37, 38, 41, 40);
+            GetLookParameters(ref lookLeftRight, ref lookUpDown, true,  67, 42, 45, 43, 44, 47, 46);
+            
+            lookLeftRight = Mathf.Lerp(currentLookLeftRight, lookLeftRight, 1f - gazeSmoothing);
+            lookUpDown = Mathf.Lerp(currentLookUpDown, lookUpDown, 1f - gazeSmoothing);
+
+            if (Mathf.Abs(lookLeftRight - currentLookLeftRight) + Mathf.Abs(lookUpDown - currentLookUpDown) > gazeStabilizer) {
+                lastLookLeftRight = Mathf.Lerp(lastLookLeftRight, currentLookLeftRight, t);
+                lastLookUpDown = Mathf.Lerp(lastLookUpDown, currentLookUpDown, t);
+                currentLookLeftRight = lookLeftRight;
+                currentLookUpDown = lookUpDown;
+                lastGaze = openSeeData.time;
+                gazeInterpolate.UpdateTime(lastGaze);
+            }
+        }
     }
 
     void BlinkEyes() {
@@ -1011,27 +1033,54 @@ public class OpenSeeVRMDriver : MonoBehaviour {
                 upDownAngle += 360f;
             while (upDownAngle >= 360f)
                 upDownAngle -= 360f;
-            if (lookDownCompensation && upDownAngle > turnDownBoundaryAngle && autoBlinkLookingDown) {
+            if (lookDownCompensation && upDownAngle > turnDownBoundaryAngle - lookDownAdjustment - wasLookingDown && autoBlinkLookingDown) {
+                if (!(wasLookingDown > 0f))
+                    eyeBlinker.Blink();
                 float blink = eyeBlinker.Blink();
                 vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink), blink);
                 return;
             }
 
+            if (blinkInterpolate == null)
+                blinkInterpolate = new TimeInterpolate();
+            float t = blinkInterpolate.Interpolate();
+            
+            left = Mathf.Lerp(lastBlinkLeft, currentBlinkLeft, t);
+            right = Mathf.Lerp(lastBlinkRight, currentBlinkRight, t);
+            
+            if (openSeeIKTarget.mirrorMotion) {
+                float tmp = left;
+                left = right;
+                right = tmp;
+            }
+            
+            if (linkBlinks && !allowWinking) {
+                float v = Mathf.Max(left, right);
+                vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink), v);
+            } else {
+                vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink_R), right);
+                vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink_L), left);
+            }
+
             if (openSeeData != null && lastBlink < openSeeData.time) {
                 lastBlink = openSeeData.time;
+                blinkInterpolate.UpdateTime(lastBlink);
                 float openThreshold = eyeOpenedThreshold;
                 float closedThreshold = eyeClosedThreshold;
                 
                 if (openSeeData.rawEuler.y < turnLeftBoundaryAngle || openSeeData.rawEuler.y > turnRightBoundaryAngle)
                     openThreshold = Mathf.Lerp(eyeOpenedThreshold, eyeClosedThreshold, 0.4f);
 
-                if (lookDownCompensation && upDownAngle > turnDownBoundaryAngle) {
+                if (lookDownCompensation && upDownAngle > turnDownBoundaryAngle - lookDownAdjustment - wasLookingDown) {
                     openThreshold = Mathf.Lerp(eyeOpenedThreshold, eyeClosedThreshold, 0.85f);
                     closedThreshold = Mathf.Lerp(eyeClosedThreshold, 0f, 0.6f);
-                    if (upDownAngle > turnDownBoundaryAngle + 10f) {
+                    if (upDownAngle > turnDownBoundaryAngle - lookDownAdjustment + 10f) {
                         openThreshold = 0.1f;
                         closedThreshold = 0f;
                     }
+                    wasLookingDown = 1.5f;
+                } else {
+                    wasLookingDown = 0f;
                 }
                 
                 if (openSeeData.rightEyeOpen > openThreshold)
@@ -1061,8 +1110,8 @@ public class OpenSeeVRMDriver : MonoBehaviour {
                     }
                 }
 
-                lastBlinkLeft = currentBlinkLeft;
+                lastBlinkLeft = Mathf.Lerp(lastBlinkLeft, currentBlinkLeft, t);
-                lastBlinkRight = currentBlinkRight;
+                lastBlinkRight = Mathf.Lerp(lastBlinkRight, currentBlinkRight, t);
                 currentBlinkLeft = Mathf.Lerp(currentBlinkLeft, left, 1f - blinkSmoothing);
                 currentBlinkRight = Mathf.Lerp(currentBlinkRight, right, 1f - blinkSmoothing);
                 
@@ -1074,32 +1123,6 @@ public class OpenSeeVRMDriver : MonoBehaviour {
                     currentBlinkLeft = 1f;
                 if (right > 0.99999f)
                     currentBlinkRight = 1f;
-                
-                if (blinkInterpolationState < 2)
-                    blinkInterpolationState++;
-            }
-            
-            if (blinkInterpolationState < 2)
-                return;
-            
-            float t = Mathf.Clamp((float)blinkInterpolationCount / openSeeIKTarget.averageInterpolations, 0f, 0.985f);
-            blinkInterpolationCount++;
-            
-            left = Mathf.Lerp(lastBlinkLeft, currentBlinkLeft, t);
-            right = Mathf.Lerp(lastBlinkRight, currentBlinkRight, t);
-            
-            if (openSeeIKTarget.mirrorMotion) {
-                float tmp = left;
-                left = right;
-                right = tmp;
-            }
-            
-            if (linkBlinks && !allowWinking) {
-                float v = Mathf.Max(left, right);
-                vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink), v);
-            } else {
-                vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink_R), right);
-                vrmBlendShapeProxy.AccumulateValue(BlendShapeKey.CreateFromPreset(BlendShapePreset.Blink_L), left);
             }
         }
     }

Unity/OpenSeeExpression.cs

@@ -166,10 +166,10 @@ public class OpenSeeExpression : MonoBehaviour
                 oser = formatter.Deserialize(gzipStream) as OpenSeeExpressionRepresentation;
             }
             expressions = oser.expressions;
-            if (oser.thunderSVM)
+            /*if (oser.thunderSVM)
                 model = new ThunderSVMModel(oser.modelBytes);
-            else
+            else*/
-                model = new SVMModel(oser.modelBytes);
+            model = new SVMModel(oser.modelBytes);
             classLabels = oser.classLabels;
             indices = oser.indices;
             //pointSelection = oser.pointSelection;

Unity/OpenSeeIKTarget.cs

@@ -22,6 +22,8 @@ public class OpenSeeIKTarget : MonoBehaviour
     [Tooltip("This smoothed out the detected motion. Values can be between 0 (no smoothing) and 1 (no motion). The appropriate value probably depends on the camera's frame rate.")]
     [Range(0, 1)]
     public float smooth = 0.2f;
+    [Tooltip("When set to true, a Kalman filter is used for smoothing.")]
+    public bool kalman = false;
     [Tooltip("When enabled, tracking will lag by one video capture frame, but the captured motion information is interpolated between frames, making things look smoother and less jittery and jumpy.")]
     public bool interpolate = true;
     [Tooltip("When set to true, the transform will be updated in FixedUpdate, otherwise it will be updated in Update.")]
@@ -82,9 +84,65 @@ public class OpenSeeIKTarget : MonoBehaviour
     private bool skippedPosition = false;
     private float skipStartTimePosition = -1f;
     
+    // Kalman
+    private float frameTime = 0f;
+    private bool kalmanInit = false;
+    private float kalmanSmooth = -1f;
+    private KF kalmanPVX;
+    private KF kalmanPVY;
+    private KF kalmanPVZ;
+    private KF kalmanRVX;
+    private KF kalmanRVY;
+    private KF kalmanRVZ;
+    private Vector3 kalmanLastRot = Vector3.zero;
+    
     public void Calibrate() {
         calibrate = true;
     }
+    
+    public static Vector3 PrettyEuler(Vector3 last, Vector3 current) {
+        // Ew, but how would you do componentwise filtering/interpolation with quaternions?
+        if (Mathf.Abs(current.x - last.x) > Mathf.Abs((current.x - 360f) - last.x))
+            current.x -= 360f;
+        else if (Mathf.Abs(current.x - last.x) > Mathf.Abs((current.x + 360f) - last.x))
+            current.x += 360f;
+        if (Mathf.Abs(current.y - last.y) > Mathf.Abs((current.y - 360f) - last.y))
+            current.y -= 360f;
+        else if (Mathf.Abs(current.y - last.y) > Mathf.Abs((current.y + 360f) - last.y))
+            current.x += 360f;
+        if (Mathf.Abs(current.z - last.z) > Mathf.Abs((current.z - 360f) - last.z))
+            current.z -= 360f;
+        else if (Mathf.Abs(current.z - last.z) > Mathf.Abs((current.z + 360f) - last.z))
+            current.z += 360f;
+        return current;
+    }
+    
+    Vector3 FilterPos(Vector3 a, Vector3 b) {
+        if (!kalman || frameTime > 1f)
+            return Vector3.Lerp(a, b, 1f - smooth);
+        else {
+            Vector3 v = (b - a) / frameTime;
+            v.x = kalmanPVX.Update(v.x);
+            v.y = kalmanPVY.Update(v.y);
+            v.z = kalmanPVZ.Update(v.z);
+            return a + v * frameTime;
+        }
+    }
+    
+    Quaternion FilterRot(Quaternion a, Quaternion b) {
+        if (!kalman || frameTime > 1f)
+            return Quaternion.Lerp(a, b, 1f - smooth);
+        else {
+            Vector3 rotA = PrettyEuler(kalmanLastRot, a.eulerAngles);
+            Vector3 rotB = PrettyEuler(rotA, b.eulerAngles);
+            kalmanLastRot = rotA;
+            Vector3 v = Mathf.Deg2Rad * (rotB - rotA) / frameTime;
+            v.x = kalmanRVX.Update(v.x);
+            v.y = kalmanRVY.Update(v.y);
+            v.z = kalmanRVZ.Update(v.z);
+            return Quaternion.Euler(rotA + Mathf.Rad2Deg * v * frameTime);
+        }
+    }
 
     void Interpolate() {
         if (!interpolate || interpolateState < 2)
@@ -110,6 +168,7 @@ public class OpenSeeIKTarget : MonoBehaviour
         if (openSeeData == null || openSeeData.fit3DError > openSee.maxFit3DError)
             return;
         if (openSeeData.time > updated) {
+            frameTime = (float)(openSeeData.time - updated);
             updated = openSeeData.time;
             trackingFrames++;
             if (trackingFrames < skipFirst)
@@ -118,6 +177,25 @@ public class OpenSeeIKTarget : MonoBehaviour
             Interpolate();
             return;
         }
+        
+        if (!kalmanInit) {
+            kalmanPVX = new KF(smooth);
+            kalmanPVY = new KF(smooth);
+            kalmanPVZ = new KF(smooth);
+            kalmanRVX = new KF(smooth);
+            kalmanRVY = new KF(smooth);
+            kalmanRVZ = new KF(smooth);
+            kalmanInit = true;
+        }
+        if (smooth != kalmanSmooth) {
+            kalmanPVX.SetSmoothness(smooth);
+            kalmanPVY.SetSmoothness(smooth);
+            kalmanPVZ.SetSmoothness(smooth);
+            kalmanRVX.SetSmoothness(smooth);
+            kalmanRVY.SetSmoothness(smooth);
+            kalmanRVZ.SetSmoothness(smooth);
+            kalmanSmooth = smooth;
+        }
 
         Quaternion convertedQuaternion = new Quaternion(-openSeeData.rawQuaternion.y, -openSeeData.rawQuaternion.x, openSeeData.rawQuaternion.z, openSeeData.rawQuaternion.w);
         Vector3 t = openSeeData.translation;
@@ -219,12 +297,12 @@ public class OpenSeeIKTarget : MonoBehaviour
             avgInterps = Mathf.Lerp(avgInterps, (float)interpolationCount, 0.15f);
         interpolationCount = 0;
         averageInterpolations = avgInterps;
-
+        
         lastT = transform.localPosition;
         lastR = transform.localRotation;
         if (interpolateState > 0) {
-            currentT = Vector3.Lerp(currentT, (t - dT) * translationScale, 1f - smooth);
+            currentT = FilterPos(currentT, (t - dT) * translationScale);
-            currentR = Quaternion.Lerp(transform.localRotation, convertedQuaternion * dR, 1f - smooth);
+            currentR = FilterRot(transform.localRotation, convertedQuaternion * dR);
         } else {
             currentT = (t - dT) * translationScale;
             currentR = convertedQuaternion * dR;
@@ -235,11 +313,14 @@ public class OpenSeeIKTarget : MonoBehaviour
         if (interpolate) {
             Interpolate();
         } else {
-            transform.localPosition = Vector3.Lerp(transform.localPosition, (t - dT) * translationScale, 1f - smooth);
+            transform.localPosition = FilterPos(transform.localPosition, (t - dT) * translationScale);
-            transform.localRotation = Quaternion.Lerp(transform.localRotation, convertedQuaternion * dR, 1f - smooth);
+            transform.localRotation = FilterRot(transform.localRotation, convertedQuaternion * dR);
         }
         if (kinematicInterpolation != null && kinematicInterpolation.gameObject != gameObject) {
-            kinematicInterpolation.UpdateKI(updated, transform.localPosition, transform.localRotation);
+            if (interpolate)
+                kinematicInterpolation.UpdateKI(updated, currentT, currentR);
+            else
+                kinematicInterpolation.UpdateKI(updated, transform.localPosition, transform.localRotation);
         }
         
         if (driftBack) {
@@ -266,6 +347,34 @@ public class OpenSeeIKTarget : MonoBehaviour
         if (!fixedUpdate)
             RunUpdate();
     }
+    
+    class KF {
+        private float R, P, Q, x;
+
+        public KF(float smooth) {
+            P = 1f;
+            x = 0f;
+            SetSmoothness(smooth);
+        }
+        
+        public void SetSmoothness(float smooth) {
+            P = 1f;
+            Q = 0.3f;// * (Mathf.Exp(3f * smooth) + 1f);
+            R = 0.000001f + Mathf.Exp(smooth * 5f);
+        }
+
+        public float Update(float input) {
+            //x = x * A;
+            //P = A * P * A;
+            
+            float K = P / (P + R);
+            x = x + K * (input - x);
+            //Debug.Log("P = " + P + " 1-K = " + (1f - K));
+            P = (1f - K) * P + Q;
+
+            return x;
+        }
+    }
 }
 
 }

Unity/OpenSeeKinematicInterpolation.cs

@@ -3,6 +3,8 @@ using System.Collections;
 using System.Collections.Generic;
 using UnityEngine;
 
+namespace OpenSee {
+
 public class OpenSeeKinematicInterpolation : MonoBehaviour
 {
     private Vector3 lastPos;
@@ -13,7 +15,7 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour
     private Vector3 currentPosV;
     private double currentT = 0f;
     
-    private Vector3 lastRot;
+    private Vector3 lastRot = Vector3.zero;
     private Vector3 lastRotV;
     
     private Vector3 currentRot;
@@ -28,27 +30,12 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour
     private double updateT;
     private double interpolateT;
     
-    public void PrettyEuler() {
-        // Ew, but how would you do this interpolation with quaternions?
-        if (currentRot.x > 270f && lastRot.x < 90f)
-            currentRot.x -= 360f;
-        if (currentRot.y > 270f && lastRot.y < 90f)
-            currentRot.y -= 360f;
-        if (currentRot.z > 270f && lastRot.z < 90f)
-            currentRot.z -= 360f;
-        if (currentRot.x < 90f && lastRot.x > 270f)
-            lastRot.x -= 360f;
-        if (currentRot.y < 90f && lastRot.y > 270f)
-            lastRot.y -= 360f;
-        if (currentRot.z < 90f && lastRot.z > 270f)
-            lastRot.z -= 360f;
-    }
-    
     public void UpdateKI(double newT, Vector3 newPos, Quaternion newRot) {
         Interpolate();
         currentPos = transform.localPosition;
+        currentRot = OpenSeeIKTarget.PrettyEuler(currentRot, transform.localRotation.eulerAngles);
+        float dTNow = (float)(interpolateT - updateT);
         if (interpolateT - updateT > 0) {
-            float dTNow = (float)(interpolateT - updateT);
             currentPosV = (currentPos - lastPos) / dTNow;
             currentRotV = (currentRot - lastRot) / dTNow;
         } else {
@@ -63,8 +50,7 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour
         lastT = currentT;
         
         currentPos = newPos;
-        currentRot = newRot.eulerAngles;
+        currentRot = OpenSeeIKTarget.PrettyEuler(lastRot, newRot.eulerAngles);
-        PrettyEuler();
         currentT = newT;
         dT = (float)(currentT - lastT);
         currentPosV = (currentPos - lastPos) / dT;
@@ -109,4 +95,6 @@ public class OpenSeeKinematicInterpolation : MonoBehaviour
     public void LateUpdate() {
         Interpolate();
     }
+}
+
 }