1   /* Copyright 2002-2017 CS Systèmes d'Information
2    * Licensed to CS Systèmes d'Information (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.estimation.measurements;
18  
19  import java.util.Arrays;
20  
21  import org.hipparchus.geometry.euclidean.threed.Vector3D;
22  import org.orekit.errors.OrekitException;
23  import org.orekit.propagation.SpacecraftState;
24  import org.orekit.time.AbsoluteDate;
25  import org.orekit.utils.TimeStampedPVCoordinates;
26  
27  /** Class modeling a position-velocity state.
28   * @author Luc Maisonobe
29   * @since 8.0
30   */
31  public class PV extends AbstractMeasurement<PV> {
32  
33      /** Identity matrix, for states derivatives. */
34      private static final double[][] IDENTITY = new double[][] {
35          {
36              1, 0, 0, 0, 0, 0
37          }, {
38              0, 1, 0, 0, 0, 0
39          }, {
40              0, 0, 1, 0, 0, 0
41          }, {
42              0, 0, 0, 1, 0, 0
43          }, {
44              0, 0, 0, 0, 1, 0
45          }, {
46              0, 0, 0, 0, 0, 1
47          }
48      };
49  
50      /** Simple constructor.
51       * <p>
52       * The measurement must be in the orbit propagation frame.
53       * </p>
54       * <p>
55       * This constructor uses 0 as the index of the propagator related
56       * to this measurement, thus being well suited for mono-satellite
57       * orbit determination.
58       * </p>
59       * @param date date of the measurement
60       * @param position position
61       * @param velocity velocity
62       * @param sigmaPosition theoretical standard deviation on position components
63       * @param sigmaVelocity theoretical standard deviation on velocity components
64       * @param baseWeight base weight
65       */
66      public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
67                final double sigmaPosition, final double sigmaVelocity, final double baseWeight) {
68          this(date, position, velocity, sigmaPosition, sigmaVelocity, baseWeight, 0);
69      }
70  
71      /** Simple constructor.
72       * <p>
73       * The measurement must be in the orbit propagation frame.
74       * </p>
75       * @param date date of the measurement
76       * @param position position
77       * @param velocity velocity
78       * @param sigmaPosition theoretical standard deviation on position components
79       * @param sigmaVelocity theoretical standard deviation on velocity components
80       * @param baseWeight base weight
81       * @param propagatorIndex index of the propagator related to this measurement
82       * @since 9.0
83       */
84      public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
85                final double sigmaPosition, final double sigmaVelocity, final double baseWeight,
86                final int propagatorIndex) {
87          super(date,
88                new double[] {
89                    position.getX(), position.getY(), position.getZ(),
90                    velocity.getX(), velocity.getY(), velocity.getZ()
91                }, new double[] {
92                    sigmaPosition, sigmaPosition, sigmaPosition,
93                    sigmaVelocity, sigmaVelocity, sigmaVelocity
94                }, new double[] {
95                    baseWeight, baseWeight, baseWeight,
96                    baseWeight, baseWeight, baseWeight
97                }, Arrays.asList(propagatorIndex));
98      }
99  
100     /** Get the position.
101      * @return position
102      */
103     public Vector3D getPosition() {
104         final double[] pv = getObservedValue();
105         return new Vector3D(pv[0], pv[1], pv[2]);
106     }
107 
108     /** Get the velocity.
109      * @return velocity
110      */
111     public Vector3D getVelocity() {
112         final double[] pv = getObservedValue();
113         return new Vector3D(pv[3], pv[4], pv[5]);
114     }
115 
116     /** {@inheritDoc} */
117     @Override
118     protected EstimatedMeasurement<PV> theoreticalEvaluation(final int iteration, final int evaluation,
119                                                              final SpacecraftState[] states)
120         throws OrekitException {
121 
122         // PV value
123         final TimeStampedPVCoordinates pv = states[getPropagatorsIndices().get(0)].getPVCoordinates();
124 
125         // prepare the evaluation
126         final EstimatedMeasurement<PV> estimated =
127                         new EstimatedMeasurement<>(this, iteration, evaluation, states,
128                                                    new TimeStampedPVCoordinates[] {
129                                                        pv
130                                                    });
131 
132         estimated.setEstimatedValue(new double[] {
133             pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
134             pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ()
135         });
136 
137         // partial derivatives with respect to state
138         estimated.setStateDerivatives(0, IDENTITY);
139 
140         return estimated;
141 
142     }
143 
144 }