计算骨架关节的速度SDK

Question

我正在开发一个C＃WPF跌倒检测应用程序，我需要计算头部关节的速度并每隔1/3秒（大约）显示一次。因此，如果这个人摔倒（头部位置距离地面太近），我会计算最后一秒的平均速度，以确定它是跌倒还是跌倒式活动。

问题有时我得到“非数字”速度值，我不确定我的算法是否正确计算速度。你能帮我吗？

这是我的代码：

int init = 1;
double yHead;
int compteur=0;

void sensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            if (closing)
            {
                return;
            }


            using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
            {
                if (skeletonFrameData == null) 
                {
                    return ;
                }

                skeletonFrameData.CopySkeletonDataTo(allSkeletons);
                skeletonFrameData.Dispose();

                Skeleton first = (from s in allSkeletons
                                  where s.TrackingState == SkeletonTrackingState.Tracked
                                  select s).FirstOrDefault();

                if (first == null)
                {
                    return;
                }

                compteur++; // Compteur de frames
                plane = skeletonFrameData.FloorClipPlane;

                GetCameraPoint(first, e);
                if  ((init==1)&&(first.Joints[JointType.Head].TrackingState == JointTrackingState.Tracked))
                {
                    yHead = calculerDistance(first.Joints[JointType.Head]);
                    init = 0;
                    Di = yHead;
                    Ti = skeletonFrameData.Timestamp;
                }
                else{

                    if(init == 0)

                    if (fall == false)
                    {

                        yHead = calculerDistance(first.Joints[JointType.Head]);

                        Dj = yHead; Tj = skeletonFrameData.Timestamp;
                        Vi = Math.Abs(1000 * ((Dj - Di) / (Tj - Ti)));

                        if (compteur == 8)
                        { 
                            speed.Content = "Speed=" + Vi.ToString("0.##") + "m/s";             
                            compteur = 0;
                        }
                        posit.Content = "Head position: " + yHead.ToString("0.##");
                        Di = Dj;
                        Ti = Tj;

                    }
                }

            }

        }


        double calculerDistance(Joint joint)
        {
            float A = plane.Item1;
            float B = plane.Item2;
            float C = plane.Item3;
            float D = plane.Item4;

            float x = (joint.Position.X) * A;
            float y = (joint.Position.Y) * B;
            float z = (joint.Position.Z) * C;

            double distance = Math.Abs((x + y + z + D) / Math.Sqrt((A * A) + (B * B) + (C * C)));
            return distance;
        }