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 19979 days, 18 hours, 39 minutes ago.