对于每个点,我们知道:日期和时间、纬度、经度、海拔、速度和航向。数据可能每10秒记录一次,我们希望能够估计中间的点并将粒度增加到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)
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中,即可得到基于初始和最终位置和速度的点的时间插值。
希望对你有所帮助!
编辑
以下是在二维空间中出现了突然速度变化的几个例子(在三维情况下完全相同)。如果初始速度和最终速度相似,则路径会更加“直”。
假设:
X0 = {0, 0}; Xf = {1, 1};
T0 = 0; Tf = 1;
V0 = {0, 1}; Vf = {-1, 3};
V0 = {0, 1}; Vf = {-1, 5};
V0 = {0, 1}; Vf = {1, 3};
这里有一个动画,您可以看到速度从V0 = {0, 1}变化到Vf = {1, 5}:
这里您可以看到一个在3D中加速的物体,位置在等间隔时间内被记录:
编辑
一个完整的问题:
为了方便起见,我将使用笛卡尔坐标系。如果您想将经纬度/高度转换为笛卡尔坐标,请执行以下操作:
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 = {-(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)}
编辑2 如果您不想干扰垂直加速度(也许因为您的“速度计”没有读取它),您可以将一个恒定速度分配给z轴(考虑到它与Rho轴平行,存在非常小的误差),等于(Zfinal-Zinit)/(Tf-T0),然后在平面上解决问题,忽略高度。
线性插值很容易。给定一个任意数量的时间点,这些时间点在你的系统中以误差n内的一条直线拟合,构建该直线并计算每个点之间的时间距离。从这里开始,尝试将时间点拟合到以下两种情况之一:恒定速度或恒定加速度。
曲线插值有点困难,但仍然是可行的。对于俯仰、偏航或组合俯仰+偏航的情况,在时间上包含任意数量的点,这些点在你的系统中以误差m为曲线读数。从这些数据中构建平面曲线,再次考虑沿曲线的恒定速度或加速度。
通过尝试预测飞机在决策树或神经网络中相对于飞行路径的预期操作,您可以比这做得更好。我将把这留给读者作为练习。
祝你设计系统顺利。
--
* 根据问题的描述,预计两个错误读数都来自GPS数据。对这些数据中的错误进行核算和调整是一个单独而有趣的问题。
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
}
我猜测,严肃的应用会使用http://en.wikipedia.org/wiki/Kalman_filter。顺便说一下,除非你稍微调整一下参数,否则这可能也不能保证报告的点相交。它会期望在给定的每个数据点中存在一定程度的误差,因此它认为对象在时间T的位置不一定是它在时间T时所在的位置。当然,您可以将误差分布设置为表明您绝对确定了在时间T时它所在的位置。
除了使用卡尔曼滤波器之外,我会尝试将其转化为优化问题。以1秒为粒度工作,并编写以下方程式:
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 + ...根据您对错误的真实性质以及您希望答案平滑程度的想法,并找到使惩罚函数尽可能小的值-如果方程式很好,则使用最小二乘。
您可以使用Hough变换算法在3D和2D空间中找到穿过点的近似直线。然而,我只熟悉它在2D中的用途,但是如果您知道要查找什么类型的直线,则它仍将适用于3D空间。有一个基本的实现描述链接。您可以搜索预制品,这里是一个2D C实现的链接CImg。
算法流程(大致)...首先,您找到一个方程式,该方程式最能近似线条的形状(在2D中为抛物线,对数,指数等)。您采用该公式并解决其中一个参数。
y = ax + b
变成
b = y - ax
(2, 3) : b = 3 - 2a
(4, 1) : b = 1 - 4a
(10, -5): b = -5 - 10a
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
。
祝你好运。