# bjornbm/dimensional-experimental

First commit.

• Loading branch information...
0 parents commit c262ac3c5e1a464e4bd6cf334b6a54b6efc1e1a7 committed Apr 8, 2011
Showing with 606 additions and 0 deletions.
1. +7 −0 .gitignore
2. +31 −0 AD.hs
3. +92 −0 G19.hs
4. +52 −0 GM.hs
5. +65 −0 KalmanExample.hs
6. +31 −0 LICENSE
7. +98 −0 PosVel.lhs
8. +41 −0 PosVelTest.hs
9. +10 −0 README
10. +76 −0 VectorAD.hs
11. +63 −0 VectorADTest.hs
12. +40 −0 dimensional-experimental.cabal
 @@ -0,0 +1,7 @@ +*.[oa] +*.exe +*.exe.manifest +*.hi +*~ +*.swp +dist/
31 AD.hs
 @@ -0,0 +1,31 @@ +-- | This module provides automatic differentiation for Quantities. + +{-# LANGUAGE ExistentialQuantification #-} +{-# LANGUAGE Rank2Types #-} + +module AD (diff, Lift (lift)) where + +import Numeric.Units.Dimensional (Dimensional (Dimensional), Quantity, Div) +import Numeric.AD (AD, Mode) +import qualified Numeric.AD (diff, lift) + +-- | Unwrap a Dimensionals numeric representation. +undim :: Dimensional v d a -> a +undim (Dimensional a) = a + +-- | @diff f x@ computes the derivative of the function @f(x)@ for the +-- given value of @x@. +diff :: (Num a, Div d2 d1 d3) + => (forall tag. Mode tag => Quantity d1 (AD tag a) -> Quantity d2 (AD tag a)) + -> Quantity d1 a -> Quantity d3 a +diff f = Dimensional . Numeric.AD.diff (undim . f . Dimensional) . undim + +-- | Class to provide 'Numeric.AD.lift'ing of constant data structures +-- (data structures with numeric constants used in a differentiated +-- function). +class Lift w where + -- | Embed a constant data structure. + lift :: (Num a, Mode t) => w a -> w (t a) + +instance Lift (Dimensional v d) + where lift = Dimensional . Numeric.AD.lift . undim
92 G19.hs
 @@ -0,0 +1,92 @@ +import Numeric.Units.Dimensional.Prelude +import Numeric.Units.Dimensional.LinearAlgebra +import qualified Prelude + + +{- +-- | Convert from LVLH frame to ECI frame, assuming zero inclination. +lvlh2eci scra = rotZ scra `matMat` rotX ((-90)*~degree) `matMat` rotY ((-90)*~degree) + +inLVLH scra mat = lvlh2eci scra `matMat` mat `matMat` transpose (lvlh2eci scra) + +ypys = rotZ (0.081*~degree) `matMat` rotY ((65.125)*~degree) `matMat` rotZ ((-179.833)*~degree) +r = rotX ((1.022)*~degree) + +a1 = ypys `matVec` x +a2 = ypys `matMat` r `matVec` x +a3 = ypys `matVec` z + +-- Desired final attitude. +ra = 1.05 *~ degree +dec = 0.01 *~ degree +scra = 270 *~ degree +--att_f = rotY (negate dec) `matMat` rotZ ra `matMat` lvlh2eci scra `matVec` x +att_f = rotZ (negate dec) `matMat` rotY (negate ra) `matVec` x + +-- Reverse-engineer the initial vector. +att_i = transpose ypys `matVec` att_f +dec_i = acos (y `dotProduct` att_i) - 90*~degree +ra_i = acos (x `dotProduct` att_i) -- For very small dec'. + +-- Compute the problematic final vector and its ra and dec. +att_f' = ypys `matMat` r `matVec` att_i +dec' = acos (y `dotProduct` att_f') - 90*~degree +ra' = acos (x `dotProduct` att_f') -- For very small dec'. + +-- The angle between the desired final vector and the problematic final vector. +err = acos (att_f `dotProduct` att_f') +-- -} + + +-- SEQUENCE OF BODY VECTORS, ROW-WISE +-- (we convert the rows to columns in a matrix) +atti = consCol (vCons (( 0.9177274117e+00)*~one) \$ vCons ((-0.3972104506e+00)*~one) \$ vSing ((-0.5057800831e-03)*~one)) + \$ consCol (vCons ((-0.4759639595e-03)*~one) \$ vCons (( 0.1736481140e-03)*~one) \$ vSing ((-0.9999998717e+00)*~one)) + \$ colMatrix (vCons (( 0.3972104874e+00)*~one) \$ vCons (( 0.9177275346e+00)*~one) \$ vSing ((-0.2969622460e-04)*~one)) + +-- G-19 AMF1 slew sequence. +rotR = rotX (( 1.022)*~degree) -- ES bias. +rot1 = rotZ ((-179.833)*~degree) +rot2 = rotY (( -65.125)*~degree) +rot3 = rotZ ( 0.081 *~degree) + +-- Slew without ES bias. +att1 = atti `matMat` rot1 +att2 = att1 `matMat` rot2 +att3 = att2 `matMat` rot3 +z3 = att3 `matVec` z + +-- Slew with ES bias. +attiR = atti `matMat` rotR +att1R = attiR `matMat` rot1 +att2R = att1R `matMat` rot2 +att3R = att2R `matMat` rot3 +z3r = att3R `matVec` z + +-- Diff between the two. +err = acos (z3 `dotProduct` z3r) + +dec v = 90*~degree - acos (v `dotProduct` z) +ra v = acos (v `dotProduct` x) -- only for very small declinations +-- 0.9996655998D+00 0.2566988705D-01 -0.3121784926D-02 +z_ideal = vCons ((0.9996655998e+00)*~one) \$ vCons ((0.2566988705e-01)*~one) \$ vSing ((-0.3121784926e-02)*~one) + + + +--8<------- From G19_AMF3.hs ---------8<------------------ + +-- minus x +negX = rotZ (180*~degree) `matVec` x + +sunECI = rotY sunDec `matMat` rotZ sunRA `matVec` x where + sunRA = 184.965 *~ degree + sunDec = (-2.149) *~ degree +sunECI' = rotZ sunRA `matMat` rotY sunDec `matVec` x where + sunRA = 184.965 *~ degree + sunDec = (-2.149) *~ degree + +ecassSC = rotY pitchBias `matMat` rotZ yawBias `matVec` negX where + pitchBias = (-15.038) *~ degree + yawBias = ( -0.24 ) *~ degree + +
52 GM.hs
 @@ -0,0 +1,52 @@ +import Numeric.Units.Dimensional.Prelude +import Vector +import PosVel +import qualified Prelude + + +posvel = ( fromTuple ( 4383.9449203752 *~ kilo meter :: Length Double + , (-41940.917505092) *~ kilo meter :: Length Double + , 22.790255916589 *~ kilo meter :: Length Double + ) + , fromTuple ( 3.0575666627812 *~ (kilo meter / second) :: Velocity Double + , 0.32047068607303 *~ (kilo meter / second) :: Velocity Double + , 0.00084729371755294 *~ (kilo meter / second) :: Velocity Double + ) + ) + +semiMajorAxis :: RealFloat a => GravitationalParameter a -> CPosVel a -> Length a +semiMajorAxis mu (pos, vel) = negate mu / _2 / (e_pot + e_kin) + where + r = vNorm pos + v = vNorm vel + e_pot = negate mu / r + e_kin = v^pos2 / _2 + +mu_FDS = 398600.44 *~ (kilo meter^pos3 / second^pos2) +mu_STA = 398601.19 *~ (kilo meter^pos3 / second^pos2) + + +{- +-- | Approximation of geostationary semi-major axis. +semiMajorAxisGEO mu = cbrt (mu / phi^pos2) + where + phi = _2 * pi / (1*~day) -- Approximation of Earth's rotation rate. +-} + +printAs :: Fractional a => String -> Quantity d a -> Unit d a -> IO () +printAs desc val unit = putStrLn \$ desc ++ ": " ++ show (val/~unit) + +sma_FDS = semiMajorAxis mu_FDS posvel +sma_STA = semiMajorAxis mu_STA posvel + +main = do + --print "GEO Semi-major axis in FDS [km]:" + --print \$ semiMajorAxisGEO mu_FDS /~ kilo meter + --print "GEO Semi-major axis in STA [km]:" + --print \$ semiMajorAxisGEO mu_STA /~ kilo meter + printAs "Semi-major axis in FDS [km]" sma_FDS (kilo meter) + printAs "Semi-major axis in STA [km]" sma_STA (kilo meter) + let dsma = sma_FDS - sma_STA + printAs "Difference [km]" dsma (kilo meter) + printAs "Drift rate difference [deg / day]" ((-0.0128)*~(degree / day / kilo meter) * dsma) (degree / day) +
 @@ -0,0 +1,65 @@ +{-# LANGUAGE NoMonomorphismRestriction#-} + + +module KalmanExample where + +import qualified Prelude +import Data.HList +import Numeric.Units.Dimensional.Prelude +import Vector +import Matrix hiding (x,y,z) + +-- Types. +type State = Vec (HCons DLength (HCons DVelocity HNil)) +type Obs = Vec (HCons DLength HNil) +type Control = Vec (HCons DAcceleration HNil) + + +-- The process. +f dt = fromRowHLists \$ (_1 .*. dt .*. HNil) + .*. (0 *~ second ^ neg1 .*. _1 .*. HNil) + .*. HNil + +g dt = colMatrix \$ fromHList \$ dt ^ pos2 / _2 + .*. dt + .*. HNil + +--q dt = (sigma_a ^ pos2) `scaleMat` gg where gg = colMatrix (g dt) `matMat` rowMatrix (g dt) + +-- Updating. +x :: Fractional a => State a -> Control a -> Time a -> State a +x x_old a dt = (f dt `matVec` x_old) `elemAdd` (g dt `matVec` a) + +p p_old dt = (f dt `matMat` p_old) `matMat` transpose (f dt) + +-- Observation. +z :: Num a => State a -> Obs a -> Obs a +z x v = matVec h x `elemAdd` v +h = rowMatrix \$ fromHList \$ _1 .*. 0 *~ second .*. HNil + +r = rowMatrix \$ vSing \$ sigma_z ^ pos2 + +-- Initial state and covariance. +x_0 :: Fractional a => State a +x_0 = fromHList \$ 0 *~ meter + .*. 0 *~ (meter / second) + .*. HNil +p_0 = fromRowHLists \$ (b *~ meter ^ pos2 .*. 0 *~ (meter ^ pos2 / second) .*. HNil) + .*. (0 *~ (meter ^ pos2 / second) .*. b *~ (meter ^ pos2 / second ^ pos2) .*. HNil) + .*. HNil where b = 0 + +-- Some simulation test values. +dt_1 = 1.0 *~ second -- Time step. +a_1 = vSing \$ 0.01 *~ (meter / second ^ pos2) -- Acceleration (noisy but not noise!). +sigma_a = 0.01 *~ (meter / second ^ pos2) +v_1 = vSing \$ 0.01 *~ meter -- Measurement noise. +sigma_z = 0.001 *~ meter + +-- Some updates... +x_1 = x x_0 a_1 dt_1 +p_1 = p p_0 dt_1 +z_1 = z x_1 v_1 + +y_1 = z_1 `elemSub` matVec h x_1 -- Innovation. +s_1 = h `matMat` p_0 `matMat` transpose h `mElemAdd` r +
31 LICENSE
 @@ -0,0 +1,31 @@ +Copyright (c) 2008, Bjorn Buckwalter. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + + * Neither the name of the copyright holder(s) nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE.
 @@ -0,0 +1,98 @@ + +The convention in this module is that a @C@ denotes cartesian coordinates and an @S@ denotes spherical coordinates. + +> {-# OPTIONS_GHC -fglasgow-exts #-} +> {-# LANGUAGE NoMonomorphismRestriction #-} + +> module PosVel where + +> import qualified Prelude +> import Numeric.Units.Dimensional.Prelude +> import Numeric.Units.Dimensional (Dimensional (Dimensional)) +> import Numeric.Units.Dimensional.LinearAlgebra +> import Data.HList +> import VectorAD (applyLinear) + + +Type synonyms for clearer documentation. + +> type DRadius = DLength; type Radius = Length +> -- | Angle from Z-axis. +> type DZenith = DPlaneAngle; type Zenith = Angle +> -- | Angle from X-axis towards Y-axis (positive about Z). +> type DRightAscension = DPlaneAngle; type RightAscension = Angle + + +Some type synonyms for convenience. + +> type Vec3 d1 d2 d3 = Vec (d1:*:d2:*.d3) +> type CPos = Vec3 DLength DLength DLength -- ^ x y z +> type CVel = Vec3 DVelocity DVelocity DVelocity +> type SPos = Vec3 DRadius DZenith DRightAscension +> type SVel = Vec3 DVelocity DAngularVelocity DAngularVelocity + +Data type combining position and velocity into a state vector (minus epoch). + +> type CPosVel a = (CPos a, CVel a) +> type SPosVel a = (SPos a, SVel a) + + +Querying +-------- + +Cartesian position. + +> x :: CPos a -> Length a +> x = vElemAt zero +> y :: CPos a -> Length a +> y = vElemAt pos1 +> z :: CPos a -> Length a +> z = vElemAt pos2 + +Spherical position. + +> radius :: SPos a -> Radius a +> radius = vElemAt zero + +> zenith :: SPos a -> Zenith a +> zenith = vElemAt pos1 +> colatitude = zenith +> polarAngle = zenith +> latitude s = pi / _2 - colatitude s +> declination = latitude + +> rightAscension :: SPos a -> RightAscension a +> rightAscension = vElemAt pos2 +> longitude = rightAscension +> hourAngle = rightAscension + + +Converting +---------- +Converts a cartesian position vector into a spherical position vector. + +> c2s :: RealFloat a => CPos a -> SPos a +> c2s c = fromTuple (r, zen, az) +> where +> (x, y, z) = toTuple c +> r = vNorm c -- sqrt (x^pos2 + y^pos2 + z^pos2) +> zen = if r == 0 *~ meter then _0 else acos (z / r) +> az = atan2 y x + +> c2sEphem :: RealFloat a => CPosVel a -> SPosVel a +> c2sEphem = applyLinear c2s -- unlinearize (c2s . linearize c :: RealFloat b => Time b -> SPos b) + +Converts a spherical position vector into a cartesian position vector. + +> s2c :: Floating a => SPos a -> CPos a +> s2c s = fromTuple (x, y, z) +> where +> (r, zen, az) = toTuple s +> x = r * sin zen * cos az +> y = r * sin zen * sin az +> z = r * cos zen + +> s2cEphem :: RealFloat a => SPosVel a -> CPosVel a +> s2cEphem = applyLinear s2c -- unlinearize (s2c . linearize s :: RealFloat b => Time b -> CPos b) + +

#### 0 comments on commit `c262ac3`

Please sign in to comment.