Download or view inertialNav.frink in plain text format
/** This program contains routines to process, integrate, graph, and curve-fit
data for inertial navigation.
*/
use Grid.frink
use LeastSquares.frink
headerRows = 2
timestep = 0.5 s
grid = new array
for line = readLines["file:inertialdata01.txt.csv"]
{
line = split["\t",line]
grid.push[line]
}
ax = mulvec[eval[rest[grid.getColumn[8], headerRows]], m/s^2]
ay = mulvec[eval[rest[grid.getColumn[9], headerRows]], m/s^2]
az = mulvec[eval[rest[grid.getColumn[10], headerRows]],m/s^2]
vx = sumAccel[ax, timestep]
vy = sumAccel[ay, timestep]
vz = sumAccel[az, timestep]
dx = sumVelocity[vx, timestep]
dy = sumVelocity[vy, timestep]
dz = sumVelocity[vz, timestep]
xvalues = makeTimes[dx, timestep, 0 s]
basisFuncs = noEval[[x]]
ls = new LeastSquares[xvalues, vy, basisFuncs]
println[ls.toMatrix[].formatMatrix[]]
println[ls.toExpressionString["t"]]
println["Residual: " + ls.residual[]]
println["r-value: " + ls.rValue[]]
fitfunc = ls.toFunction[]
lsdy = new LeastSquares[xvalues, dy, noEval[[x^2, x]]]
//graphVel[vx, timestep, "x"]
graphVel[vy, timestep, fitfunc]
graphDist[dx, timestep]
graphDist[dy, timestep, lsdy.toFunction[]]
graphVel[c, timestep, func=undef] :=
{
g = new graphics
t0 = 0 s
p = new polyline
pp = new polyline
for i = rangeOf[c]
{
t = i*timestep
p.addPoint[t, -(c@i)]
if func != undef
pp.addPoint[t, -func[t]]
}
g.add[p]
g.color[0,0,1,.5]
g.add[pp]
g.color[0,0,0]
grid = new Grid
grid.setUnits[s, -m/s]
grid.auto[g]
g.add[grid.getGrid[]]
g.show[]
}
graphDist[c, timestep, func = undef] :=
{
g = new graphics
t0 = 0 s
p = new polyline
pp = new polyline
for i = rangeOf[c]
{
t = i*timestep
d = c@i / m
p.addPoint[t, -d]
if func != undef
pp.addPoint[t, -func[t]/m]
}
g.add[p]
g.color[0,0,1,.5]
g.add[pp]
g.color[0,0,0,.5]
grid = new Grid
grid.setUnits[s, -1]
grid.auto[g]
g.add[grid.getGrid[]]
g.show[]
}
sumAccel[a, timestep, v0 = 0 m/s] :=
{
lastV = v0
ret = new array
for i = rangeOf[a]
{
lastV = lastV + timestep * a@i
ret.push[lastV]
}
return ret
}
sumVelocity[v, timestep, d0 = 0 m] :=
{
lastD = d0
ret = new array
for i = rangeOf[v]
{
lastD = lastD + timestep * v@i
ret.push[lastD]
}
return ret
}
mulvec[vec, scalar] :=
{
ret = new array
for i = rangeOf[vec]
{
v = vec@i
if isUnit[v]
ret.push[vec@i * scalar]
}
return ret
}
makeTimes[vec, timestep, t0] :=
{
lastT = t0
ret = new array
for i = rangeOf[vec]
{
lastT = lastT + timestep
ret.push[lastT]
}
return ret
}
/** Function to turn a spreadsheet column designator into a column index.
For example, this turns column name "A" to 0 and column name "Z" to 25 and
column name "AA" to 26.
*/
ssColumnToIndex[name] :=
{
name = uc[trim[name]]
ret = 0
len = length[name]
pow = 1
for i = len-1 to 0 step -1
{
ret = ret + (char[substrLen[name, i, 1]] - char["A"] + 1) * pow
pow = pow * 26
}
return ret-1 // We want zero-based column number.
}
Download or view inertialNav.frink in plain text format
This is a program written in the programming language Frink.
For more information, view the Frink
Documentation or see More Sample Frink Programs.
Alan Eliasen was born 19976 days, 4 hours, 55 minutes ago.