Question

尝试找到有助于我们通过一系列点绘制3D线的函数。

我们知道的每一点：日期和时间，纬度，经度，海拔高度，速度和航向。数据可能每10秒记录一次，我们希望能够猜测两者之间的点数并将粒度增加到1秒。从而在3D空间中创建虚拟飞行路径。

我发现了许多曲线拟合算法，这些算法将通过一系列点逼近一条直线，但它们不能保证这些点相交。他们也没有考虑速度和航向来确定物体到达下一个点所采取的最可能路径。

Answer 1

从物理学的角度来看：

您必须假设中间点的加速度才能获得插值。

如果您的物理系统相对较好（如汽车或飞机），而不是例如弹跳球，您可以继续假设加速度随点之间的时间线性变化。

恒定变化加速运动的矢量方程是：

 x''[t] = a t + b

除了t之外的所有大小都是向量。

对于你已经知道的每个段v（t = t0）x（t = t0）tfinal和x（tfinal）v（tfinal）

通过求解微分方程式得到：

Eq 1:
x[t_] := (3 b t^2 Tf + a t^3 Tf - 3 b t Tf^2 - a t Tf^3 - 6 t X0 + 6 Tf X0 + 6 t Xf)/(6 Tf)

并强调你获得的位置和速度的初始和最终限制：

方程2：

 a -> (6 (Tf^2 V0 - 2 T0 Tf Vf + Tf^2 Vf - 2 T0 X0 + 2 Tf X0 + 
        2 T0 Xf - 2 Tf Xf))/(Tf^2 (3 T0^2 - 4 T0 Tf + Tf^2))

 b -> (2 (-2 Tf^3 V0 + 3 T0^2 Tf Vf - Tf^3 Vf + 3 T0^2 X0 - 
        3 Tf^2 X0 - 3 T0^2 Xf + 3 Tf^2 Xf))/(Tf^2 (3 T0^2 - 4 T0 Tf + Tf^2))}}

因此，将等式2的值插入到等式1中，您可以根据初始和最终位置和速度获得点的时间插值。

HTH！

修改

一些示例在两个维度上具有突然的速度变化（在3D中完全相同）。如果初始和最终速度相似，您将获得“更直”的路径。

假设：

X0 = {0, 0}; Xf = {1, 1};
T0 = 0;      Tf = 1;

如果

V0 = {0, 1}; Vf = {-1, 3};

alt text

V0 = {0, 1}; Vf = {-1, 5};

alt text

V0 = {0, 1}; Vf = {1, 3};

alt text

这是一个动画，您可以看到速度从V0 = {0,1}变为Vf = {1,5}： alt text

在这里你可以看到一个3D加速体，其间位置相等：

alt text

修改

一个完整的问题：

为方便起见，我将使用笛卡尔坐标。如果你想从lat / log / alt转换为Cartesian，请执行：

x = rho sin(theta) cos(phi)
y = rho sin(theta) sin(phi)
z = rho cos(theta)

当phi是经度时，θ是纬度，rho是你的高度加上地球的半径。

假设我们在以下地点开始我们的细分：

 t=0 with coordinates (0,0,0) and velocity (1,0,0)

并在

结束

 t=10 with coordinates (10,10,10) and velocity (0,0,1)

我清楚地对坐标原点进行了更改，以便在起点处设置原点。这只是为了得到漂亮的数字......

因此我们在a和b的公式中替换这些数字并得到：

a = {-(3/50), -(3/25), -(3/50)}  b = {1/5, 3/5, 2/5}

用那些我们去eq 1，对象的位置由下式给出：

p[t] = {1/60 (60 t + 6 t^2 - (3 t^3)/5), 
        1/60 (18 t^2 - (6 t^3)/5), 
        1/60 (12 t^2 - (3 t^3)/5)}

就是这样。你得到1到10秒的位置，用上面的等式中的valus代替t 动画运行：

alt text

编辑2

如果你不想弄乱垂直加速度（也许是因为你的“速度计”没有读取它），你可以给z轴分配一个恒定的速度（考虑它有一个很小的错误平行于Rho轴），等于（Zfinal-Zinit）/（Tf-T0），然后解决平面遗忘海拔的问题。

Answer 2

您要问的是一般interpolation问题。我的猜测是你的实际问题不是由于使用了曲线拟合算法，而是你将它应用于系统记录的所有离散值而不是相关的值集。

让我们分解你的问题。您目前正在绘制球形贴图3D空间中的一个点，调整线性和弯曲路径。如果我们将具有六个自由度（roll, pitch, and yaw）的对象执行的操作离散化，那么您唯一感兴趣的操作是线性路径和弯曲路径，以解决任何方向上的俯仰和偏航。考虑到basic physics.

，也可以考虑加速和减速

处理球形映射很容易。只需将您的点相对于它们在平面上的位置展开，调整纬度，经度和海拔高度。这应该允许您展平沿弯曲路径存在的数据，尽管这对于您的问题的解决方案可能不是必需的（见下文）。

线性插值很容易。给定一个任意数量的点，这些点在时间上适合由系统确定的 n 误差内的线，*构造线并计算每个点之间的时间距离。从这里开始，尝试将时间点拟合为两种情况之一：恒定速度或恒定加速度。

曲线插值有点困难，但仍然合理。对于俯仰，偏航或组合俯仰+偏航的情况，在系统的弯曲读数的 m 误差范围内构建一个包含任意数量的点的平面，在 m 误差范围内。*从这些数据构建planar curve并再次考虑恒定速度或acceleration along the curve。

通过尝试预测飞行中飞机的预期操作作为相对于飞行路径的决策树或神经网络的一部分，您可以做得更好。我将把它作为读者的练习。

祝你好运设计你的系统。

-

*考虑到问题的描述，预计两个错误读数都来自GPS数据。对这些数据中的错误进行核算和调整是一个单独的有趣问题。

Answer 3

您需要（而不是物理建模）是通过数据拟合样条曲线。我使用了一个数字收件簿（http://www.nrbook.com/a有免费的C和FORTRAN算法。查看F77第3.3节以获得所需的数学）。如果你想要简单，那么只需通过点拟合线，但这根本不会产生平滑的飞行路径。时间将是您的x值，每个参数都将包含自己的公共样条参数。

因为我们喜欢这个问题的长篇贴文，所以这里是完整的代码：

//驱动程序

static void Main(string[] args)
{
    double[][] flight_data = new double[][] {
        new double[] { 0, 10, 20, 30 }, // time in seconds
        new double[] { 14500, 14750, 15000, 15125 }, //altitude in ft
        new double[] { 440, 425, 415, 410 }, // speed in knots
    };

    CubicSpline altitude = new CubicSpline(flight_data[0], flight_data[1]);
    CubicSpline speed = new CubicSpline(flight_data[0], flight_data[2]);

    double t = 22; //Find values at t
    double h = altitude.GetY(t);
    double v = speed.GetY(t);

    double ascent = altitude.GetYp(t); // ascent rate in ft/s

}

//立方样条定义

using System.Linq;

/// <summary>
///     Cubic spline interpolation for tabular data
/// </summary>
/// <remarks>
///     Adapted from numerical recipies for FORTRAN 77 
///     (ISBN 0-521-43064-X), page 110, section 3.3.
///     Function spline(x,y,yp1,ypn,y2) converted to 
///     C# by jalexiou, 27 November 2007.
///     Spline integration added also Nov 2007.
/// </remarks>
public class CubicSpline 
{
    double[] xi;
    double[] yi;
    double[] yp;
    double[] ypp;
    double[] yppp;
    double[] iy;

    #region Constructors

    public CubicSpline(double x_min, double x_max, double[] y)
        : this(Sequence(x_min, x_max, y.Length), y)
    { }

    public CubicSpline(double x_min, double x_max, double[] y, double yp1, double ypn)
        : this(Sequence(x_min, x_max, y.Length), y, yp1, ypn)
    { }

    public CubicSpline(double[] x, double[] y)
        : this(x, y, double.NaN, double.NaN)
    { }

    public CubicSpline(double[] x, double[] y, double yp1, double ypn)
    {
        if( x.Length == y.Length )
        {
            int N = x.Length;
            xi = new double[N];
            yi = new double[N];
            x.CopyTo(xi, 0);
            y.CopyTo(yi, 0);

            if( N > 0 )
            {
                double p, qn, sig, un;
                ypp = new double[N];
                double[] u = new double[N];

                if( double.IsNaN(yp1) )
                {
                    ypp[0] = 0;
                    u[0] = 0;
                }
                else
                {
                    ypp[0] = -0.5;
                    u[0] = (3 / (xi[1] - xi[0])) * 
                          ((yi[1] - yi[0]) / (x[1] - x[0]) - yp1);
                }
                for (int i = 1; i < N-1; i++)
                {
                    double hp = x[i] - x[i - 1];
                    double hn = x[i + 1] - x[i];
                    sig = hp / hn;
                    p = sig * ypp[i - 1] + 2.0;
                    ypp[i] = (sig - 1.0) / p;
                    u[i] = (6 * ((y[i + 1] - y[i]) / hn) - (y[i] - y[i - 1]) / hp)
                        / (hp + hn) - sig * u[i - 1] / p;
                }
                if( double.IsNaN(ypn) )
                {
                    qn = 0;
                    un = 0;
                }
                else
                {
                    qn = 0.5;
                    un = (3 / (x[N - 1] - x[N - 2])) * 
                          (ypn - (y[N - 1] - y[N - 2]) / (x[N - 1] - x[N - 2]));
                }
                ypp[N - 1] = (un - qn * u[N - 2]) / (qn * ypp[N - 2] + 1.0);
                for (int k = N-2; k > 0; k--)
                {
                    ypp[k] = ypp[k] * ypp[k + 1] + u[k];
                }

                // Calculate 1st derivatives
                yp = new double[N];
                double h;
                for( int i = 0; i < N - 1; i++ )
                {
                    h = xi[i + 1] - xi[i];
                    yp[i] = (yi[i + 1] - yi[i]) / h
                           - h / 6 * (ypp[i + 1] + 2 * ypp[i]);
                }
                h = xi[N - 1] - xi[N - 2];
                yp[N - 1] = (yi[N - 1] - yi[N - 2]) / h
                             + h / 6 * (2 * ypp[N - 1] + ypp[N - 2]);
                // Calculate 3rd derivatives as average of dYpp/dx
                yppp = new double[N];
                double[] jerk_ij = new double[N - 1];
                for( int i = 0; i < N - 1; i++ )
                {
                    h = xi[i + 1] - xi[i];
                    jerk_ij[i] = (ypp[i + 1] - ypp[i]) / h;
                }
                Yppp = new double[N];
                yppp[0] = jerk_ij[0];
                for( int i = 1; i < N - 1; i++ )
                {
                    yppp[i] = 0.5 * (jerk_ij[i - 1] + jerk_ij[i]);
                }
                yppp[N - 1] = jerk_ij[N - 2];
                // Calculate Integral over areas
                iy = new double[N];
                yi[0] = 0;  //Integration constant
                for( int i = 0; i < N - 1; i++ )
                {
                    h = xi[i + 1] - xi[i];
                    iy[i + 1] = h * (yi[i + 1] + yi[i]) / 2
                             - h * h * h / 24 * (ypp[i + 1] + ypp[i]);
                }
            }
            else
            {
                yp = new double[0];
                ypp = new double[0];
                yppp = new double[0];
                iy = new double[0];
            }
        }
        else
            throw new IndexOutOfRangeException();
    }

    #endregion

    #region Actions/Functions
    public int IndexOf(double x)
    {
        //Use bisection to find index
        int i1 = -1;
        int i2 = Xi.Length;
        int im;
        double x1 = Xi[0];
        double xn = Xi[Xi.Length - 1];
        bool ascending = (xn >= x1);
        while( i2 - i1 > 1 )
        {
            im = (i1 + i2) / 2;
            double xm = Xi[im];
            if( ascending & (x >= Xi[im]) ) { i1 = im; } else { i2 = im; }
        }
        if( (ascending && (x <= x1)) || (!ascending & (x >= x1)) )
        {
            return 0;
        }
        else if( (ascending && (x >= xn)) || (!ascending && (x <= xn)) )
        {
            return Xi.Length - 1;
        }
        else
        {
            return i1;
        }
    }

    public double GetIntY(double x)
    {
        int i = IndexOf(x);
        double x1 = xi[i];
        double x2 = xi[i + 1];
        double y1 = yi[i];
        double y2 = yi[i + 1];
        double y1pp = ypp[i];
        double y2pp = ypp[i + 1];
        double h = x2 - x1;
        double h2 = h * h;
        double a = (x-x1)/h;
        double a2 = a*a;
        return h / 6 * (3 * a * (2 - a) * y1 
                 + 3 * a2 * y2 - a2 * h2 * (a2 - 4 * a + 4) / 4 * y1pp
                 + a2 * h2 * (a2 - 2) / 4 * y2pp);
    }


    public double GetY(double x)
    {
        int i = IndexOf(x);
        double x1 = xi[i];
        double x2 = xi[i + 1];
        double y1 = yi[i];
        double y2 = yi[i + 1];
        double y1pp = ypp[i];
        double y2pp = ypp[i + 1];
        double h = x2 - x1;
        double h2 = h * h;
        double A = 1 - (x - x1) / (x2 - x1);
        double B = 1 - A;

        return A * y1 + B * y2 + h2 / 6 * (A * (A * A - 1) * y1pp
                     + B * (B * B - 1) * y2pp);
    }

    public double GetYp(double x)
    {
        int i = IndexOf(x);
        double x1 = xi[i];
        double x2 = xi[i + 1];
        double y1 = yi[i];
        double y2 = yi[i + 1];
        double y1pp = ypp[i];
        double y2pp = ypp[i + 1];
        double h = x2 - x1;
        double A = 1 - (x - x1) / (x2 - x1);
        double B = 1 - A;

        return (y2 - y1) / h + h / 6 * (y2pp * (3 * B * B - 1)
                   - y1pp * (3 * A * A - 1));
    }

    public double GetYpp(double x)
    {
        int i = IndexOf(x);
        double x1 = xi[i];
        double x2 = xi[i + 1];
        double y1pp = ypp[i];
        double y2pp = ypp[i + 1];
        double h = x2 - x1;
        double A = 1 - (x - x1) / (x2 - x1);
        double B = 1 - A;

        return A * y1pp + B * y2pp;
    }

    public double GetYppp(double x)
    {
        int i = IndexOf(x);
        double x1 = xi[i];
        double x2 = xi[i + 1];
        double y1pp = ypp[i];
        double y2pp = ypp[i + 1];
        double h = x2 - x1;

        return (y2pp - y1pp) / h;
    }

    public double Integrate(double from_x, double to_x)
    {
        if( to_x < from_x ) { return -Integrate(to_x, from_x); }
        int i = IndexOf(from_x);
        int j = IndexOf(to_x);
        double x1 = xi[i];
        double xn = xi[j];
        double z = GetIntY(to_x) - GetIntY(from_x);    // go to nearest nodes (k) (j)
        for( int k = i + 1; k <= j; k++ )
        {
            z += iy[k];                             // fill-in areas in-between
        }            
        return z;
    }


    #endregion

    #region Properties
    public bool IsEmpty { get { return xi.Length == 0; } }
    public double[] Xi { get { return xi; } set { xi = value; } }
    public double[] Yi { get { return yi; } set { yi = value; } }
    public double[] Yp { get { return yp; } set { yp = value; } }
    public double[] Ypp { get { return ypp; } set { ypp = value; } }
    public double[] Yppp { get { return yppp; } set { yppp = value; } }
    public double[] IntY { get { return yp; } set { iy = value; } }
    public int Count { get { return xi.Length; } }
    public double X_min { get { return xi.Min(); } }
    public double X_max { get { return xi.Max(); } }
    public double Y_min { get { return yi.Min(); } }
    public double Y_max { get { return yi.Max(); } }
    #endregion

    #region Helpers

    static double[] Sequence(double x_min, double x_max, int double_of_points)
    {
        double[] res = new double[double_of_points];
        for (int i = 0; i < double_of_points; i++)
        {
            res[i] = x_min + (double)i / (double)(double_of_points - 1) * (x_max - x_min);
        }
        return res;
    }

    #endregion
}

Answer 4

您可以使用Hough Transformation算法找到与3d和2d空间中的点相交的线的近似值。我只熟悉它在2d中的用途但是它仍然适用于3d空间，因为你知道你正在寻找什么样的线。有一个基本的实现描述链接。您可以使用Google进行预制，这里是指向2d C实施的链接CImg。

算法过程（粗略地）......首先，您会发现一条线的方程，您认为该方程最接近线的形状（以2d抛物线，对数，指数等）。您采用该公式并求解其中一个参数。

y = ax + b

变为

b = y - ax

接下来，对于您尝试匹配的每个点，您将点插入y和x值。有3分，你将有3个单独的b函数。

(2, 3)  : b =  3 -  2a
(4, 1)  : b =  1 -  4a
(10, -5): b = -5 - 10a

接下来，理论是您可以找到通过每个点的所有可能的线，对于每个点无限多，但是当在累加器空间中组合时，只有几个可能的参数最合适。在实践中，这通过为参数选择范围空间（我选择-2＆lt; = a＆lt; = 1,1，＆lt; = b＆lt; = 6）并开始插入变量参数的值来完成。解决对方。计算累加器中每个函数的交点数。具有最高值的点为您提供参数。

处理后的累加器b = 3 - 2a

    a: -2   -1    0    1
b: 1                   
   2                  
   3              1   
   4                  
   5         1        
   6

处理后的累加器b = 1 - 4a

    a: -2   -1    0    1
b: 1              1     
   2                  
   3              1   
   4                  
   4                  
   5         2        
   6

处理后的累加器b = -5 - 10a

    a: -2   -1    0    1
b: 1              1     
   2                  
   3              1   
   4                  
   5         3        
   6

具有最高累积值的参数集为(b a) = (5 -1)，并且最适合给定点的函数为y = 5 - x。

祝你好运。

Answer 5

我的猜测是，严重的应用会使用http://en.wikipedia.org/wiki/Kalman_filter。顺便说一句，这可能不能保证报告的点也相交，除非你稍微摆弄参数。它会在每个数据点给出一定程度的错误，因此它认为对象在时间T不一定是在时间T的位置。当然，你可以设置错误分布来说明你是绝对确定你知道时间T的位置。

如果不使用卡尔曼滤波器，我会尝试将其转化为优化问题。以1s粒度工作并写下类似的方程式 x_t'= x_t +（Vx_t + Vx_t'）/ 2 + e，

Vx_t_reported = Vx_t + f，

Vx_t'= Vx_t + g 其中e，f和g代表噪音。然后创建惩罚函数，如e ^ 2 + f ^ 2 + g ^ 2 + ... 或者一些加权版本，例如1.5e ^ 2 + 3f ^ 2 + 2.6g ^ 2 + ...根据你对错误的真实含义以及你得到答案的顺畅程度，并找到使得罚函数尽可能小 - 如果方程结果很好，则最小二乘法。

三维空间中的曲线拟合点

5 个答案: