当前位置： 文江博客话题详情

3D空间中的曲线拟合点

发布于 2024-10-06 08:09:40 字数 217 浏览 4 评论 0原文

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

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

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

原文

Trying to find functions that will assist us to draw a 3D line through a series of points.

For each point we know: Date&Time, Latitude, Longitude, Altitude, Speed and Heading.
Data might be recorded every 10 seconds and we would like to be able to guestimate the points in between and increase granularity to 1 second. Thus creating a virtual flight path in 3D space.

I have found a number of curve fitting algorithms that will approximate a line through a series of points but they do not guarantee that the points are intersected. They also do not take into account speed and heading to determine the most likely path taken by the object to reach the next point.

如果你对这篇内容有疑问，欢迎到本站社区发帖提问 参与讨论，获取更多帮助，或者扫码二维码加入 Web 技术交流群。

扫码二维码加入Web技术交流群

发布评论

需要 登录 才能够评论， 你可以免费 注册 一个本站的账号。

评论（5）

掐死时间 2024-10-13 08:09:40

从物理学的角度来看：

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

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

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

 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 中，您将获得基于初始和最终位置以及速度的点的时间插值。

哈！

编辑

一些二维速度突然变化的示例（在 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 转换为笛卡尔坐标系，只需执行以下操作：

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

其中 phi 是经度，theta 是纬度，而 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}

用这些我们进入等式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)}

就是这样。将 t 替换为上面等式中的值，即可得到 1 到 10 秒的位置。
动画运行：

alt text

编辑 2

如果您不想弄乱垂直方向加速度（也许因为你的“速度计”没有读取它），你可以为 z 轴分配一个恒定的速度（考虑它平行于 Rho 轴有一个非常小的错误），等于（Zfinal - Zinit） /(Tf-T0)，然后解决飞机上忘记高度的问题。

From a physics viewpoint:

You have to assume something about the acceleration in your intermediate points to get the interpolation.

If your physical system is relatively well-behaved (as a car or a plane), as opposed to for example a bouncing ball, you may go ahead supposing an acceleration varying linearly with time between your points.

The vector equation for a constant varying accelerated movement is:

 x''[t] = a t + b

where all magnitudes except t are vectors.

For each segment you already know v(t=t0) x(t=t0) tfinal and x(tfinal) v(tfinal)

By solving the differential equation you get:

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)

And imposing the initial and final contraints for position and velocity you get:

Eqs 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))}}

So inserting the values for eqs 2 into eq 1 you get the temporal interpolation for your points, based on the initial and final position and velocities.

HTH!

Edit

A few examples with abrupt velocity change in two dimensions (in 3D is exactly the same). If the initial and final speeds are similar, you'll get "straighter" paths.

Suppose:

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

If

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

alt text

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

alt text

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

alt text

Here is an animation where you may see the speed changing from V0 = {0, 1} to Vf = {1, 5}:
alt text

Here you may see an accelerating body in 3D with positions taken at equal intervals:

alt text

Edit

A full problem:

For convenience, I'll work in Cartesian coordinates. If you want to convert from lat/log/alt to Cartesian just do:

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

Where phi is the longitude, theta is the latitude, and rho is your altitude plus the radius of the Earth.

So suppose we start our segment at:

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

and end at

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

I clearly made a change in the origin of coordinates to set the origin at my start point. That is just for getting nice round numbers ...

So we replace those numbers in the formulas for a and b and get:

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

With those we go to eq 1, and the position of the object is given by:

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)}

And that is it. You get the position from 1 to 10 secs replacing t by its valus in the equation above.
The animation runs:

alt text

Edit 2

If you don't want to mess with the vertical acceleration (perhaps because your "speedometer" doesn't read it), you could just assign a constant speed to the z axis (there is a very minor error for considering it parallel to the Rho axis), equal to (Zfinal - Zinit)/(Tf-T0), and then solve the problem in the plane forgetting the altitude.

回复 原文
请恋爱 2024-10-13 08:09:40

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

让我们分解一下你的问题。您当前正在球面映射的 3D 空间中绘制一个点，并调整线性和曲线路径。如果我们离散化具有六个自由度的对象执行的操作（滚动、俯仰和偏航），您特别感兴趣的唯一操作是线性路径和曲线路径，以说明任何方向的俯仰和偏航。 基础物理学，也可以考虑加速和减速。

如果了解 映射很容易。只需相对于平面上的位置展开点，调整纬度、经度和高度即可。这应该允许您展平原本沿着弯曲路径存在的数据，尽管这对于解决您的问题可能并不是严格必要的（见下文）。

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

曲线插值有点困难，但仍然合理。对于俯仰、偏航或俯仰+偏航组合的情况，构建一个包含时间上向后任意数量的点的平面，系统的曲线读数误差在 m 以内。* 根据这些数据，构建一个平面曲线并再次考虑恒定速度或沿曲线的加速度。

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

祝您设计系统好运。

--

* 根据问题描述，两个错误读数均应来自 GPS 数据。对这些数据中的错误进行核算和调整是一个单独的有趣问题。

What you're asking is a general interpolation problem. My guess is your actual problem isn't due to the curve-fitting algorithm being used, but rather your application of it to all discrete values recorded by the system instead of the relevant set of values.

Let's decompose your problem. You're currently drawing a point in spherically-mapped 3D space, adjusting for linear and curved paths. If we discretize the operations performed by an object with six degrees of freedom (roll, pitch, and yaw), the only operations you're particularly interested in are linear paths and curved paths accounting for pitch and yaw in any direction. Accounting for acceleration and deceleration also possible given understanding of basic physics.

Dealing with the spherical mapping is easy. Simply unwrap your points relative to their position on a plane, adjusting for latitude, longitude, and altitude. This should allow you to flatten data that would otherwise exist along a curved path, though this may not strictly be necessary for the solutions to your problem (see below).

Linear interpolation is easy. Given an arbitrary number of points backwards in time that fit a line within n error as determined by your system,* construct the line and compute the distance in time between each point. From here, attempt to fit the time points to one of two cases: constant velocity or constant acceleration.

Curve interpolation is a little more difficult, but still plausible. For cases of pitch, yaw, or combined pitch+yaw, construct a plane containing an arbitrary number of points backwards in time, within m error for curved readouts from your system.* From these data, construct a planar curve and once again account for constant velocity or acceleration along the curve.

You can do better than this by attempting to predict the expected operations of a plane in flight as part of a decision tree or neural network relative to the flight path. I'll leave that as an exercise for the reader.

Best of luck designing your system.

--

* Both error readouts are expected to be from GPS data, given the description of the problem. Accounting and adjusting for errors in these data is a separate interesting problem.

回复 原文
不必在意 2024-10-13 08:09:40

您需要的是（而不是对物理进行建模）通过数据拟合样条线。我使用了一本数字食谱书（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
}

What you need (instead of modeling the physics) is to fit a spline through the data. I used a numerical recipies book (http://www.nrbook.com/a has free C and FORTRAN algorithms. Look into F77 section 3.3 to get the math needed). If you want to be simple then just fit lines through the points, but that will not result in a smooth flight path at all. Time will be your x value, and each parameter loged will have it's own cublic spline parameters.

Since we like long postings for this question here is the full code:

//driver program

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

}

// Cubic spline definition

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
}
回复 原文
追星践月 2024-10-13 08:09:40

您可以使用霍夫变换算法找到与 3d 和 2d 空间中的点相交的直线的近似值。我只熟悉它在 2d 中的用途，但它仍然适用于 3d 空间，因为你知道你正在寻找什么样的线。有一个链接的基本实现描述。您可以通过 Google 搜索预制版本，这里有一个 2d C 实现 CImg 的链接。

算法过程（大致）...首先，您找到您认为最能近似直线形状的直线方程（二维抛物线、对数、指数等）。您采用该公式并求解其中一个参数。

y = ax + b

变为

b = y - ax

下一步，对于您尝试匹配的每个点，将这些点插入 y 和 x 值。有了 3 个点，就可以得到 b 相对于 a 的 3 个独立函数。

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

接下来，理论是，您会找到穿过每个点的所有可能的线，对于每个单独的点来说，这些线是无限多的，但是当组合在累加器空间中时，只有几个可能的参数最适合。实际上，这是通过选择参数的范围空间（我选择 -2 <= a <= 1, 1 <= b <= 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

参数设置为最高累积值是(ba) = (5 -1)，最适合给定点的函数是y = 5 - x。

祝你好运。

You can find an approximation of a line that intersects points in 3d and 2d space using a Hough Transformation algorithm. I am only familiar with it's uses in 2d however but it will still work for 3d spaces given that you know what kind of line you are looking for. There is a basic implementation description linked. You can Google for pre-mades and here is a link to a 2d C implementation CImg.

The algorithm process (roughly)... First you find equation of a line that you think will best approximate the shape of the line (in 2d parabolic, logarithmic, exponential, etc). You take that formula and solve for one of the parameters.

y = ax + b

becomes

b = y - ax

Next, for each point you are attempting to match, you plugin the points to the y and x values. With 3 points, you would have 3 separate functions of b with respect to a.

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

Next, the theory is that you find all possible lines which pass through each of the points, which is infinitely many for each individual point however when combined in an accumulator space only a few possible parameters best fit. In practice this is done by choosing a range space for the parameters (I chose -2 <= a <= 1, 1 <= b <= 6) and begin plugging in values for the variant parameter(s) and solving for the other. You tally up the number of intersections from each function in an accumulator. The points with the highest values give you your parameters.

Accumulator after processing b = 3 - 2a

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

Accumulator after processing b = 1 - 4a

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

Accumulator after processing b = -5 - 10a

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

The parameter set with the highest accumulated value is (b a) = (5 -1) and the function best fit to the points given is y = 5 - x.

Best of luck.

回复 原文
腻橙味 2024-10-13 08:09:40

我的猜测是，严肃的应用程序将使用 http://en.wikipedia.org/wiki/Kalman_filter 。顺便说一句，这可能也不能保证报告的点相交，除非您稍微修改一下参数。它期望给定的每个数据点都存在某种程度的错误，因此它认为对象在时间 T 的位置不一定是它在时间 T 的位置。当然，您可以设置错误分布来表示您是绝对确定你知道它在时间 T 的位置。

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

Vx_t_报告 = 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 +... 根据您对错误的真正含义以及您想要的答案的平滑程度的想法，并找到使惩罚函数尽可能小 - 如果方程结果良好，则使用最小二乘。

My guess is that a serious application of this would use a http://en.wikipedia.org/wiki/Kalman_filter. By the way, that probably wouldn't guarantee that the reported points were intersected either, unless you fiddled with the parameters a bit. It would expect some degree of error in each data point given to it, so where it thinks the object is at time T would not necessarily be where it was at time T. Of course, you could set the error distribution to say that you were absolutely sure you knew where it was at time T.

Short of using a Kalman filter, I would try and turn it into an optimisation problem. Work at the 1s granularity and write down equations like
x_t' = x_t + (Vx_t + Vx_t')/2 + e,

Vx_t_reported = Vx_t + f,

Vx_t' = Vx_t + g
where e, f, and g represent the noise. Then create a penalty function such as e^2 + f^2 + g^2 +...
or some weighted version such as 1.5e^2 + 3f^2 + 2.6g^2 +... according to your idea of what the errors really are and how smooth you wnat the answer to be, and find the values that make the penalty function as small as possible - with least squares if the equations turn out nicely.

回复 原文
~没有更多了~

关于作者

放低过去

暂无简介

0 文章
0 评论
22 人气
发私信

相关话题

更多

热门标签

操作系统 程序设计 IT运维 Linux系统管理 JavaScript 服务器应用 solaris C/C++ PHP Shell BSD Vue.js aix Oracle Python HTML 系统管理 HTML5 CSS 前端
更多

推荐作者

内心激荡

文章 0 评论 0

JSmiles

文章 0 评论 0

赏烟花じ飞满天

文章 0 评论 0

左秋

文章 0 评论 0

迪街小绵羊

文章 0 评论 0

瞳孔里扚悲伤

文章 0 评论 0

更多

友情链接

文江博客
    我们使用 Cookies 和其他技术来定制您的体验包括您的登录状态等。通过阅读我们的 隐私政策 了解更多相关信息。 单击 接受 或继续使用网站，即表示您同意使用 Cookies 和您的相关数据。
    原文