Permalink
Browse files

First commit.

  • Loading branch information...
0 parents commit c262ac3c5e1a464e4bd6cf334b6a54b6efc1e1a7 @bjornbm 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)
+
+
Oops, something went wrong.

0 comments on commit c262ac3

Please sign in to comment.