Open Collections

UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Antenna boresight calibration using optimal estimation techniques Christianson, John G. 1993

Your browser doesn't seem to have a PDF viewer, please download the PDF to view this item.

Item Metadata

Download

Media
831-ubc_1993_fall_christianson_john.pdf [ 8.39MB ]
Metadata
JSON: 831-1.0065098.json
JSON-LD: 831-1.0065098-ld.json
RDF/XML (Pretty): 831-1.0065098-rdf.xml
RDF/JSON: 831-1.0065098-rdf.json
Turtle: 831-1.0065098-turtle.txt
N-Triples: 831-1.0065098-rdf-ntriples.txt
Original Record: 831-1.0065098-source.json
Full Text
831-1.0065098-fulltext.txt
Citation
831-1.0065098.ris

Full Text

ANTENNA BORESIGHT CALIBRATIONUSING OPTIMAL ESTIMATION TECHNIQUESbyJOHN GORDON CHRISTIANSONB.A.Sc., The University of British Columbia, 1975A THESIS SUBMITTED IN PARTIAL FULFILLMENT OFTHE REQUIREMENTS FOR THE DEGREE OFMASTER OF APPLIED SCIENCEinTHE FACULTY OF GRADUATE STUDIES(Department of Electrical Engineering)We accept this thesis as conformingt he required standardTHE UNIVERSITY OF BRITISH COLUMBIASeptember 1993John Gordon Christianson, 1993In presenting this thesis in partial fulfilment of the requirements for an advanceddegree at the University of British Columbia, I agree that the Library shall make itfreely available for reference and study. I further agree that permission for extensivecopying of this thesis for scholarly purposes may be granted by the head of mydepartment or by his or her representatives. It is understood that copying orpublication of this thesis for financial gain shall not be allowed without my writtenpermission.(Signature)Department of Electrical EnctineerinqThe University of British ColumbiaVancouver, CanadaDate  September 28, 1993DE-6 (2/88)ABSTRACTAs part of the Canadian Department of National Defence Spotlight SAR program, the DefenceResearch Establishment Ottawa (DREO) has been developing a Synthetic Aperture Radar MotionCompensation System (SARMCS). The SARMCS processes accelerometer and gyromeasurements from an inertial measurement unit (IMU) to estimate and compensate for radarantenna motion. Uncompensated azimuth misalignment of the antenna boresight with respectto the IMU is a significant contributor to motion compensation error. Currently, an awkwardto perform, laser based antenna boresight calibration procedure is employed.This thesis investigates an alternative approach in which the antenna azimuth misalignment isestimated in-flight by means of a Kalman filter. The filter integrates data from an inertialnavigation system (INS), a Global Positioning System receiver (GPS) and the IMU to maintainposition, velocity and orientation information. During calibration, measurements of the antennaazimuth angle to a fixed radar target are provided by a Target Acquisition Unit (TAU). Ameasurement of antenna azimuth misalignment is obtained by comparing the TAU-determinedantenna azimuth with a computed value based on the IMU position and heading and the knowntarget location. Several of these measurements are processed by the Kalman filter to obtain anaccurate estimate of the antenna azimuth misalignment.The current SARMCS Kalman filter integrates INS, GPS and IMU data for navigation andmotion compensation purposes. Formulation of an antenna boresight calibration Kalman filteris discussed with the objective of augmenting the SARMCS filter for antenna calibration. Muchof the investigation involves simulations to determine a flight trajectory and aircraft/targetgeometry that will allow the filter to accurately estimate antenna misalignment in a minimumamount of time. A computer simulation software package, developed to support this effort, isdescribed and the analysis methods and results of the simulations are presented. It is shown thattwo horizontal manoeuvres a few minutes apart followed by a period of TAU measurementswhile flying directly toward the target is effective. A performance evaluation using synthesizedsensor data provided by DREO suggests that calibration to an accuracy of better than 4 arcminutes rms within a 20 minute period is feasible.TABLE OF CONTENTSAbstractTable of Contents^ ivList of Tables viiList of Figures^ viiiAcronyms and AbbreviationsNomenclature^ xiAcknowledgement xivSection One^Introduction^ 11.1 Proposed Calibration Technique  ^41.2 Scope of this Thesis  ^6Section Two^Review of Navigation Concepts^ 82.1 Frames of Reference and Coordinate Systems  ^82.1.1 Earth-Centred Inertial Frame  ^82.1.2 Geocentric Earth-Fixed Frame  ^82.1.3 Geographic North-East-Down Frame  ^92.1.4 Wander Azimuth Navigation Frame  ^92.1.5 Vehicle Body Frame  ^102.1.6 Stabilized Antenna Ring Gear Frame  ^112.1.7 IMU Body Frame  ^112.2 The Earth Model  ^112.2.1 Shape  ^122.2.2 Rotation  ^162.2.3 Gravity  ^162.2.4 World Geodetic System  ^17iv2.3 Bearing Calculation  ^182.4 Inertial Navigation  ^232.4.1 Navigation in Wander Azimuth Coordinates  ^232.4.2 Attitude Computation  ^282.4.3 Vertical Channel Mechanization  ^292.5 Inertial Navigation Error Analysis  ^32Section Three Kalman Filter Formulation^ 453.1 The SARMCS Kalman Filter  ^463.2 Antenna Azimuth Misalignment Estimation  ^633.3 Discrete Time Computation  ^703.4 Feedback Configuration  ^73Section Four^Simulation Software^ 764.1 Trajectory Generation  ^774.1.1 The Command File  ^784.1.2 Trajectory Calculation  ^814.2 Sensor Data Synthesis  ^834.2.1 INS Data Synthesis  ^854.2.2 IMU Data Synthesis  ^864.2.3 GPS Data Synthesis  ^874.2.4 Azimuth Encoder Data Synthesis  ^884.2.5 Baroaltitude Data Synthesis  ^904.3 Data Processing  ^904.4 Data Analysis  ^93Section Five^Flight Trajectory and Aircraft/Target Geometry^965.1 Preliminary Considerations  ^965.2 Simulation Methods  ^975.3 Simulation Results  ^1015.3.1 Flight Profiles  ^102vi5.3.2 Simulation Run la  ^1065.3.3 Simulation Run 2a  ^1115.3.4 Simulation Run 3a  ^1155.3.5 Summary  ^118Section Six^Performance Evaluation^ 1216.1 Synthesized SARMCS Sensor Data  ^1226.2 Initial Performance Evaluation  ^1286.2.1 Simulation Run 4a  ^1306.2.2 Simulation Run 4b  ^1346.3 Increased Kalman Filter Update Interval  ^1376.3.1 Simulation Run 4c  ^1386.3.2 Simulation Run 4d  ^1416.4 Separated INS/GPS Kalman Filter  ^1426.4.1 Simulation Run 4e  ^146Section Seven Conclusions and Reconunendations^ 1517.1 Kalman Filter Design  ^1517.2 Test Methods  ^1527.3 Flight Profile and Aircraft/Target Geometry  ^1547.4 Antenna Boresight Calibration Performance  ^155References^ 157Appendix A^Flight Profile Simulation Plots^ 159Appendix B^Performance Evaluation Simulation Plots^ 208Appendix C^Software Listings^ 229LIST OF TABLES2-1 WGS 84 Constants ^ 183-1 SARMCS Kalman Filter States ^ 493-2 Operations Performed at Each Kalman Filter Update Time ^ 754-1 Typical Trajectory Generation Command File ^ 794-2 Sensor Data Synthesis Programs ^ 844-3 Basic Kalman Filter Using System Model and Generic Kalman FilterProcedures ^ 925-1 Process Noise Spectral Density and Initial Error Covariance ^ 995-2 Measurement Noise Covariance ^ 1005-3 Flight Profile 1 Command File 1035-4 Flight Profile 2 Command File 1045-5 Flight Profile 3 Command File ^ 1056-1 INS Data (at 16 Hz) ^ 1226-2 IMU Data (at 50 Hz) 1236-3 GPS Data (at 1 Hz) 1236-4 SARMCS Lever Arms ^ 1246-5 Initial INS System Errors 1256-6 INS Accelerometer and Gyro Errors ^ 1256-7 IMU Accelerometer and Gyro Errors 126viiLIST OF FIGURES1-1 Location of Motion Compensation Sensors on Convair 580 ^ 21-2 SAR Antenna and Strapdown IMU ^ 31-3 Plan View of Azimuth Angle Relationships ^ 42-1 Relationship Between Earth-Fixed Frame and Wander Azimuth Frame 102-2 Relationship Between Physical Surface, Geoid and Ellipsoid ^ 122-3 The Ellipse ^ 132-4 Bearing Plane Viewed from an Aircraft Above Point A ^ 192-5 Target Meridian Plane ^ 202-6 Aircraft Meridian Plane 202-7 Mechanization of Strapdown Inertial Navigation System ^ 302-8 Third Order Baro-inertial Loop ^ 312-9 Mechanization of Vertical Channel Baro-inertial Loop ^ 312-10 Relationships Between Inertial Navigation Error Analysis Frames ^ 322-11 Inertial Navigation Error Dynamics Matrix ^ 443-1 Elements of Fm and Fs ^ 514-1 Block Diagram of Simulation Package ^ 774-2 Flight Profile ^ 834-3 Azimuth Encoder Measurements ^ 894-4 Azimuth Encoder Measurements Showing TAU Error ^ 894-5 Difference Between Computed IMU Ground Speed and ReferenceGround Speed ^ 945-1 Flight Profile 1 1035-2 Flight Profile 2 ^ 1045-3 Flight Profile 3 1055-4 Error in Corrected IMU Position for Simulation Run la ^ 1095-5 Error in Corrected z IMU Platform Alignment for Simulation Run la 1105-6 Error in z IMU Gyro Bias Estimate for Simulation run la ^ 1105-7 Error in Antenna Azimuth Misalignment Estimate forSimulation Run 2a ^ 113viiiix5-8 Antenna Azimuth Matching Measurement Residual ^ 1145-9 Antenna Azimuth Matching Kalman Gain for Simulation Run 2a ^ 1145-10 Error in Corrected IMU Position for Simualtion Run 2a ^ 1155-11 Error in Antenna Azimuth Misalignment Estimate forSimulation Run 3a ^ 1176-1 Performance Evaluation Flight Profile ^ 1276-2 Error in Antenna Azimuth Misalignment Estimate forSimulation Run 4a ^ 1316-3 Qunatization Noise in y IMU/INS Position Matching Measurement Residual 1326-4 Error in Corrected z IMU Platform Alignment for Simulation Run 4a 1336-5 Error in Corrected IMU Position for Simulation Run 4a ^ 1346-6 Error in Antenna Azimuth Misalignment Estimate forSimulation Run 4b ^ 1356-7 Error in Corrected z IMU Platform Alignment for Simulation Run 4b 1366-8 Error in Corrected IMU Position for Simulation Run 4b ^ 1376-9 Error in Antenna Azimuth Misalignment Estimate forSimulation Run 4c ^ 1396-10 Error in Corrected IMU Position for Simulation Run 4c ^ 1406-11 Error in Corrected z IMU Platform Alignment for Simulation Run 4c 1406-12 Error in Antenna Azimuth Misalignment Estimate forSimulation Run 4d ^ 1426-13 Error in Antenna Azimuth Misalignment Estimate forSimulation Run 4e ^ 1476-14 Error in Corrected IMU Position for Simulation Run 4e ^ 1486-15 Error in Corrected z IMU Platform Alignment for Simulation Run 4e 1486-16 Antenna Azimuth Matching Kalman Gain for Simulation Run 4e ^ 149ACRONYMS AND ABBREVIATIONSDREO^Defence Research Establishment OttawaGPS^Global Positioning SystemIMU^Inertial Measurement UnitINS^Inertial Navigation SystemLOS^Line-of-sightSAR^Synthetic Aperture RadarSARMCS^Synthetic Aperture Radar Motion Compensation SystemTAU^Target Acquisition UnitWGS 84^World Geodetic System 1984NOMENCLATUREa^wander angle(P^geodetic latitudeX^longitudeh^altitude0^aircraft pitch4)^aircraft roll0^aircraft headingazimuth (platform heading)13^bearinge^antenna boresight azimuth misalignment6^antenna boresight azimuthx, y, z^coordinate system axis designationsa^ellipsoid semimajor axisb^ellipsoid semiminor axisf^ellipsoid flatteninge^ellipsoid first eccentricityC4Iie^earth rotation rateGM^earth gravitational constant7^theoretical gravity at surface of ellipsoidYe^theoretical gravity at equator1 ' p^theoretical gravity at polesxixiiradius of curvature in plane of meridianradius of curvature in plane of prime verticalRx, Ry, T general radii of curvature termsgravityAg^gravity anomaly11^small angle vertical deflection of gravity vectorposition vector (also measurement noise covariance matrix)V^velocity vectorspecific force vectorgravity vectorWab^vector angular velocity of b frame with respect to a framedirection cosine matrix which transforms a vector from a coordinates to b coordinatesflab^skew symmetric matrix representation of [wab x] operatorposition error vectorvelocity error vectorsmall angle platform frame misalignment vectorSO^small angle computer frame misalignment vectorSf^accelerometer specific force measurement error vectorgyro angular velocity measurement error vectorstate vectorcontinuous time system dynamics matrixprocess noise vectorprocess noise spectral density matrixXk^discrete time state vectordiscrete time state transition matrixWk^discrete time process noise vectorQk^process noise covariance matrixmeasurement vectormeasurement matrixmeasurement noise vectormeasurement noise covariance matrix (also position vector)xivACKNOWLEDGEMENTThe idea of antenna boresight calibration using optimal estimation techniques was suggested tome as a thesis topic by Mr. D.J. DiFilippo of the Defence Research Establishment Ottawa(DREO). The work was supported by DSS contract number W7714-2-9649/01-ST.I wish to thank Dr. M.R. Ito, my supervisor at the University of British Columbia, for hissupport and especially for his efforts in obtaining and administering the contract with DREO.I also thank my external supervisor Mr. D.J. DiFilippo for his guidance and for preparing thesynthesized sensor data used in the antenna boresight calibration performance evaluation.1SECTION ONEINTRODUCTIONThe Canadian Department of National Defence is developing a high resolution airborne spotlightsynthetic aperture radar (SAR) (Haslam, Vant and DiFilippo 1988). As part of the SpotlightSAR program, the Defence Research Establishment Ottawa (DREO) has been involved in thedevelopment of a SAR Motion Compensation System (SARMCS) (Hepburn et al. 1984;DiFilippo, Haslam and Widnall 1988). This system compensates SAR signal returns for theeffects of aircraft motion during the imaging interval. The SARMCS design incorporates a smallstrapdown inertial measurement unit (IMU), mounted directly on the radar antenna structure,to measure antenna motion. Raw accelerometer and gyro measurements are processed toestimate the motion of the antenna phase centre along the radar line-of-sight (LOS). Currently,an experimental version of the SARMCS is installed on board the National AeronauticalEstablishment Convair 580 research aircraft. Figure 1-1 shows the locations of the motioncompensation sensors on the aircraft. Since it is intended that the new antenna boresightcalibration technique described herein will ultimately be incorporated into the SARMCS as aspecial operating mode, these sensors will also be used for the antenna boresight calibration.Figure 1-2 depicts the SAR antenna and the strapdown IMU. In the figure, the point 0represents the centre of the IMU accelerometer triad and the directions x, y and z are theaccelerometer input axes. Antenna azimuth is defined as the angle 6 between the input axis ofthe forward pointing accelerometer and the direction of the antenna boresight. Measurementsof this angle are provided by an azimuth encoder on the antenna structure. In general, there isa fixed error component associated with the measurements due to mechanical misalignment ofGPS antennaIMUSAR antennastructureINSFigure 1-1 Location of Motion Compensation Sensors on Convair 580the antenna boresight with respect to the strapdown IMU mount. Antenna boresight calibrationrefers to the determination of this azimuth misalignment.An analysis of the error in the phase centre LOS displacement (DiFilippo, Haslam and Widnall1988) indicates the importance of accurately resolving the phase centre acceleration, as measuredby the strapdown IMU accelerometers, along the radar LOS. Uncompensated azimuthmisalignment of the antenna boresight with respect to the IMU is a significant contributor to theerror. The SARMCS error budget specifies that the antenna azimuth be calibrated to anaccuracy of 5 arc minutes rms or better. To achieve this level of accuracy, the fixed mechanicalmisalignment must be estimated and used to correct the azimuth encoder measurements.23yz pitchaxis1)% boresightSARantennaazimuthaxisrollaxisFigure 1-2 SAR Antenna and Strapdown IMUCurrently, a laser based antenna boresight calibration procedure is employed. While theprocedure provides the required accuracy, it is awkward to perform. Firstly, it requires removalof the SAR antenna structure from the aircraft and attachment of a small laser in place of theIMU. Secondly, the entire structure must be mounted atop a 20 metre tower and operated tolock the antenna onto a radar transponder target located a distance of 5.5 kilometres away.Finally, the azimuth misalignment is estimated by comparing azimuth encoder measurementswith measurements obtained by laser sighting. Although this calibration need only be performedinfrequently, it is still too cumbersome to be an operational procedure. This thesis investigatesan alternative approach in which the antenna azimuth misalignment is estimated in-flight bymeans of a Kalman filter.Northaircraftheading41.1 PROPOSED CALIBRATION TECHNIQUEThe proposed calibration technique uses a Kalman filter to estimate the antenna azimuthmisalignment. Figure 1-3 is a plan view of the azimuth angle relationships relevant to thisdiscussion.aircrafttargetFigure 1-3 Plan View of Azimuth Angle RelationshipsWhile in flight, the aircraft position and heading are computed by processing IMU measurementsin a strapdown inertial navigation algorithm. Inertial navigation errors are controlled byprocessing measurements based on comparing Global Positioning System (GPS) (Milliken andZoller 1978; Zachmann 1988) and inertial information in the Kalman filter. During calibration,a radar transponder, situated at a well surveyed fixed location on the ground, is used to generatepoint target returns. The radar incorporates a Target Acquisition Unit (TAU) which can detect5the point target, by processing radar returns, and measure the corresponding antenna azimuthangle using the azimuth encoder. The TAU provides a measurement of the azimuth angle tothe target and at the same time, a value for the angle is computed aswhere i3 is the bearing of the target from north, computed from the strapdown navigatorindicated position and the surveyed location of the target, and is the strapdown navigatorindicated heading. A measurement of the antenna azimuth error is now obtained as8=6TAu-6c (1-2)where OTAu is the TAU-determined antenna azimuth and /9c is the computed value based on thestrapdown IMU inertial navigation quantities and the known target location. By processing anumber of these measurements in the Kalman filter, an accurate estimate of the antenna azimuthmisalignment is achieved.A further consideration is the possibility of losing the GPS information for short periods (forexample, the GPS antenna may be in the shadow of the fuselage during a turn). The high driftcharacteristics of the small IMU gyros make this of particular concern. In order to ensure arobust system, the proposed calibration Kalman filter includes data from an inertial navigationsystem (INS) with low drift gyros to maintain stability during those short periods of GPSunavailability.61.2 SCOPE OF THIS THESISThe proposed antenna boresight calibration technique offers the potential of a simple procedurefor accurate, in-flight, real-time calibration. This thesis investigates the proposed technique todetermine if the anticipated benefits can be realized. The objectives of the investigation are to:• Formulate an antenna boresight calibration Kalman filter to augment the currentSARMCS Kalman filter.• Determine an effective flight profile and aircraft/target geometry for antennaboresight calibration.• Evaluate the resulting antenna calibration performance in terms of misalignmentestimation accuracy and required calibration time.The first task in the investigation is a review the navigation concepts applicable to bearingcalculations and inertial navigation on the earth. Results important to the present work aredescribed in Section 2.Section 3 describes the formulation of the Kalman filter. This task begins with an examinationof the current SARMCS Kalman filter. It is shown that the SARMCS filter can be augmentedfor antenna calibration with a random constant state representing antenna azimuth misalignment,and a new antenna azimuth matching measurement. A linearized antenna azimuth matchingmeasurement model is derived.7Much of the investigation involves simulations to determine the flight profile and aircraft/targetgeometry that will allow the Kalman filter to estimate the antenna boresight azimuthmisalignment to the required accuracy in a minimum amount of time. A computer simulationsoftware package, developed to support this effort, is described in Section 4. Major features ofthe package include: generation of arbitrary test flight trajectories specified by simple commandfile input; synthesis of sensor data corresponding to the flight trajectory with major sensor errorsources included; processing of sensor data using the inertial navigation algorithm described inSection 2 and the Kalman filter formulation described in Section 3; evaluation of the results bygraphing and comparison with true trajectory and true sensor error data.Section 5 describes the test methods and presents the results of the flight profile andaircraft/target geometry simulations. It is shown that two simple turning manoeuvres a fewminutes apart followed by a period of TAU measurements while flying directly toward the targetis effective in allowing the Kalman filter to estimate the antenna azimuth misalignment.Evaluation of the Kalman filter performance is described in Section 6. This task involves theprocessing of simulated sensor data provided by DREO that is generated using their SARMCSsynthesis package. Results suggest that estimation of the antenna azimuth misalignment to anaccuracy of better than 4 arc minutes rms within a 20 minute period is feasible.Conclusions and recommendations regarding the Kalman filter design, the test methods, theflight profile and aircraft/target geometry, and the calibration performance are summarized inSection 7.8SECTION TWOREVIEW OF NAVIGATION CONCEPTSAircraft navigation and, in particular, inertial navigation, has a dominating influence on theformulation of the antenna boresight calibration Kalman filter. A review of some fundamentalconcepts and specific results, which will be used in subsequent sections, is presented here.2.1 FRAMES OF REFERENCE AND COORDINATE SYSTEMSSeveral coordinate systems are commonly encountered in navigation. The coordinate systemsused in this thesis and described in the following paragraphs are all right-handed, orthonormalsystems and are chosen to be compatible with the SARMCS.2.1.1 Earth-Centred Inertial FrameNewton's laws of motion hold true only in an inertial frame. A coordinate system that has itsorigin at the mass centre of the earth and that is non-rotating relative to the "fixed" stars, canbe considered to be an inertial frame for measurements made in the vicinity of the earth. TheIMU provides measurements of angular velocity and specific force with respect to this frame.2.1.2 Geocentric Earth-Fixed FrameThe geocentric earth-fixed frame has its origin at the earth's centre and its basis vectors fixedto the earth. The x axis is along the rotational axis of the earth and points towards the northpole, the y axis is in the equatorial plane at -900 longitude and the z axis is in the equatorialplane at 00 longitude (Greenwich Meridian). The earth-fixed frame rotates with respect to theinertial frame at the constant sidereal rate.92.1.3 Geographic North-East-Down FrameThe geographic north-east-down frame is a locally level frame with its axes pointing true north,east and down. The x axis points north, the y axis points east and the z axis points down.Although this frame is intuitively appealing, it is usually not used as an inertial navigationcomputational coordinate system due to singularities at the poles. The closer the aircraft passesto the pole, the faster the geographic frame must rotate about the vertical axis to keep the x axispointing north, going to infinite angular velocity in the limit.2.1.4 Wander Azimuth Navigation FrameThe wander azimuth frame solves the high latitude problem of the geographic frame. It is usedas the computational coordinate system for inertial navigation. Like the geographic frame, thewander azimuth frame is locally level. Unlike the geographic frame, however, the verticalcomponent of its angular velocity relative to the earth-fixed frame is, by definition, identicallyequal to zero. The direction of the horizontal axes varies (wanders) depending on the path takenby the aircraft. Poor (1989) has given a description of the wander azimuth frame in particularlyunderstandable terms. The frame has its x axis displaced from true north through the wanderangle, its z axis pointing along the local vertical in the upward direction and its y axiscompleting the right-handed system. The Euler angles relating the wander azimuth frame to theearth-fixed frame are the longitude X, the geodetic latitude co, and the wander angle a.Figure 2-1 illustrates the relationship between the earth-fixed frame and the wander azimuthframe.Figure 2-1 Relationship Between Earth-Fixed Frame and Wander Azimuth Frame2.1.5 Vehicle Body FrameThe vehicle body frame is fixed to the aircraft body. The x axis points in the forward directionalong the longitudinal (roll) axis, the y axis points to the right of the aircraft along the lateral(pitch) axis and the z axis points downward along the normal (yaw) axis. For this work, it isconvenient to define any point fixed to the aircraft body in terms of its displacement vector(lever arm) from the INS. The vehicle body frame is, therefore, defined to have its origin atthe centre of the INS accelerometer/gyro instrument cluster. Lever arms from the INS to theGPS antenna and from the INS to the intersection of the antenna roll and pitch axes (point P inFigure 1-2) are defined in this frame.10112.1.6 Stabilized Antenna Ring Gear FrameThe stabilized antenna ring gear frame has its origin at the intersection of the antenna roll andpitch axes (point P in Figure 1-2). The x axis points in the forward direction along the antennaroll axis, the y axis is to the right along the antenna pitch axis and the z axis is downward alongthe antenna azimuth axis. Since the antenna ring gear is roll and pitch stabilized, the ring gearframe approximates a locally level frame. The origin of the antenna ring gear frame is theorigin for the bearing calculations performed during antenna boresight calibration.2.1.7 11VIU Body FrameIMU accelerometer and gyro measurements are given in the IMU body frame. This frame hasits origin at the centre of the IMU accelerometer/gyro instrument cluster and its basis vectorsaligned with the instrument input axes (see Figure 1-2). The x axis points forward, the y axispoints to the right, and the z axis points downward. The orientation of the IMU body frame isfixed with respect to the antenna ring gear frame. A fixed lever arm (point P to point 0 inFigure 1-2) locates the IMU relative to the antenna roll/pitch intersection.2.2 THE EARTH MODELComputations involving positions on the earth (for example, bearing calculations and inertialnavigation) require a mathematical model of the shape of the earth. In addition, whenmeasurements from accelerometers and gyros are to be used, a model describing the earth'sgravity and a knowledge of the earth's rotational rate are required. Detailed descriptions of themodels and their derivations are given in Ewing and Mitchell (1970), Heiskanen and Moritz(1967), Dragomir et al. (1982) and others.122.2.1 ShapeRoughly speaking, the earth is spherical. However, its departure from a true spherical shapeis too great for this model to be used for navigation calculations. Surface irregularities areeasily handled by adopting mean sea level as the altitude reference. The surface defined bymean sea level is an equipotential surface known as the geoid. It is everywhere normal to thedirection of gravity but, due to mass concentration variations within the earth, the shape of thegeoid can only be approximated. Because the earth rotates, it assumes a shape that is bulgingat the equator and flattened at the poles. A figure which adequately approximates the shape ofthe geoid is the ellipsoid of revolution. It is produced by rotating an ellipse about its minor axis,with the major axis generating the equatorial plane. Figure 2-2 illustrates the relationshipbetween the physical surface, the geoid and the ellipsoid of revolution. The vertical separationof the geoid from the ellipsoid is known as the geoidal undulation and the angle between thenormal to the ellipsoid and the normal to the geoid (the direction of gravity) is called thedeflection of the vertical.Figure 2-2 Relationship Between Physical Surface, Geoid and Ellipsoid13Figure 2-3 shows the ellipse with a major axis of length 2a and a minor axis of length 2b. Theequation of the ellipse isX 2 y2 ib2 a2The flattening of the ellipse, known as f, is defined byfr a-baVery often, the two parameters a and f are used to define the ellipsoid.(2-1)(2-2)Figure 2-3 The Ellipse14A parameter of the ellipse often used in computations is the square of the first eccentricity,designated e2, which is defined asa2-b2e2 = ^ = 2f-f2a 2(2-3)Additional properties of the ellipsoid which are needed for navigation calculations are the radiusof curvature in the plane of the meridian, designated M, and the radius of curvature in the planeof the prime vertical, designated N. At any point on the ellipse (for example, point S inFigure 2-3), the radius of curvature of the ellipse in the plane of the meridian is given byM=  a(1 -e 2)^(2-4)(1 -e 2sireso)312where (p, the geodetic latitude, is the angle between the equatorial plane and the normal to theellipsoid at the point. The radius of curvature in the plane of the prime vertical is the radius ofcurvature through any point on the surface of the ellipsoid (for example, point S in Figure 2-3)in a plane at right angles to the plane of the meridian. The distance QS in Figure 2-3 isidentical to the radius of curvature in the prime vertical through point S. The value of N isgiven byaN= ^ (2-5)(1 -e 2sin241)1/2xN+htan yo =(1-e2)N+h 0, 2 +z 2)1/215The position of a point (for example, point T in Figure 2-3) relative to the centre of the ellipsoidmay be expressed in Cartesian coordinates (x, y, z) or in curvilinear coordinates (geodeticlatitude co, longitude X and ellipsoidal height h). Ellipsoidal height (or altitude) is the distanceabove the ellipsoid measured along the normal to the ellipsoid at the point. The transformationfrom curvilinear geodetic coordinates to Cartesian coordinates is given by[x][((1-e2)N+h)sinyoly = -(N+h)cos co sin Xz^(N+h)cos yo cos X^ (2-6)The transformation from Cartesian coordinates to curvilinear geodetic coordinates is most oftenperformed iteratively. Equations (2-6) can be rearranged to obtaintan X = 2-^ (2-7)zh = • ^_(1_e2)N = 6, 2 +z 2)1/2 -Nsm co cos coX is obtained directly from (2-7). To solve for latitude and altitude, first obtain an initial(2-8)(2-9)estimate for the latitude from (2-8) by setting e2 to zero. Next, calculate N using (2-5). Now,(a2cos2co +b 2sin2co)"2=^ec0s2 +b-y^(1^P °7 (2-10)16compute h using (2-9) and 40 using (2-8). Finally, iterate the calculation of N, h and yo until therequired accuracy is achieved (typically four to six iterations).2.2.2 RotationThe rate of the earth's rotation with respect to the inertial frame is designated coie.Conceptualization of the rotation rate is aided by imagining a line joining the earth and the sun.The period of rotation of the earth with respect to the earth-sun line is one day and the earth-sunline experiences one revolution in approximately 365.25 days. The result is a constant angularvelocity of the earth (called the sidereal rate) of about (365.25+1)1(365.25 x24) cycles per hour.2.2.3 GravityThe force acting on a body at rest on the earth's surface is the resultant of the gravitational forceand the centrifugal force of the earth's rotation. The total force is called gravity. Theoreticalgravity at the surface of the ellipsoid iswhich represents the closed formula due to Somigliana (Heiskanen and Moritz 1967). In thisequation, e is the theoretical gravity at the equator and 7p is the theoretical gravity at the poles.The formula may be expressed as a function of the eccentricity as7= (1 _e 2sin201/2e(1+ksin2 co) (2-11)17where k is the constant given bybry -1ayeFor a small height h above the ellipse, normal gravity can be expanded into a power series inh. To second order, the resulting expression for gravity isg =y[1--2-(1 +f+m -2fsin2(p)h+-3a^a 2(2-12)where m is the constant given byco? a2bm. teGMand GM is the earth's gravitational constant.2.2.4 World Geodetic SystemGPS-determined position coordinates refer to the World Geodetic System 1984 earth-centredearth-fixed geodetic datum (Defense Mapping Agency 1987). WGS 84 specifies a set of definingparameters for the ellipsoid from which all of the constants referenced in subsections 2.2.1through 2.2.3 may be derived. Table 2-1 lists the required constants.18Table 2-1 WGS 84 ConstantsConstant Notation ValueSemimajor Axis a 6.378137 x106 mFlattening f 3.35281066474 x 10-3First Eccentricity Squared e2 6.69437999013 x 10-3Angular Velocity Wie 7.292115 x10-5 rad/sGravitational Constant GM 3.986005 x 10'4 m3/s2Normal Gravity at Equator 7 e 9.7803267714 m/s2Gravity Formula Constant k 1.93185138693 x 10-32.3 BEARING CALCULATIONBearing is the direction to the target measured relative to north and about the vertical at theaircraft's position. Figure 2-4 represents a horizontal plane viewed from an aircraft directlyabove point A. The plane contains point T which is the position of the target. Note thatalthough the plane is horizontal with respect to the normal to the ellipsoid at A, in general it isnot tangent to the ellipsoid since it is defined to contain point T. In the figure, x is the lengthof line segment BT which is perpendicular to the meridian plane containing A and extends topoint T, and y is the length of line segment AB which is perpendicular to the vertical containingA and extends to join the first line segment. Bearing is then given by13 =arctan [^(2-13)19NorthFigure 2-4 Bearing Plane Viewed from an Aircraft Above Point AFigure 2-5 shows the meridian plane containing the target point T. In the figure, QP is theprojection of QT onto a plane parallel to the equator. The length of QP isI QP I =(NT+hT)coscorwhere NT is the prime vertical at the target location, liT is the height of the target and (pi is thegeodetic latitude of the target.pole20equatorFigure 2-5 Target Meridian PlanepoleequatorFigure 2-6 Aircraft Meridian Plane21Figure 2-6 shows the meridian plane containing the aircraft and the points A and B. Theprojection of QP onto this plane is QR which has lengthI QR I =(NT-FhT)cos soTcos(XT-XA)where XT is the longitude of the target and XA is the longitude of the aircraft.The result of these projections is a right triangle parallel to the equatorial plane with hypotenuseQT and sides QR and RP. But, RP is parallel to BT and, therefore, must have length x so thatx=(NT+hT)cos scTsin(XT-XA)^ (2-14)To find an expression for y, define the angle 0 and the distance r as shown in Figure 2-6. Nowy is given byy = rsin(0 -SCA) = r(sin 0 cos coA-cos0 sin coA)where soA is the geodetic latitude of the aircraft. The length of BR in Figure 2-6 is equal to thelength of TP in Figure 2-5 so thatI BR I =(NT+hT)sin (dor22andsin 0 =  (NT+hT)sin wr-dcos 0 =  (NT+hT)cos wrcos(XT-XA)where d accounts for the fact that, in general, the prime vertical at the target location and theprime vertical at the aircraft location do not intersect the earth's axis at the same point.Knowing the earth-fixed frame x coordinates of the aircraft and target from equation (2-6) andadjusting for the x displacements to the axis intersection points given by (NA+hA)sin WA and(Nr+hT)sin wr, an expression for the difference d is obtained asd=e2(Nrsin 607.-NA sin WA)^ (2-15)Now y can be writteny=[(Nr+hOsin WT-d]cos W -017.1"h000S W 7. sin WA cos(XT-XA)which, after some algebraic manipulation, becomes(2-16)y=(NT+hT) [ sin (coT-coA) +cos wTsin WA (l -cos(XT-XA)) - NT+hTdcos WA ](2-17)(3=arctan ^cos 10Tsin AXsin Aco +cos 'Pr sin coA (1 -cos AX) -  d cos (PANT÷hTCombining (2-13), (2-14) and (2-16) the expression for bearing becomes23where d is given by (2-15) and Ayo and AX are defined as2.4 INERTIAL NAVIGATIONA strapdown IMU provides three-dimensional measurements of specific force (nongravitationalacceleration) and angular velocity with respect to an inertial frame. Inertial navigation systemssolve Newton's force equations using the accelerometer measurements coordinatized in a framewhose orientation is known by processing the gyro measurements.2.4.1 Navigation in Wander Azimuth CoordinatesTerrestrial navigation equations in wander azimuth coordinates are derived in many references(Britting 1971; Farrell 1976). The equations are24=f +g -(2wie^x V^ (2-18)k=V—coewxR (2-19)whereR = position with respect to earth centreV = velocity with respect to earthf^= specific forceg = gravity= angular velocity of earth-fixed frame with respect to inertial frame&Jew = angular velocity of wander azimuth frame with respect to earth-fixed frameand all vectors are expressed in wander azimuth coordinates. The cross product terms arecorrections required because the wander azimuth frame and the earth-fixed frame are rotatingwith respect to the inertial frame.Typically, (2-19) is not solved explicitly for position. Instead, the angular velocity co, isintegrated to obtain the geodetic latitude, longitude and wander angle and (2-19) is used only forthe vertical position. If the orientation of the wander azimuth frame with respect to the earth-fixed frame is expressed in terms of the direction cosine matrix C„ which transforms a vectorin wander azimuth coordinates to the same vector in earth-fixed coordinates, then the horizontalposition equation becomesee,„=C:pe„,^ (2-20)C13tamp =a., 2 4.r, 2 N1/2l'-'11 ' `-'12./(2-23)25where ft ^the skew symmetric matrix0^—CO^COewz^ewyflew= [(any X ] = COewz^0^—COewx—Wewy Wewx^0(2-21) with we., (dewy and co ^the components of coew.Cwe can be expressed in terms of the Euler angles so, X and a asCOS so COS a^—COS so sin a[^sin so 1C ew= COS X sin cx +sin co sin X cos a cos X cos a -sin so sin X sin a -cos so sin Xsin X sin a -sin so cos X cos a sin X cos a +sin so cos X sin a cos so cos X(2-22)and so, X and a can be obtained from the elements of Cwe using the equationstan x . —C23C33tan a =-C12C 11(2-24)(2-25)26To solve the navigation equations, it remains to express the angular velocities wie and w interms of the current position and velocity. wie is obtained by transforming its earth-fixedcoordinate representationCOlee=[00to wander azimuth coordinates using the direction cosine matrix CweTrWielcew wiee=CO ieCOS COCOS CI—(4 ieCOS (p sin awiesin(2-26) we„, is computed from knowledge of the velocity and the radii of curvature. When a =0, thewander azimuth x and y axes correspond to the north and west directions and u given by theexpression— —VN +h(4)ewl^V xM +h0For arbitrary a, the expression for weiv becomes27-Vxsin a -Vycos a^Vxcos a -V sin a sin acos a +^Y^siN+h^M+h(Jew= -Vxsin a -V ycos a^ Vxcos a -V sin aSinai-  ^Cosa(2-27)N+h^M+hY0Rearranging (2-27) and defining the inverse radii of curvature terms1 .(M+h)sin2a+(N+h)cos2a (2-28)(M+h)(N+h)1 _(M+h)cos2a+(N+h)sin2a (2-29)Ry^(M+h)(N+h)1 _(N+h)-(M+h) .sin a cos a (2-30)T^(M+h)(N+h)permits wew to be written as—V v_^y^xRy^Twew= Vx Vy (2-31)Rx^T0Note that the italicized symbols Rx and Ry used for the earth radii should not be confused withthe components of the position vector R denoted by Rx and R.282.4.2 Attitude ComputationAn inertial navigation system maintains position and velocity information by solving equations(2-18) through (2-31) given initial values of position and velocity and continuing measurementsof specific force expressed in wander azimuth coordinates. Since a strapdown IMU providesspecific force measurements f6 in IMU body coordinates, it is necessary to transform themeasurements to wander azimuth coordinates before they can be used in the equations. Thedirection cosine matrix C, which transforms from IMU body coordinates to wander azimuthcoordinates, is maintained by processing the IMU gyro measurements. The strapdown gyrosmeasure the angular velocity of the IMU body frame with respect to the inertial frame andprovide measurements co,b in IMU body coordinates. cote and we,,, after transformation to IMUbody coordinates, are subtracted from coi:b to obtain the angular velocity of the IMU body framewith respect to the wander azimuth frame7'b^b^wwb =1-4)ib Jcbi (2-32)which is then integrated (typically using a quaternion based algorithm) to maintain C. Cnb' canbe expressed in terms of the roll angle 0, the pitch angle 0, and the azimuth angle (platformheading) as(2-33)[ cos 0 cos E -cos 0 sin +sin 0 sin 0 cos E sin 0 sin +cos 0 sin 0 cos EC= -cos 0 sin -cos 0 cos -sin 0 sin 0 sin sin 0 cos -cos 0 sin 0 sin Esin 0^-sin 0 cos 0^-cos 0 cos 0tan t . -c 21S C 11(2-36)Figure 2-7 Mechanization of Strapdown Inertial Navigation System29and (/), 0 and E can be obtained from the elements of q, using the equationstan A. -c32W -C33(2-34)tan 0 = ^C31(C +C221)"2Also, heading relative to north is given by300 -a^ (2-37)Figure 2-7 illustrates the mechanization of a strapdown inertial navigation system performingthe computations in wander azimuth coordinates.2.4.3 Vertical Channel MechanizationIntegrating vertical acceleration without some form of error control using a stable externalaltitude reference results in an unstable system. This is because altitude errors are reinforcedby positive feedback through the gravity calculation. Typically, barometric altitude is used asthe reference in a PID (proportional, integral, derivative) controller. The resulting third orderbaro-inertial loop is shown in Figure 2-8. In the figure, A, is the total vertical acceleration(including gravity and the Coriolis correction), hb is the barometric reference altitude, and kbk2 and k3 are the gains. A typical vertical channel mechanization is shown in Figure 2-9. Notethat the vertical velocity is picked off before the derivative feedback through 1(1. This resultsin reduced vertical velocity error. The altitude must track the barometric reference over the longterm thus altitude error is determined primarily by the accuracy of the barometric altitude.31Figure 2-8 Third Order Baro-Inertial LoopVz+/D.IhFigure 2-9 Mechanization of Vertical Channel Baro-Inertial Loop322.5 INERTIAL NAVIGATION ERROR ANALYSISThe inertial navigation error analysis defines three new frames of reference. These are thecomputer frame, the true frame and the analytic platform frame. In the absence of errors, thesethree frames are equivalent. The computer frame refers to a wander azimuth navigation frameconstructed at the computed aircraft position. It is related to the earth-fixed frame by thecomputed latitude, longitude and wander angle. The true frame is related to the earth-fixedframe by the true latitude, longitude and wander angle. Due to errors in the computed values,the computer frame is slightly misaligned with respect to the true frame. Similarly, the analyticplatform frame is related to the body frame of the strapdown inertial measurement instrumentsby the computed roll, pitch and azimuth angles. It is slightly misaligned with respect to the trueframe due to errors in the computed angles. Figure 2-10 shows the relationships between theframes.Figure 2-10 Relationships Between Inertial Navigation Error Analysis Frames33The SARMCS Kalman filter uses a perturbation (or true frame) model of the inertial navigationerror dynamics. Quantities computed or measured by the navigation system are referred to asindicated quantities and are denoted by enclosure in parentheses followed by the subscript i. Forthe true frame error model, the relationships between indicated quantities, true quantities anderror quantities are defined as follows(Re), = R: = C(R+SR)(V), = V = Vi-SV= C: = CX`c = C1I+Sex](C1b)1 = CPb =^= [I+Ox10,(cote), =^= co,e+Scote(wet), = w:c^wel+bw(g), = gec^g+Og(fb), = fb+6fbb b= Wib÷EwhereRec = computed position in earth-fixed coordinatesR^= true position in true frame coordinatesSR = position errorVec = computed velocity in computer frame coordinatesV^= true velocity in true frame coordinatesOV = velocity error34cec = computer frame to earth-fixed frame direction cosine matrix= true frame to earth-fixed frame direction cosine matrix= identity matrixSO = small angle misalignment of computer frame with respect to true frameCP = instrument body frame to analytic platform frame direction cosine matrix= instrument body frame to true frame direction cosine matrix= small angle misalignment of analytic platform frame with respect to true frameWe = angular velocity of earth-fixed frame with respect to inertial frame in computerframe coordinatesWie = angular velocity of earth-fixed frame with respect to inertial frame in true framecoordinatesSwie = error in indicated angular velocity of earth-fixed frame with respect to inertialframe(dee = angular velocity of computer frame with respect to earth-fixed frame incomputer frame coordinatesWet = angular velocity of true frame with respect to earth-fixed frame in true framecoordinates&de, = error in indicated angular velocity of true frame with respect to earth-fixedframegee^= computed gravity in computer frame coordinates= true gravity in true frame coordinatesOg = gravity error= true specific force in instrument body coordinatesOfb = accelerometer errors in instrument body coordinates= true angular velocity of instrument body frame with respect to inertial frame ininstrument body coordinates= gyro errors in instrument body coordinates35A derivation of the true frame model of the inertial navigation error dynamics for an arbitrarytrue navigation frame t is described by Benson (1975) and results in the following equations^Mk —wax 511-V xbei-OV^ (2-38)^=-(26.)ie 'e1) SIT +V x (2Owie^et) ^X (/) +Of +Og^(2-39)+wei) X +(bwie ÷6Wei) (2-40)Benson expands these equations for a local-level north-pointing navigation frame. However, theSARMCS navigation calculations are performed in a wander azimuth frame. An expansion forthe wander azimuth navigation frame is presented in the following paragraphs. This derivationclosely follows a derivation by DiFilippo (pers. corn. July 10, 1992).An expression for wie is obtained from equation (2-26).[ COieCOS SO cos a(oie = —WieCOS SO sin awiesin co(2-41) Expressions for the x and y components of we, are obtained from equation (2-31). The zcomponent of a; is, by the definition of the wander azimuth frame, identically equal to zero,therefore, the z component of we, is -Swei, (derived later). The complete expression for coe, is36Wet=—^ —v vY—^+ xR TYVx _ VyRx T-6coa,_^_(2-42)To obtain an expression for 60, first observe that cce equals CciCte which, for small anglemisalignment, is approximated byc ce -4 -sex iceNow, defining the computed latitude, longitude and wander angle asic'c'IP ÷8VX, =X i-OXac=a-}-baexpressions can be obtained for ce, in terms of (pc, Xc and ac, and Ce, in terms of co, X and a,by using equation (2-22). Solving for 60, to first order in the error quantities, results in60x=6co sin a i-SX cos co cos a60 =64o cos a -6X cos cc sin aY606«+6X sin vSX=^(N+h)cos-SRxsin a -SRysin a (2-44)37Substituting the first order approximationsandSR cos a -SR sin abyo ^M+h(2-43)into the expressions for Mx and 60 y gives, after some manipulation,SOx= - SR), + SR'Ry TSRyMY= R TSince 1/Rx and 1/Ry are always much larger than 1/T, the terms containing 1/T can be ignored.60, defines the azimuth misalignment of the computer frame due to the error in the computedwander angle Sa and the error in the knowledge of the north direction owing to the error in thecomputed longitude SX. Since Mx and My are sufficient to define the level position error, thereis a form of redundancy in the azimuth misalignment definition. This redundancy is exploitedin the error analysis by defining0^ (2-45)and, hence,38ba = -A sin (p^ (2-46)The expression for 30 is thenSRRyM= oRx (2-47)Using the definitionScoie =4.11.e —Wieand noting that 41i e equals Ccmie which, for small angle misalignment, is approximated by[I-60 X]wie, an expression for Scoie isSo) =—SO X coor, using (2-47),Swie=- SR-R YCt)ct' ex SR +-1.e—Y ORRx^R YY(2-48) 39Expressions for Owe„, and Swegy are obtained by perturbing wet), and way. Consider\Ix=— Y +R Twhich leads to1^V^1^V5coe,„=- --5V +-2Ry Y^T^T2For velocities less than 200 metres per second, position errors less than 1000 metres, altitudeerrors less than 100 metres and velocity errors on the order of 1 metre per second, only the firstterm is significant and &deb, is5we,„=---R-1^yThe expression for Way is similarly obtained as50) = —1 SVegYTo find an expression for S(4,, consider the definitions(5b.wiec=weCe_coeCiand40and also observe that we', equals Ceme, which, for small angle misalignment, is approximated by[I-50 x]co„, so that=Scoet+60 X co,The z component of this expression isz=6 o c 4,00 y+co eiy(50xSince 60z is, by the definition in equation (2-45), identically equal to zero, its derivative is alsozero and the expression for Sco, becomes^esz So =co^y — coety50 xor, using (2-47),Sco^OR 1-.Y(' ) OR^ezz Rx^R^YThe complete expression for Owe, is then(4'^co(512x+ YORRx^R Y(2-49)41The gravity error Sg is a function of latitude and altitude errors as well as vertical deflectionsand gravity anomalies. An expression for the gravity error can be obtained using the definitionSg -g:-gThe magnitude of the true gravity vector is g but, due to the small angle vertical deflection 17,the true gravity in true frame coordinates isg =[I+nx][ g^;1 = [111-gi^-gThe magnitude of the computed gravity vector differs from the true value due to errors in theknowledge of latitude and altitude and due to the gravity anomaly. The gravity anomaly is zig.Of the remaining gravity error, only that due to the altitude error is significant assuming positionerrors of less than 1000 metres. Approximating the local gravity using an inverse square lawwith a gravity value of g at radius R+h, the computed gravity can be written asgc=g +4 --2g OhR+ho,^ogc=42Replacing R+h with an approximate value equal to the earth equatorial radius a and noting that617 is equal to the vertical position error 6R„ the computed gravity vector in computer framecoordinates isThe expression for the gravity error is thenSg=-- (2-50)Now, substituting (2-42) and (2-47) through (2-50) into equations (2-38) through (2-40), theinertial navigation error dynamics can be expressed in the state -space formi=Fx+wwhere the state vector x is given byx=[6Rx OR y SI 2 z (5V x Aly 6V, 4x Oy (kr (2-51)43the forcing function vector w is given byw=[0 0 0 Of, +nyg Sfy-nxg Of,-4 -Ex -Ey -edT^(2-52)and the system dynamics matrix F is as shown in Figure 2-11.The error analysis presented in this subsection does not include any dynamics due to barometricaltitude damping of the vertical channel. Baro-inertial vertical channel equations correspondingto the mechanization shown in Figure 2-9 areii=V,-ki(h-hdV, =A, -k2(h -hb) -AA,AA, =k3(h -hb)whereh^= inertial altitudehb^= barometric altitudeV,^= vertical velocityA,^= vertical accelerationAA,^= estimated vertical acceleration errorkb k2, k3 = baro-inertial loop gainsThe corresponding vertical channel error dynamics can be obtained by perturbing theseequations. However, in Section 3, the vertical channel error states are shown to be unimportantin the Kalman filter formulation and so the results are not included here.0W ._ tezVz—Rx0 -co ewY1 0 0 0V z0 COewx 0 1 0 0Ry0 0 0 0 0 1 0(2c3 ia+^ )V 2w1ezVz -4-(2cuiey -1-coe„,)Vy0V-- 2wiez -(2wiey +coewy) 0Rx2coiezVz +(24.4i„, +coewx)Vx o^)V(2c&,.^+c ewy_^zey ^x 0Rx-2cojezV z--R2, iex +CO ewx fzRx0^00^00^0fy02V .y(4 tezRx2Vxcoiezy-3--g 2 (coiey + ewy) -2(coiex + ewx)^0^-f^ 0a^ fx 10^0^ 0^0 co.^-(co. +co )R^ iez tey^ewy0^ 0^0 iez 0^cohtt ÷co.—6)0^CO . +CO^-(co + ewx)^0tey^ewy0wie,, +we.R.Figure 2-11 Inertial Navigation Error Dynamics Matrix^41,4=,45SECTION THREEKALMAN FILTER FORMULATIONOptimal estimation of the state of a linear system by Kalman filtering is well known (Gelb 1974;Brown and Hwang 1992). The discrete form of the Kalman filter is used here because of thediscrete nature of the available sensor information and because it is well suited to computerimplementation. Application of the discrete Kalman filter requires the process dynamics and themeasurement relationship to have the formxk +1 =4) kx k+w k^ (3-1)zk=Hkxk+v k^ (3-2)whereXktcII ' kZkIlkV k= (nx 1) process state vector at time 4(n X n) state transition matrix from xk to xk+1= (nx 1) process noise vector= (m x 1) measurement vector at time tk= (m xn) measurement matrix relating the measurement to the state at time 4= (m x 1) measurement noise vectorThe process noise Wk must be white with zero mean and is assumed to be normally distributedwith (nxn) covariance matrix Qk. Similarly, the measurement noise vk must be white with zeromean and is assumed to be normally distributed with (m x m) covariance matrix Rk.46Inertial navigation error equations have been derived in many references ( Benson 1975; Huddle1983). The result is a nine state system (three position errors, three velocity errors, threemisalignments) driven by accelerometer and gyro errors and errors in the computed gravity. Bycomparing the inertially computed position with the position provided by a GPS receiver,measurements of position error are obtained (referred to as position matching measurements).These process and measurement models form the basis for the current SARMCS Kalman filter.The antenna boresight calibration Kalman filter adds an antenna azimuth misalignment statecorresponding to a random constant process model. Azimuth matching measurements areobtained by comparing the TAU/azimuth encoder measurements with the azimuth computed fromthe known target location together with GPS-aided inertial position and heading information.3.1 THE SARMCS KALMAN FILTERThe antenna boresight calibration Kalman filter is intended to be incorporated into the SARMCSas a special operating mode. As such, it is appropriate to formulate the calibration Kalman filteras an extension of the current SARMCS Kalman filter. The current SARMCS Kalman filterformulation is described here. This description is based on a description by DiFilippo (Dec. 23,1992).The strapdown IMU, employed in the SARMCS to measure SAR antenna motion, uses small,low-cost gyros that can result in very large misalignment of the IMU navigator analytic platform.Control of this misalignment, so that the IMU navigator provides accurate motion informationfor SAR motion compensation, is the objective of the SARMCS Kalman filter. The basicprocess model consists of the inertial navigation error dynamics associated with the INS plus the47inertial navigation error dynamics associated with the IMU navigator. INS/GPS positionmatching measurements are used to control the INS errors providing a very stable INS platform.A transfer of alignment from the stable INS platform to the IMU navigator is accomplishedusing IMU/INS position matching measurements. The SARMCS Kalman filter designrecognizes that measurements constructed by comparing information from two systems withessentially the same error dynamics allows observability of only the relative error between thesystems. The IMU/INS position matching measurements are of this type and consequently theIMU navigator errors are modelled relative to the INS rather than as absolute errors. INS/GPSposition matching measurements, however, do allow observability of absolute INS errors.Absolute IMU navigator errors are obtained as the sum of the absolute INS errors plus therelative IMU navigator errors.There are nine error states for each inertial navigator as described in subsection 2.5, however,since the vertical channel errors are kept small by baroaltitude feedback and since there is littlecross coupling between the vertical axis and the level axes, the vertical channel states are notmodelled in the SARMCS Kalman filter. The basic SARMCS process model includes two levelposition error states, two level velocity error states and three misalignment states for eachinertial navigator for a total of 14 states.The inertial navigation error process is driven by accelerometer errors, gyro errors and verticaldeflections of the gravity vector. In the SARMCS Kalman filter, INS level accelerometerbiases, INS gyro biases and IMU gyro biases are modelled as random processes that areexponentially correlated in time with long correlation times. The process model is augmentedto include first-order Markov processes for these errors adding eight new states. Certain other48time-correlated or distance-correlated errors (for _example, sensor scale factor errors andmisalignments, gyro mass unbalance, and vertical deflections) are only modelled, if at all, aswhite noise processes, driving the inertial error states, because their effects are too small towarrant modelling as separate states in the Kalman filter.The errors in the GPS-indicated level position are correlated in time and consequently aremodelled as states in the SARMCS Kalman filter. First-order Markov processes with longcorrelation times are used to model these errors adding two more states to the process model.Other Kalman filter position matching measurement errors, due primarily to timing errors whencomparing measurements from different sensors, are modelled as white noise processes.The complete 24-element state vector of the current SARMCS Kalman filter is shown inTable 3-1. The subvectors xm and xs contain INS (or master navigator) system states and IMUnavigator (or slave navigator) system states respectively, xpa and ximu contain INS and IMUinstrument error states, and X Gps contains GPS position error states. The states in xm, xiNs, xGpsand x1 defined as indicated or apparent quantities minus true quantities while the states inxs are defined as IMU navigator indicated quantities minus INS indicated quantities. For thisKalman filter configuration, the estimated IMU navigation errors must be constructed whenneeded as xs+xm.The continuous time process model has the formit(t) =F(t)x(t) +w(t)49Table 3-1 SARMCS Kalman Filter StatesSubvector State Description CoordinateFramexmWm,SRmySVm,OVmyOmx4) MyOmzINS position error along x axisINS position error along y axisINS velocity error along x axisINS velocity error along y axisINS platform misalignment about x axisINS platform misalignment about y axisINS platform misalignment about z axisINS wanderazimuthX INSAmxAmyG AixGmyGmzINS x accelerometer biasINS y accelerometer biasINS x gyro biasINS y gyro biasINS z gyro biasAircraftbodyx GPS61(a,ORGYGPS position error along x axisGPS position error along y axisINS wanderazimuthxsORSRsyWs,SVsyOs,Os),Osznavigator position error along x axisIMU navigator position error along y axisIMU navigator velocity error along x axisIMU navigator velocity error along y axisIMU navigator platform misalignment about x axisIMU navigator platform misalignment about y axisIMU navigator platform misalignment about z axisIMUnavigatorwanderazimuthximuGs,GsyGszIMU x gyro biasIMU y gyro biasIMU z gyro biasIMU body50where x(t) is the 24-element state vector, F(t) is the 24 x24 system dynamics matrix, and therandom forcing function w(t) is a 24-element vector of zero-mean white noise processes. Therandom forcing function has the same structure as the state vector, namely147 =W mWINSGPSW SIMU The system dynamics matrix has the structureFM FM/INS 0000 F INS 0 0^0F= 0 0 F Gps 0^00 0 0 Fs F saw0 0 0 0^F 'muwhere Fm is a 7x7 matrix, FINS is a 5x5 matrix, FM/INS is a 7x5 matrix, F Gps is a 2x2 matrix,Fs is a 7X7 matrix, Fimu is a 3x3 matrix, and Fsamu is a 7x3 matrix.The elements of Fm, shown in Figure 3-1, correspond to the inertial error model expressed inwander azimuth coordinates, as developed in subsection 2.5, but with the vertical channeldeleted. Fs has the same form as Fm but the quantities apply to the IMU navigator rather thanthe INS.vz^0^10^ vz^0(2wi„, +co x)Vy^2comzV, +.(2cojey +wewy)Vy _ VzRx R^RxY—^^2co1ezVz +(2coie, i-co,,,x)Vx^—  (2cojey +conyy)Vx —2coiRx^ Ry^a0 0 0 01 0 0 02cojez 0 —fz fYV—R fz 0 —fxYW .10^0^0 C'') iez^—(44) iey + CI) ewy)Rx ITy(,)Iez0^ _^1 a0^0^wi ÷wemRy—Rx^— (4) iezWierx^ iey +COewy0^0 W. ÷W^ 0+CO )Rx ley ewy^leX ewxFigure 3-1 Elements of Fm and FsLet52The dynamics of the error states in xp,,s, xGps and /caw are modelled by first-order Markovprocesses, resulting in the submatrices1 0_^1000000TMAB0MABFINS = 0 0 -1 0 0TMGB0 0 0 _^1 0TMGB0 0 0 0 1TMGB1 0F =GPSTGP0^0TSGBo-1 0TSGB0^0 1 TSGBwhere the variables TMAI3) TM GB' T GP and T SGB represent the correlation times associated with theINS accelerometer bias, INS gyro bias, GPS position error and IMU gyro bias states.uTGP0^153The submatrix Fm/INS has the form- 0 0 0^0^0^000 ^0^0^C11 C12 0^0^0FM/INS• C21 C22 0^0^00 0 -C11 -C12 C130 0 -C21 -C22 -C2300 .""C31 -C32 C33where Cu is the element in the ith row and the jth column of the direction cosine matrix [C]Mwhich transforms a vector in aircraft body frame coordinates to INS wander azimuth coordinates,and [q]m is computed using equation (2-33). Similarly, the submatrix Fsamu has the form Fsnmu0^0^00^0^00^0^00^0^0-C11 -C12 -C13- C21 -C22 -C23- C31 -C32 -C33where the Cu are elements of the direction cosine matrix [C]s which transforms a vector in IMUbody frame coordinates to IMU navigator wander azimuth coordinates, and [Cl/ is computedusing equation (2-33) but with IMU navigator quantities rather than INS quantities.54The vector of continuous time, white noise forcing functions w(t) is described in terms of itscovariance matrix given byE[w (t)w T(7)] =Q(t)6(t —I)where Q(t) is the process noise spectral density matrix and O(t-r) is the Dirac delta function.The process noise spectral density matrix has the structureQMo0QINS0000_00Q= 0 0 QGPS 0 00 0 0 Qs 00 0 0 0 QimuVertical deflections of the gravity vector and several of the less significant errors associated withthe INS and IMU sensors are not estimated by the SARMCS Kalman filter, but instead aremodelled as motion dependent white noise processes driving the inertial error states xm and xs.The techniques used to determine suitable spectral densities for these white noise processes arenot important for the present work and are not examined in detail. Briefly, a white noiseprocess having spectral density q produces an incremental variance in the error state of qAt anda value for q is chosen by matching either the spectral density or the variance to that whichwould result from the more complex error model under some assumed motion.55The white noise driving the master navigator is described by the spectral density submatrix_o0^000^00 0 a a.,I 4- .MASF000000000000QM . 00 0 qn+qMASF 0 0 000 0 0 (IMGN 0 000 0 0 0 (IMGN 000 0 0 0 0 (IMGN—where qn and a.MASF are the spectral densities of the white noise models to account for verticaldeflections and INS accelerometer scale factor errors, and gAIGN is the spectral density of thewhite noise associated with the random drift of the INS ring laser gyros.The white noise driving the slave navigator is described by the spectral density submatrix00^0 0 0 0 000^0 0 0 0 00 0 a SAS aF+ .MASF 0 0 0 000^0 qSASF+qMASF 0 0 0Qs 00^0 0 qSABVSGM 0 0VSGMUVMGN00^0 (ISABVSGM 000^0VSGMUIVMGNqSGSFVSGMU56where aAMASF and (1MGN are previously defined and aASASF, qSAB, qSGM, gSGMU and qsaw are the spectraldensities of the white noise models to account for the effects of IMU accelerometer scale factorerrors, IMU accelerometer biases, IMU gyro misalignments, IMU gyro mass unbalances andIMU gyro scale factor errors.The spectral densities appearing in QM and Qs are given by the expressions2dg2 2q,,= Vnd4 V agnd^2(1MASF=^ C(MASF72 „CrAIGN(t)(1MGN=^ =constant4V „da 2(1SASF= g aSASF741Wzi 2qSAB=^(fSAB7g41Wzi 2q SGM =^a SGM741'a 2,- ^8nd  ,Tq SGMU "SGMU72q^=71U/ I a SGSFSGSF^z57wherea^= standard deviation of the deflection of the verticalnCIMASF = standard deviation of the INS accelerometer scale factor erroramGN(t) = time varying standard deviation of the INS gyro random drift (random walkprocess implies standard deviation increases as the square root of time)Cr SASFCr SABCrSGMaSGMU(TSGSF= standard deviation of the IMU accelerometer scale factor error= standard deviation of the IMU accelerometer bias= standard deviation of the IMU gyro misalignments with respect to theaccelerometers= standard deviation of the IMU gyro mass unbalance= standard deviation of the IMU gyro scale factor errorVvul^= aircraft ground speeda^= magnitude of aircraft acceleration vectorIIz^= absolute value of the aircraft heading rateg^= nominal gravitycln^= correlation distance of random deflections of the vertical5 8White noise drives the first-order Markov sensor error models, resulting in the process noisespectral density submatrices_ -20-MAB2^0^0^0^0rMAB20 2!f 0^0^0TM2(TMGB0^0 2^0^0TMGB2CTMGB0^0^0 2^0TMGB20^0^0^0^2 0-MGB-Q GPS =TGPTMGB--Q =INSTGP_-QIMU =2aSGB2^0^0TSGB2SGB0 2!^0TSGB2USGB0^0 2TSGB--59where the variables Cr MA13, CIMG13, aGP and asGe represent the standard deviations associated withthe INS accelerometer bias, INS gyro bias, GPS position error and IMU gyro bias states.The SARMCS Kalman filter uses INS/GPS position matching measurements to estimate INSposition errors, velocity errors and misalignments, and IMU/INS position matchingmeasurements to accomplish the transfer of alignment to the IMU navigator. Computation ofthe measurement vectors involves quantities having the same or similar definitions butdetermined separately in each of the three navigation systems (the INS, the GPS and the IMUnavigator). Quantities associated with a particular navigation system are indicated here byenclosure in square brackets followed by a subscript M, GPS or S. M refers to INS (masternavigator) quantities, GPS refers to GPS quantities, and S refers to IMU navigator (slavenavigator) quantities.The INS/GPS position matching measurements are obtained by subtracting the GPS-indicatedposition vector from the INS-indicated position vector and adjusting for the INS-to-GPS leverarm. The measurement vector zG is computed aszG=[cew]mT({Rif*Lps)+[c]jrps]mwhere= INS position vector expressed in earth-fixed coordinates= GPS position vector expressed in earth-fixed coordinates= direction cosine matrix that transforms a vector in INS wander azimuthcoordinates to earth-fixed coordinates60[04,6], = direction cosine matrix that transforms a vector in aircraft body coordinatesto INS wander azimuth coordinatesKpsh = INS-to-GPS lever arm vector expressed in aircraft body coordinatesThe IMU/INS position matching measurements are obtained by subtracting the INS-indicatedposition vector from the IMU navigator position vector and adjusting for the INS-to-IMU leverarm. The measurement vector zp is computed asz , 4c ewl Ts ({R ei s 4R ekk ew]slc ed jc Id jem u] mwhere[Re] s = IMU navigator position vector expressed in earth-fixed coordinates[C,'„] s = direction cosine matrix that transforms a vector in IMU navigator wanderazimuth coordinates to earth-fixed coordinates[rbafG]xf = INS-to-IMU lever arm vector expressed in aircraft body coordinatesThe SARMCS Kalman filter does not process the vertical components of zG and zp but only thehorizontal components. Therefore, the Kalman filter measurement vector is given by-Z =HG/m =1 0 0 0 0 0 0][0 1 0 0 0 0 061The linear model for the discrete Kalman filter measurements iszk=Hkxk+vkwhere Ilk is the 4 x 24 measurement matrix and vk is a 4-element vector of zero-mean, whitenoise measurement errors. The measurement matrix is obtained by perturbing the zG and zpmeasurement equations and expressing the results in terms of the modelled error states. Makingthe assumptions that the effect of errors in computing the lever arms can be neglected and that[C]k1 and [C„e]ls are approximately equal, the measurement matrix is[^ ]Ilwm 0 HG/Gps 0 0whereH=k^0 0 0 Hivs 0HG/GPs = [_01 (1Hp/5 =0 1 0 0 0 0 0[1 0 0 0 0 0 0162The measurement noise vector vk has the same form as the measurement vector, namely_V k = and is characterized by its covariance matrix Rk given by_2aG 0 0-00 cr2G 0 0Rk =E[V kV kl=0 0 ap2 00 0 0 0-2p- _where CrG and ap are the standard deviations of the INS/GPS position matching measurementnoise and the IMU/INS position matching measurement noise. The major contributor to thesemeasurement noises is timing errors that result when comparing measurements from the differentsensors.To begin the Kalman filter estimation process, the state vector and the error covariance matrixmust be initialized. The SARMCS Kalman filter initial state vector estimate is 540)=0. Theinitial error covariance matrix P(0) is diagonal and contains the variances of the errors in theinitial state estimates.63Finally, it should be noted that the SARMCS Kalman filter makes use of the fact that certainparts of the system state vector are only weakly coupled. In particular, the states [xm xms xGps],corresponding to the INS stabilization part of the filter, and [xs ximu], corresponding to thetransfer-of-alignment part of the filter, are decoupled in the SARMCS design. This is achievedby neglecting 1) a weak coupling between xms and xs in the system dynamics matrix, 2) a smallcorrelation between wm and ws in the spectral density matrix, and 3) a small initial cross-covariance between xm and xs. The effect on the estimation of IMU slave navigator error statesis minimal because of the weak coupling. It has the benefit, however, of preventing corruptionof the INS master navigator error states that, otherwise, could result from slight mismodellingof the lower quality IMU.3.2 ANTENNA AZIMUTH MISALIGNMENT ESTIMATIONAccurate antenna azimuth misalignment estimation depends on accurate IMU navigator positionand heading information. IMU navigator position error is bounded by the GPS positionmeasurements while control of the IMU navigator heading error is one of the objectives of thecurrent SARMCS Kalman filter. Consequently, the SARMCS Kalman filter can be modifiedto estimate the antenna azimuth misalignment by adding a new state, e, and a new measurement,zA. The new state models the antenna azimuth misalignment as a ran dom constantt =0^ (3-3)and the new measurement of antenna azimuth misalignment is given by64zA =t,TAU-6c^ (3-4)where /9 TAU is the TAU-determined antenna azimuth and 19c is the computed value based on theIMU navigator position and heading and the known target location.Modelling of a state by a random constant is often considered risky. This is because, with noprocess noise, the Kalman filter begins to rely on the process model after a few measurementsand will not follow any small or slow variations which may occur in the true state. Estimationof the antenna azimuth misalignment is expected to be completed relatively quickly, however,and so the random constant model is reasonable here. Should testing reveal problems, a smallamount of process noise can be added to produce a random walk model. Alternatively, anexponentially correlated Markov process with a very long correlation time could be used tomodel the antenna misalignment.The linear measurement model corresponding to (3-2) is obtained by perturbing (3-4). First,?AU and t,c are written asTAU =6 +8 +VA6c=t9 +61965where 0 is the true antenna azimuth, e is the antenna azimuth misalignment, VA is the noiseassociated with the TAU/azimuth encoder measurement and SO is the error in the computedantenna azimuth. The azimuth matching measurement is thenzA = TAU -0C = e-Oi9+vAUsing 0=04 and 1k =E-0/ the measurement can be re-written aszA =e -6)3+R-ba+v A^ (3-5)It remains to express 5,3,^and Oa in terms of the defined states. In subsection 2.3, bearingis given by=arctan [ —xwhere x and y are computed using (2-14) through (2-16). Perturbing this equation yieldsof3 = y^xx2+y 2^X 2 +y 2 -766Within the operating limits on aircraft altitude and TAU range, 6x is approximately equal to thewest position error and by is approximately equal to the south position error (see Figure 2-4) sothat 513 can be written in terms of the position errors in wander azimuth coordinates as613=  Y  (SR sin a +R cos a) -  x ( -5Rxcos a +R sin a)Y^ YX 2 +y 2^x X 2 +y 2(3-6)Note that target position errors are assumed negligible and IMU position errors are "corrected"IMU navigator quantities and consequently are small (approximately equal to the GPS positionerrors). Since the target range is typically tens of thousands of metres, the ratio of positionerror to range is on the order of 1/1000 making the linearized 6f3 approximation very good.An expression for SE is obtained by considering thatCr, = CI;C fb = [I-0 xiC;,where q is the direction cosine matrix that relates the IMU body frame to the true wanderazimuth navigation frame (defined by the true roll, pitch and azimuth: 4), 0 and E) and q is thedirection cosine matrix that relates the IMU body frame to the analytic platform frame (definedby the computed roll, pitch and azimuth: 0+50, 0+60 and +6). Solving for 4 results in thedesired expressionSE =0, -(4)cos E -oysin )tan 0^ (3-7)67An expression for Sa was found in subsection 2.5 and given in equation (2-46) which is repeatedhere for convenienceSu= --(5X sin (pSubstituting for oX in terms of the position errors, as given in equation (2-44), the desiredexpression for 450/ is[ ORxsin +SR= ^N+ycos aOa  tan coh(3-8)Recalling that the IMU navigator error quantities in (3-6) through (3-8) must be constructed fromxs+xm and substituting the resulting equations into (3-5), the antenna azimuth misalignmentmeasurement model is obtained. Since the TAU/azimuth encoder measurement error vA ismodeled as white noise, the measurement model is in the form required by the Kalman filter.The measurement model can be simplified by noting that the IMU is roll and pitch stabilized sothe terms in (3-7) which are multiplied by tan 0 are negligible and can be dropped. Theresulting measurement model is-x cos a -y sin a _ tan so sin a oRa+6RAL,)^zA =    N+hX 2 +y 2+ X sin a -y cos a_ N+htan co cos ax 2 +y 2^(OR -s-ORM)y) +(osz +Om) +8 +VAk sy(3-9)68The complete antenna azimuth calibration Kalman filter process and measurement models arenow obtained by adding g and zA to the SARMCS Kalman filter described in subsection 3.1.The calibration Kalman filter 25-element state vector has the form-x=X mXINSX GPSX sX IMUX A -.where the subvectors xm, Xmrs, XGps, Xs and 'caw are defined in subsection 3.1, and xA=e. Sincethe new state models a random constant, there is no associated dynamics or process noise andthe corresponding elements of the calibration Kalman filter system dynamics matrix F, processnoise vector w, and process noise spectral density matrix Q are zeros.The calibration Kalman filter 5-element measurement vector has the formwhere the zG and zp elements are defined in subsection 3.1.The measurement noise vector has the same form as the measurement vector, namely69V k =and the measurement noise covariance matrix is given by0000-0002 nCIp V 020 Up 000 2CrA-2CrGRk =^ooo-where VG, vp, aG and crp are defined in subsection 3.1 and (TA is the standard deviation of theazimuth matching measurement noise due to the TAU/azimuth encoder measurement noise vA•Finally, the measurement matrix Hk has the form[11Gim 0 IIG/Gps 0 0 0Hk = 0 0 0 H,50 0FIA/Af 0 0 It" 0 HA/A--70where the submatrices HG/m, HG/Gps and Hp/5 are defined in subsection 3.1 and, from (3-9), thesubmatrices HA/m and HAis are given byHA/m = HAls = [  -x cos a -y sin a _ tan yo sin a x sin a -y cos a _ tan co cos aN+h^ N+hx 2 +y 2 X 2 +y 20 0 0 0 1]and HAL4 iSHA/A = 13.3 DISCRETE TIME COMPUTATIONFor digital computer implementation, the discrete form of the Kalman filter is most appropriate.The computations can be divided into extrapolation of the state vector and error covariancematrix using the process model, followed by an update using the measurement data and themeasurement model. The discrete time Kalman filter extrapolation equations areIlk- =4) k-1*; -1^ (3-10)13-k =Sk-1Pk+-14)T-1+Qk-1^ (3-11)71where tic is the estimated state vector, Pk is the error covariance matrix and the minus and plussuperscripts indicate values prior to incorporation of the measurement and after incorporationof the measurement, respectively. The discrete time Kalman filter update equations areft; =ii ÷Kkkk .-11k541^(3-12)P+k iI-Ktilkil'i^ (3-13)where Kk is the Kalman gain matrix given byKk =Pillik[HkrkHT+Rk]l^(3-14)The discrete time formulation requires a process model having the form of equation (3-1) anda measurement model having the form of equation (3-2). In subsection 3.2, the measurementmodel is given in discrete form but the process model is in continuous form. One method forcomputing the discrete time state transition matrix II)k and process noise covariance matrix Qkfrom the continuous time system dynamics matrix F(t) and process noise spectral density matrixQ(t) is described here.A discrete time processing interval of ten seconds is used for the current SARMCS Kalmanfilter. Since the modification for antenna azimuth calibration has not introduced any short time-constant dynamics, a time interval of comparable magnitude is also appropriate for thecalibration Kalman filter. This time interval is very much less than the system time constants72(ie., 5000 seconds for the Schuler period and typically greater than 10,000 seconds for theexponentially correlated processes) and is generally much less than the time required forsignificant changes in the time varying system dynamics (ie., no high rate manoeuvres). clik andQk can, therefore, be approximated by Taylor series expansions having only a few terms.If the state transition matrix from time T to time t is (t , r) and the process noise covariancematrix is Qk(t) then the differential equations describing Sk and Qk (Gelb 1974) can be writtenas(t ,r)^(t)c1) (t ,T) ,^(1)(t ,t)dtdQk(t) =F(t)Qk(t)+Qk(t)Fr(t)+Q(t) , Qk(0) =0dtUsing these equations, the truncated Taylor series approximation for tk can be found asN,At n4)(t + At ,t)^—FnO n!(3-15)73where F is assumed constant over the interval At and Ar<p+ 1 is the number of terms in the series.Similarly, the process noise covariance matrix Qk is computed from the process noise spectraldensity matrix Q using the truncated Taylor series approximationn nQk E^)Fn -ntwir -1n=1 n!(3-16)where both F and Q are assumed constant over the interval At and NQ+1 is the number of termsin the series. The coefficients ( n-1) are binomial coefficients.m-1/3.4 FEEDBACK CONFIGURATIONThe high drift IMU gyros can lead to large navigation errors in the IMU inertial navigator.Since the linear inertial navigation error model assumes small errors, error control feedback isemployed.Consider a system whose error dynamics are described by (3-1) and assume that aiding sourcesprovide for measurements of the system output errors according to (3-2). The objective is toestimate the errors and correct the system outputs using a feedback configuration. To do this,the system outputs are corrected at each discrete Kalman filter update time tk using the bestavailable Kalman filter estimated errors. In a real-time implementation, the best availableestimate of the error state vector at time tk is tk_1 since the calculation of 4)1,4 and theincorporation of the measurements zk are still in progress. The corrected system error dynamicsand measurements are given by74X k =c11k-1 Xk—l —fik-1 +Wk-1zk=Hkxk+vkThe corresponding Kalman filter extrapolation and update equations becomeXk=srk—iXk-1—k-1i+k=fik +Kkkk E[kiiklwhere the measurements zk are obtained after the corrections are applied to the system outputs.In applying the foregoing results to the antenna boresight calibration Kalman filter, only the xsand ximu states are affected since feedback is only applied to the IMU navigation quantities andthe IMU gyro bias calibration quantities. Furthermore, the estimates used to correct the IMUnavigation quantities are obtained from xs+xm rather than xs alone. Table 3-2 lists theoperations performed at each Kalman filter update time tk.75Table 3-2 Operations Performed at Each Kalman Filter Update Time1. Correct the IMU navigation quantities using xs+xm and the IMU gyro biasquantities using x/mu.2. Construct Zk, Hk and Rk (using corrected quantities).3. Construct F and Q (using corrected quantities).4. Compute (1)k-1 and Qk1 from F and Q.5. Extrapolate usingSi- =4) t+k^k-i k-1P k- =43 k-1P;-11:11111+Qk-16. Subtract the corrections applied in step 1 from the xs and 'caw subvectors.7. Update usingKk =Pal ikiHkPiH T+Rk] 1+k =5ik- +1(kkkP; -KkHdP;76SECTION FOURSIMULATION SOFTWAREA computer simulation software package was developed to analyze the antenna boresightcalibration Kalman filter. The simulation software is coded in the C+ + language and runs ona personal computer under the DOS operating system. It is divided into a synthesis package,a processing package and an evaluation package.The synthesis package consists of programs to generate a reference (error free) trajectory andsynthesized sensor data. The package is designed to make specification of the flight profile andgeneration of the corresponding synthesized sensor data a simple and comparatively quickprocess to facilitate experimentation. This complements the SARMCS synthesis package whichmore accurately simulates the sensor environment and sensor errors but at the cost ofconsiderably more set-up effort and run time.The processing package performs the Kalman filter estimation of the antenna azimuthmisalignment using the synthesized sensor data as input. During processing, variousintermediate results of interest are saved to files for subsequent analysis. The package can alsoprocess data that has been generated using the SARMCS synthesis package or has been recordedfrom real sensors during flight trials of the SARMCS.The evaluation package facilitates reading of the generated data into MATLAB for plotting andanalysis. MATLAB is a commercially available product for scientific and engineering numericcomputation and graphing.trajectorycommand --,...filesynthesispackageprocessing^11. packageevaluationpackageI77SARMCSsimulator datarealsensor data - - -iMATLAB-Bo- datafilesreference trajectoryFigure 4-1 Block Diagram of Simulation PackageFigure 4-1 shows the relationship between the synthesis package, the processing package and theevaluation package.4.1 TRAJECTORY GENERATIONThe trajectory generator consists of two programs called TRJO and TRJC. Program TRJO readsa file of English text commands describing the flight and produces a file of records containingposition (earth-fixed x, y, z coordinates) and attitude (roll, pitch, heading) sampled at discretetime intervals. Each record also contains the sample time beginning at time zero for the firstrecord. The input command file has a default file name extension of .CMD. The outputtrajectory file takes a default file name from the command file name and applies a default filename extension of .TJO.78Program TRJC reads the trajectory file and produces a file of coefficients for cubic splineinterpolation of the position and attitude. The trajectory interpolation coefficient file takes adefault file name from the trajectory file name and has a default file name extension of . TJC.As an example, given a trajectory generation command file named TEST.CMD, the followingsequence of DOS commands generate a trajectory file named TEST. TJO and a trajectoryinterpolation coefficient file named TEST.TJC:TRJO TESTTRJC TEST4.1.1 The Command FileThe trajectory generation command file is produced using a text editor. A typical command fileis shown in Table 4-1.Following is an example of a typical command:at 100 change altitude to 1000 in 100;In general, a command consists of a condition part (at 100) an action part (changealtitude to 1000), a duration part (in 100) and a terminator (; ). The condition partspecifies the time at which the action is to be initiated, relative to the start of the simulation orto the most recently defined reference mark. The action part defines the action to be performed.The duration part specifies the duration over which the action is performed.79Table 4-1 Typical Trajectory Generation Command File% TEST.CMD% Test command file to generate a 5020 second% Initial stationary position at beginning ofinitialize position to (49:12:00,-123:45:00);initialize altitude to 0;initialize groundspeed to 0;initialize track to 45;at 10 mark;trajectory.runway.% Takeoff and initial climb.at 0 change groundspeed to 120 in 20;at 20 change altitude to 1000 in 100;at 50 change groundspeed to 200 in 100;at 120 change altitude to 5000 in 1000;mark;% Manoeuvres at altitude.at 70 change track by 180 in 60;at 400 change track by -90 in 30;% End.at 3890 end;Every command must include an action part but not all actions support a condition part and aduration part. When supported, the condition and duration parts are optional. When a conditionpart is supported but not specified, the action is initiated when all previous actions arecompleted. When a duration part is supported but not specified, the action is performed in apredetermined default duration of nominal value.80There are four action types identified by the action keywords initialize, change, markand end, and there are two action modifiers identified by the keywords to and by. Whenpresent, the condition part is introduced by the keyword at and the duration part is introducedby the keyword in. Allowed commands take one of the following forms:initialize variable to value;[at time] change variable tolby value [in time];[at time] mark;[at time] end;where the square brackets indicate an optional part and the vertical bar indicates a choice.Initialize commands set the initial values of the navigation variables and must appear before anyother commands. Change commands describe the dynamics of the flight by specifying how thenavigation variables change. Either absolute (to) or relative (by) change values may bespecified. The mark command allows the reference time to be changed. This permits commandsequences for special manoeuvres to be reused. The end command indicates the end of thetrajectory generation. It must appear exactly once at the end of the command file.In addition to the time variable, there are four action variables whose identifiers are:position, altitude, track and groundspeed. Time values are always specified inseconds, horizontal position values are specified as latitude/longitude pairs in units of degrees,minutes and seconds, altitude is specified in feet, track angle is in units of degrees and groundspeed is specified in nautical miles per hour.81Comments may also be included in the command file. The percent character % introduces acomment. All characters between the percent sign and the end of the line are ignored by thecommand parser.4.1.2 Trajectory CalculationThe generated trajectory is defined by a sequence of position and attitude samples at 0.1 secondintervals and the use of cubic spline interpolation. Cubic spline interpolation is convenientbecause it not only provides values on a continuous curve through the sample points (position),but also values on a continuous first derivative (velocity) and a piecewise continuous secondderivative (acceleration). The cubic spline interpolation algorithm is described in (Press et al.1986).Calculation of the position and attitude samples is performed in three stages. First, thetrajectory generation commands are processed and coarse values of position and attitude arecomputed. At this point, the altitude profile consists of linear segments and the horizontalposition profile consists of segments of great circles and small circles (turns) with a piecewiselinear ground speed. A roll angle (bank angle) is computed during turns (that is, during a trackchange command) from simple equilibrium conditions. The pitch angle is computed using asimplified model relating lift force to angle of attack, velocity and altitude. For details of theroll and pitch calculations see (Von Mises 1959). The second stage smooths the trajectory usinga zero phase (no delay) low pass filter. The filter is applied to each of the x, y and z earth-fixedposition coordinates and to the roll and pitch angles. The smoothing, combined with a smallsampling interval, results in a well behaved cubic spline interpolation. Note that turbulence andflexible body dynamics are not included in this simulation package (they are included in the82SARMCS simulation package). In the final stage, the heading is computed. A zero yaw angleis assumed so that heading is equal to the track angle. Since track angle is dependent on thealready computed trajectory, heading is not an independent parameter, however, it is convenientto include heading (and consequently, its first and second derivatives) in the trajectory file. Toobtain heading, the direction of the horizontal component of velocity, relative to north, iscomputed.To simplify the calculations, horizontal position is computed using a spherical earth model andassuming zero altitude. Motion along a particular great circle or small circle is calculated byrotating the position vector about a fixed axis of rotation at an angular rate which depends onthe ground speed. The resulting geocentric latitude and longitude are then converted torectangular earth-fixed coordinates as if they were geodetic latitude and longitude computed onthe ellipsoid. The effect of these approximations is a ground speed which differs slightly fromthe commanded value due of the different radius of curvature. It is most noticeable in a turnbecause the radius of curvature of the ellipsoid is dependent on the direction of travel whichchanges quickly during the turn.Figure 4-2 shows the horizontal component of a flight profile generated using the trajectorycommand file in Table 4-1. The evaluation package was used to convert the trajectory earth-fixed coordinates to geodetic latitude and longitude and plot them in a map-like format.83Figure 4-2 Flight Profile4.2 SENSOR DATA SYNTHESISSensor data synthesis is accomplished by a set of sensor simulation programs (a dedicatedprogram for each sensor). These programs use the trajectory interpolation coefficient file tocompute ideal sensor measurements which are then corrupted according to the sensor errormodel. Table 4-2 lists the sensor data synthesis programs and summarizes the synthesized datacharacteristics.84Table 4-2 Sensor Data Synthesis ProgramsSensorProgramSynthesizedDataErrorModelSampleIntervalINS latitudelongitudeground speedtrack angleheadingpitchrollbody pitch ratebody roll ratebody yaw rateplatform headingaltitudevertical speednorth velocityeast velocityaccelerometer errors: first-orderGauss-Markov (bias)gyro errors: first-order Gauss-Markov (bias) plus white noise(random drift)62.5 msIMU accelerometer AV'sgyro AO'saccelerometer errors: first-orderGauss-Markov (bias)gyro errors: first-order Gauss-Markov (bias)20 msGPS latitudelongitudealtitudenorth velocityeast velocitydown velocityposition errors: first-orderGauss-Markov; independentwhen expressed in geographiccoordinatesvelocity errors: white noise;independent when expressed ingeographic coordinates1 sAZM antenna azimuth white noise 10 sALT altitude none 0.5 sTo synthesize data from a sensor, the appropriate sensor simulation program is run with thename of the desired trajectory interpolation coefficient file specified on the DOS command line.The simulation program requests start and end times for the simulation (within the limits of the85trajectory data) then generates a file containing the time stamped sensor data records. Bydefault, the sensor data file takes the name of the trajectory coefficient data file with a file nameextension dependent on the sensor being simulated. For example, the following DOS commandline and prompt responses generate 1000 seconds of synthesized IMU data in a file namedTEST. imu from a trajectory interpolation coefficient file named TEST. TJC:IMU TESTEnter interpolation start time between 0 and 5020: 1000Enter interpolation end time between 1000 and 5020: 2000The sensor simulation programs are independent of each other, however, to generate a consistentset of synthesized sensor data files corresponding to a simulated flight, the same trajectorycoefficient file must be used by each program and, typically, the same start and end times arespecified for each sensor simulation.4.2.1 INS Data SynthesisThe INS data synthesis program is called INS. It generates simulated INS data in a file witha default file name extension of . INS. The generated INS data includes latitude, longitude,ground speed, track angle, heading, pitch, roll, body pitch rate, body roll rate, body yaw rate,platform heading, altitude, vertical speed, north velocity and east velocity, all at an update rateof 16 Hz. To generate the INS data, ideal accelerometer and gyro measurements are computedfrom the trajectory data and corrupted with random errors then integrated in a strapdown inertialnavigation algorithm.86The INS is assumed to be located at the centre of mass of the aircraft so that no lever armcorrection to the trajectory data is required. Specific force and angular velocity are computedin body coordinates at a 256 Hz rate and integrated using a five point Newton-Cotes algorithm(Press et al. 1986) to obtain the accelerometer AV's and the gyro AO's at 64 Hz. Thesemeasurements are corrupted by errors before being used in the strapdown inertial navigationalgorithm. The modeled errors include accelerometer and gyro biases and gyro random drift.The biases are modeled as first-order Gauss-Markov processes with long correlation times andthe random drift is modeled using white noise. This simple error model is adequate forevaluating the effects of flight trajectory and aircraft/target geometry on the Kalman filterperformance. Finally, the 64 Hz navigation quantities are subsampled to 16 Hz and written tothe INS data file.4.2.2 11114U Data SynthesisThe IMU data synthesis program is called DIU. It generates simulated accelerometer and gyromeasurements in a file with a default file name extension of . Imu.The IMU is mounted on the roll and pitch stabilized antenna ring gear. Synthesis of the IMUdata must take into account the lever arms from the aircraft centre of mass to the antennaroll/pitch intersection and from the roll/pitch intersection to the IMU. The first lever arm isfixed in vehicle body coordinates and the second is fixed in stabilized antenna ring gearcoordinates. Since the second lever arm is much shorter than the first, it is not significant forsimulating the IMU dynamical environment. Nor is it significant relative to the large flighttrajectory and aircraft/target geometry distances. Therefore, the IMU data synthesis calculationsare simplified by assuming that the IMU is located at the antenna roll/pitch intersection.87Specific force and angular velocity are computed in IMU body coordinates at a 200 Hz rate andintegrated to obtain accelerometer AV's and gyro AO's at 50 Hz. These measurements arecorrupted according to the IMU error model. A first-order Gauss-Markov error model is usedto generate accelerometer and gyro noise that is exponentially correlated in time. Themeasurements are then written to the IMU data file.4.2.3 GPS Data SynthesisThe GPS data synthesis program, called GPS, generates simulated GPS position and velocitymeasurements in a file having a default file name extension of . GPS. The position and velocitydata describe motion at the GPS antenna which is affixed to the top of the aircraft fuselage and,therefore, is displaced from the aircraft centre of mass by a fixed lever arm. Lever armcompensated position (latitude, longitude and altitude) and velocity (north, east and down) arecomputed from the trajectory data at one second intervals and corrupted according to the GPSerror model before being written to the GPS data file.The errors in GPS-indicated latitude, longitude and altitude are modelled as first-order Gauss-Markov processes and the errors in the GPS indicated north, east and down velocities areassumed to be uncorrelated from measurement to measurement. Although the three componentsof GPS position error (or velocity error) might properly be considered to be independent whenexpressed along the pseudo range directions, they are likely to be correlated with each otherwhen expressed in geographic coordinates. The model used for the simulator, however,generates errors which are uncorrelated when expressed in geographic coordinates.884.2.4 Azimuth Encoder Data SynthesisSynthesized azimuth encoder measurements are generated by a program called AZM and writtento a file having a default file name extension of . AZM. During calibration, the TAU keeps theantenna pointing at the target and the azimuth encoder measures the antenna azimuth angle. Thisangle is given by the target bearing minus the aircraft heading plus the antenna azimuthmisalignment.To synthesize the antenna azimuth encoder data, the position of the antenna roll/pitchintersection is computed at ten second intervals using the trajectory data and correcting for thelever arm from the aircraft centre of mass. Then, the target bearing is determined using thecomputed antenna roll/pitch intersection position and the known target position. Aftersubtracting aircraft heading obtained from the trajectory data, and adding a small fixed errorrepresenting the antenna azimuth misalignment, the resulting azimuth value is corrupted usingthe TAU error model. Simulated TAU errors are uncorrelated from measurement tomeasurement. Finally, the synthesized measurements are written to the azimuth encoder datafile.Figure 4-3 is a plot of the synthesized azimuth encoder measurements corresponding to the flightprofile and target location shown in Figure 4-2. A section of the graph is expanded in Figure4-4 to show the measurement noise introduced by the TAU error model. Note that synthesizedazimuth encoder measurements are generated for the entire flight profile. For the purpose ofanalyzing the Kalman filter, only those measurements corresponding to realistic TAU operatinglimits are used.Figure 4-3 Azimuth Encoder Measurements179.5179178.5...,g 178cneE 176.5*r7,o176175.51753000 3200 3400 3600 3800 4000 4200 4400 4600 4800 5000time (seconds)Figure 4-4 Azimuth Encoder Measurements Showing TAU Error89904.2.5 Baroaltitude Data SynthesisA reference altitude is required to stabilize the vertical channel of the strapdown inertialnavigator. Synthesized barometric altitude data is generated by a program called ALT andwritten to a file with a default file name extension of . ALT. Since altitude errors do not directlyaffect bearing calculations and are not modelled in the Kalman filter, no altitude errors areincluded in the simulator. Altitude is computed at 0.5 second intervals using the trajectory dataand written to the altitude data file.4.3 DATA PROCESSINGThe data processing package implements the antenna boresight calibration Kalman filter in aprogram called CALPRO. The software is divided into a main program, a navigation component,a generic Kalman filter component and a system model component.The main program handles housekeeping chores associated with the reading of files containingsensor data, the writing of processing results to files for later analysis and the execution of theinertial navigation and Kalman filtering procedures.The navigation component includes an implementation of the strapdown inertial navigationalgorithm in a procedure called navgtr and a collection of commonly used, navigation relatedprocedures in a library named NAVLIB.The generic Kalman filter component implements the basic Kalman filter equations for stateextrapolation and measurement updates. Also included is a procedure for Taylor seriesapproximation of the discrete time state transition matrix and process noise covariance matrix91given the continuous time system dynamics matrix and process noise spectral density matrix.The procedures, called extrap, updat and PHIQd, respectively, are independent of anyparticular system model. The basic Kalman filter equations are directly implemented except thatonly the upper triangular portion of the error covariance matrix is computed and used (thesymmetry property is used to determine the lower triangular portion). Since the simulatedflights are of relatively short duration and the Kalman filter calculations are performed usingdouble precision arithmetic, no special precautions are necessary in the simulator software toensure the positive definiteness of the error covariance matrix.The system model is defined in a module with the generic name MODEL. The name is easilychanged to something more descriptive or to accommodate multiple, uniquely named models.MODEL contains a framework for all of the data storage allocations and data processingprocedures which are required by the generic Kalman filter component and which are specificto a particular system model. Working storage is allocated for: the state vector; themeasurement vector; the continuous time system dynamics matrix; the continuous time processnoise spectral density matrix; the discrete time state transition matrix; the discrete time processnoise covariance matrix; the measurement matrix; the measurement noise covariance matrix; theerror covariance matrix; the Kalman gain matrix; and temporary vectors and matrices forintermediate results. Procedures are provided to: return pointers to the allocated workingstorage vectors and matrices; initialize the state vector and the error covariance matrix; constructthe continuous time system dynamics matrix and process noise spectral density matrix; andconstruct the measurement vector, measurement matrix and measurement noise covariancematrix. The generic procedure names are mode l_wrk, mode l_x 0 P 0 , mode l_FQ andmodel mes, respectively. To create a specific system model, the number of states and the92maximum number of measurements must be specified and the bodies of three of the procedures(model _ x0P0, model FQ and model mes) must be provided. The specific system model_^_and the generic Kalman filter combine to form a basic Kalman filter implementation as shownin Table 4-3.Table 4-3 Basic Kalman Filter Using System Model and Generic Kalman Filter ProceduresPerform once at initialization:model _wrk^Enable access to the working storage.model x0P0^Initialize the state vector and the error covariance matrix._Perform at each filter iteration (F and Q are assumed to be time varying):model_FQ^Construct the continuous time system dynamics matrix andprocess noise spectral density matrix.PHIQd^Compute the discrete time state transition matrix andprocess noise covariance matrix.extrap^Extrapolate the state vector and the error covariance matrixbased on the process model.model _mes^Construct the measurement vector, the measurement matrixand the measurement noise covariance matrix.updat^Update the state vector and the error covariance matrixbased on the measurement model and the measurement.934.4 DATA ANALYSISThe evaluation package relies on MATLAB for data analysis and plotting. Within MATLAB,entire data vectors are viewed as individual objects (the fundamental data object in MATLABis the matrix which includes vectors and scalars as special cases). MATLAB permitsmathematical operations and graphing operations to be performed on the data vectors eitherinteractively or by executing files of MATLAB statements called M-files.A data conversion program called MATCNV converts data files generated by the synthesispackage and the processing package to MATLAB format. MATCNV reads binary files of fixedlength records containing only double precision elements (all of the files generated by thesimulation software package conform to this format) and generates a set of MATLAB files (onefile for each record element) containing a single data vector each. M-files have been written toautomate the process of converting the data and reading it into MATLAB. MATCNV alsoprovides the capability to subsample the input data file. For example, a data file from asimulated two hour flight containing sample records at 20 millisecond intervals is too large tobe plotted conveniently. MATCNV can be used to subsample by a factor of, say, 50 so that onlythe samples at one second intervals are read into MATLAB for plotting.The evaluation package also includes a program called NAVO which converts the referencetrajectory file position and velocity from earth-fixed coordinates into the more standardnavigation quantities of geodetic latitude, longitude, altitude, ground speed, track angle andvertical speed.94Figure 4-5 demonstrates some of the capabilities of the evaluation package. It is a comparisonbetween the IMU ground speed and the reference ground speed. The trajectory was generatedusing the command file in Table 4-1. IMU ground speed was computed by the inertialnavigation component of CALPRO using synthesized IMU accelerometer and gyro data. Thereference ground speed is the error free aircraft centre of mass (or INS) ground speed obtainedfrom the reference trajectory file using the NAVO conversion program to convert from earth-fixed coordinates. MATCNV and the appropriate M-files were used to load the data intoMATLAB where it was differenced and then plotted.0.030.021-OsCo0V 0.01CII-CC.co0ye"—0.01>.e-'Uo> —0.02o4,V-0—0.03—0.04^^0^500 1000 1500 2000 2500 3000 3500 4000 4500 5000time (seconds)-•Figure 4-5 Difference Between Computed IMU Ground Speed and Reference Ground Speed95The spikes in the graph are the result of the INS-to-IMU lever arm in combination with pitchchanges during takeoff and roll changes during turns. Also seen is the 84-minute Schuleroscillation (observable during the period of straight and level flight starting from about 2000seconds) which is characteristic of inertial navigation errors. For this example, the IMU errormodel was disabled, thus, the velocity error is due primarily to numerical integration errors inthe inertial navigation algorithm.96SECTION FIVEFLIGHT TRAJECTORY AND AIRCRAFT/TARGET GEOMETRYIt is necessary to determine a flight profile and aircraft/target geometry that will allow theantenna azimuth misalignment to be rapidly and accurately estimated. This determination isperformed by using the simulation software package to generate various profiles and geometriesand to analyze the results. By comparing error covariances, an appropriate choice of flightprofile and aircraft/target geometry can be made.5.1 PRELIMINARY CONSIDERATIONSThe selection of possible flight profiles and aircraft/target geometries can be narrowed byconsidering the theoretical requirements for estimating the antenna azimuth misalignment andthe operational limitations of the aircraft and other equipment.Nominally straight and level flight at constant ground speed is operationally convenient. It alsominimizes the effects of instrument scale factor and misalignment errors, and minimizes errorsin constructing the time varying state transition matrix. Conversely, manoeuvres are requiredto generate the horizontal components of acceleration necessary for separation of accelerometerbiases from misalignments and, more important, for prompt estimation of azimuth misalignment.When manoeuvres are performed while making TAU antenna azimuth measurements, however,the measurements are susceptible to timing related errors due to the high antenna azimuth rates.Given these considerations, a reasonable flight profile consists of manoeuvres followed by aperiod of straight and level flight during which the antenna azimuth misalignment is estimated.97A simple manoeuvre that provides the necessary horizontal accelerations is a 180 degree turn.Another simple manoeuvre, that also preserves the original direction of travel, is an Sturn (twoconsecutive 180 degree turns in opposite directions). During the straight portion of the flightprofile, antenna azimuth rates can be kept near zero by flying directly toward the target. Also,potential problems associated with the antenna elevation angle and elevation rate can beminimized if the range between the aircraft and the target remains much greater than the aircraftheight above the target.A nominal altitude of 3300 feet (1000 metres) is a reasonable choice for the simulations (theprocess and measurement models used for the Kalman filter are virtually independent of aircraftaltitude). A convenient cruising speed for the aircraft is 210 nautical miles per hour (108 metresper second). Turn rates of 180 degrees/minute are appropriate for the manoeuvres. While theTAU azimuth measurements are being made, a maximum target range in the neighbourhood of100 kilometres and a minimum target range of about 10 kilometres represent reasonable limits.5.2 SIMULATION METHODSThe purpose of these simulations is to determine a suitable flight profile and aircraft/targetgeometry by analyzing the error covariance. The simulations are performed using the computersimulation software package (listings of the software modules comprising the data processingpackage are included in Appendix C). To calculate the error covariance Pk, it is necessary toknow ckk, Qk, Hk, Rk and P6. Although it is not strictly necessary to process measurements andgenerate the state estimate, these operations are performed to provide a reasonability check aswell as additional insight into the filter operation.98Formulation of the F, Q and Hk matrices is described in Section 3. For these simulations, aKalman filter processing cycle is performed for each GPS sensor data record (that is, at fixedone second time increments), except for antenna azimuth matching measurements (which areonly available at ten second intervals). Truncated Taylor series approximations are used toobtain 4)k (equation (3-15) with N=3) and Qk (equation (3-16) with Nt2=6). The simulationsare performed using values for the diagonal matrices Q, P6 and Rk as shown in Tables 5-1, and5-2. In Table 5-1, the process noise spectral densities corresponding to exponentially correlatedprocesses are written in the form 2 x a24- T so that the mean square values (72 and the correlationtimes r (in seconds) are easily seen. The quantities a, Km and I 0.), I appearing in some of thespectral densities are defined as follows:a^= magnitude of the aircraft acceleration vector in m/s2.Vod = aircraft ground speed in m/s (hard-limited to a minimum value of 70 m/s).I co, I = absolute value of the aircraft heading rate in rad/s.The values for the rms error in the uncalibrated antenna azimuth (see the initial error covariancein Table 5-1) and the rms error in the antenna azimuth matching measurements (see the noisecovariance in Table 5-2) have been somewhat arbitrarily chosen for these simulations. Theuncalibrated antenna azimuth rms error is assumed to be of the order of one degree. TAUazimuth measurements have an rms error of 0.05 degrees based on experiments with a stationarySARMCS radar and target. The in-flight antenna azimuth matching measurements are assumedto include an additional error component of about 0.1 degrees due to time offsets between thecompared azimuth values (more on this later).99Table 5-1 Process Noise Spectral Density and Initial Error CovarianceState Process Noise Spectral Density Initial Error Covariance612mx 0 (m/s)2/Hz 1 x106 (m)2SRmy 0 (m/s)2/Hz 1 x106 (m)2SVAL, (6 x10-3)/Vgnd + (3.2 x 109)a V_ (m/s2)2/Hz 4 (m/s)2SVmy (6 x 103)/V ^(3.2 x 10-9)a lig„„ (m/s2)2/Hz 4 (m/s)2Om, 7.6 x10 (mrad/s)2/Hz 0.09 (mrad)2Om), 7.6x 10' (mrad/s)2/Hz 0.09 (mrad)2Omz 7.6 x10' (mrad/s)2/Hz 25 (mrad)2A x (2.4 x 1(Y7)+360,000 (m/s3)2/Hz 2.4 x 10-7 (m/s2)2Amy 2 x(2.4 x1(Y7)+360,000 (m/s3)2/Hz 2.4 x 10-7 (m/s2)2Gm, 2 x (2.4 x 10-9)+360,000 (mrad/s2)2/Hz 2.4x 10-9 (mrad/s)2Gmy 2 x (2.4 x 10-9)+360,000 (mrad/s2)2/Hz 2.4 x 10-9 (mrad/s)2Gmz 2 x (2.4 x 10-9)+360,000 (mrad/s2)2/Hz 2.4x 10-9 (mrad/s)2SIZa, 2 x400 +10,800 (m/s)2/Hz 400 (m)2ORGY 2 x400+10,800 (m/s)2/Hz 400 (m)2ORst 0 (m/s)2/Hz 100 (m)2OR (m/s)2/Hz 100 (m)2ON 1 sr (5.42 x 104)a V (m/s2)2/Hz 4 x 10-4 (m/s)2OVsy (5.42 x 108)a V_ (m/s2)2/Hz 4 x10-4 (m/s)2Os, (1.2 x 10-8)a Vgnd + (1.32 x10-1) I coz I (mrad/s)2/Hz 400 (mrad)2+ 7.6x107Osy (1.2 x 10-8)aygnd + (1.32 x 10-') I coz I (mrad/s)2/Hz 400 (mrad)2+ 7.6 x 10"(tosz (1.2 x 10-8)a V ^+ (7.1 x 10-2) I coz I (mrad/s)2/Hz 225 (mrad)2+ 7.6 x10'Gs, 2 x (2.1 x 10-6)+36,000 (mrad/s2)2/Hz 2.1 x 10-6 (mrad/s)2Gsy 2 x (2.1 x10-6)+36,000 (mrad/s2)2/Hz 2.1 x 10-6 (mrad/s)2Gsz 2 x (2.1 x 10-6)4-36,000 (mrad/s2)2/Hz 2.1 x10-6 (mrad/s)2E 0 (mrad/s)2/Hz 400 (mrad)2100Table 5-2 Measurement Noise CovarianceMeasurement Measurement NoiseCovarianceINS/GPS x-position matching 25 (m)2INS/GPS y-position matching 25 (m)2IMU/INS x-position matching 4 (n)2IMU/INS y-position matching 4 (In)2Antenna azimuth matching 4 (mrad)2Each sensor provides measurements at a particular location on the aircraft. For thesesimulations, the following location and lever arm assumptions are made:• The INS is located at the aircraft centre of mass (the aircraft body frame origin).• Barometric altitude measurements are taken at the aircraft centre of mass.• The GPS antenna is attached to the fuselage on the top of the aircraft and theINS-to-GPS lever arm is [-3.0, -0.5, -2.0]T metres in aircraft body coordinates.• The IMU is located at the intersection of the antenna roll and pitch axes and theINS-to-IMU lever arm is [6.0, -0.5, -0.1]T metres in aircraft body coordinates.In order to properly test the effectiveness of the flight profile manoeuvres, the effects of anyprior manoeuvres (for example, takeoff) must be excluded. This is achieved in the simulationsby initiating the data processing during a straight and level segment of the flight profile includedprior to the test manoeuvres. At least 1000 seconds of data processing is performed beforeinitiating the test manoeuvres to allow the Kalman filter to settle.101The following points must be kept in mind when analyzing the simulation results:• Turbulence and flexible body dynamics are not simulated.• Vertical deflections and gravity anomalies are not simulated.• Accelerometer and gyro scale factor errors, misalignments and g-sensitive errorsare not simulated.• Barometric altitude errors are not simulated.• Instrument quantization errors are not simulated.• Timing errors in matching measurements are not simulated.A consequence of the limited error models used in the data synthesis package is nearly optimalKalman filter performance when processing this data. The only significant errors not fullymodelled by the filter are the horizontal IMU accelerometer biases (vertical channel errors areminimized by baroaltitude damping). But SARMCS experience has shown that modelling thesestates in the filter does not significantly change steady-state performance. These simulations,therefore, provide a best performance baseline for subsequent evaluations.5.3 SIMULATION RESULTSThis subsection presents the results of the simulation runs. In analyzing the results, emphasisis placed on the ultimate objective of estimating the antenna boresight azimuth misalignment tothe required accuracy of 5 arc minutes rms or better in a minimum amount of time. Subsection5.3.5 summarizes the findings of many simulations. Detailed results of three representativesimulations are contained in subsections 5.3.2, 5.3.3 and 5.3.4 using flight profiles describedin subsection 5.3.1. These simulation runs are referred to as la, 2a and 3a, where the number102identifies the flight profile and the letter is used to distinguish between different target locationsor calibration times with the same flight profile. For convenience, the large number of plotsassociated with these simulations have been placed in Appendix A. Only selected plots areincluded in this section. All of the plots were produced using the computer simulationevaluation package described in Section 4.5.3.1 Flight ProfilesThree flight profiles are referenced in the following subsections. These flight profiles weregenerated using the computer simulation software package trajectory generator described inSection 4. The command files used to generate the flight profiles are listed in Tables 5-3through 5-5. In the command files, times and durations are specified in seconds, horizontalposition values are specified as latitude/longitude pairs in units of degrees, minutes and seconds,altitude is specified in feet, track angle is in units of degrees and ground speed is specified innautical miles per hour.Following the takeoff and initial climb, each flight profile has a nominal altitude of 3300 feetand a nominal ground speed of 210 nautical miles per hour. The manoeuvres are performed ata turn rate of 180 degrees per minute. These values correspond to those specified insubsection 5.1. In addition, each profile is generated at a mid latitude of about 45 degrees andis arbitrarily oriented in a nominally east/west direction. Plots of the horizontal plane of eachflight profile are shown in Figures 5-1 through 5-3. The " + " marks along the profile indicate100 second intervals from time 0 seconds at position 45 degrees north, 75 degrees west to time5000 seconds at the end of the profile.% Manoeuvres.at 2200 change track by -180at 2360 change track by 180at 3800 change track by 180% End.at 5020 end;in 60;in 60;in 60;103Table 5-3 Flight Profile 1 Command File% CALPRO.CMD% Command file to generate flight profile #1.% Initial stationary position at beginning of runway.initialize position to (45:00:00,-75:00:00);initialize altitude to 0;initialize groundspeed to 0;initialize track to 90; % And initialize wander angle to -90.% Takeoff and initial climb.at 10 change groundspeed to 120 in 20;at 30 change altitude to 1000 in 100;at 60 change groundspeed to 210 in 100;at 130 change altitude to 3300 in 660;4645.5 ■45 "'"-÷4--"-'-'.-4-4-1-1-4-4---4--1-4443.543—75^—74^—73^—72^—71^—70^—69longitude [deg]Figure 5-1 Flight Profile 1% CALPRO.CMD% Command file to generate flight profile #2.% Initial stationary position at beginning of runway.initialize position to (45:00:00,-75:00:00);initialize altitude to 0;initialize groundspeed to 0;initialize track to 90; % And initialize wander angle to -90.% Takeoff and initial climb.at 10 change groundspeed to 120 in 20;at 30 change altitude to 1000 in 100;at 60 change groundspeed to 210 in 100;at 130 change altitude to 3300 in 660;% Manoeuvres.at 2200 change track by -180 in 60;at 3260 change track by -180 in 60;% End.at 5020 end;44—75 —74.5 —74 —73.5 —73^—72.5 —72 —71.5 —714645.544.5104Table 5-4 Flight Profile 2 Command Filelongitude [deg]Figure 5-2 Flight Profile 243—75 —74 —73 —72 —71 —70 —694645.5454443.5% CALPRO.CMD% Command file to generate flight profile #3.% Initial stationary position at beginning of runway.initialize position to (45:00:00,-75:00:00);initialize altitude to 0;initialize groundspeed to 0;initialize track to 90; % And initialize wander angle to -90.% Takeoff and initial climb.at 10 change groundspeed to 120 in 20;at 30 change altitude to 1000 in 100;at 60 change groundspeed to 210 in 100;at 130 change altitude to 3300 in 660;% Manoeuvres.at 2200 change track by -180 in 60;at 2360 change track by 180 in 60;at 3420 change track by 180 in 60;at 3580 change track by -180 in 60;% End.at 5020 end;105Table 5-5 Flight Profile 3 Command Filelongitude [deg]Figure 5-3 Flight Profile 31065.3.2 Simulation Run laFlight Profile:^#1 (see subsection 5.3.1)Target Location:^n/aData Processing Start Time: 1000 sData Processing End Time: 5000 sCalibration Start Time:^n/aCalibration End Time:^n/aSpecial Conditions:^• Data processing initiated during straight and levelflight prior to manoeuvres.• Manoeuvres consist of an S-turn at 2200 seconds anda 180 degree turn at 3800 seconds.• No antenna boresight calibration.Plots:^la-1 True and estimated x INS position error(see la-2 True and estimated y INS position errorAppendix^la-3 True and estimated x INS velocity errorA)^la-4 True and estimated y INS velocity errorla-5 True and estimated x INS platform misalignmentla-6 True and estimated y INS platform misalignmentla-7 True and estimated z INS platform misalignmentla-8 True and estimated x INS accelerometer biasla-9 True and estimated y INS accelerometer biasla-10 True and estimated x INS gyro biasla-11 True and estimated y INS gyro biasla-12 True and estimated z INS gyro biasla-13 Error in x INS position error estimatela-14 Error in y INS position error estimatela-15 Error in x INS velocity error estimatela-16 Error in y INS velocity error estimatela-17 Error in x INS platform misalignment estimatela-18 Error in y INS platform misalignment estimatela-19 Error in z INS platform misalignment estimatela-20 Error in x INS accelerometer bias estimatela-21 Error in y INS accelerometer bias estimate107la-22 Error in x INS gyro bias estimatela-23 Error in y INS gyro bias estimatela-24 Error in z INS gyro bias estimatela-25 Error in x GPS position error estimatela-26 Error in y GPS position error estimatela-27 Error in corrected x IMU positionla-28 Error in corrected y IMU positionla-29 Error in corrected x IMU velocityla-30 Error in corrected y IMU velocityla-31 Error in corrected x IMU platform alignmentla-32 Error in corrected y IMU platform alignmentla-33 Error in corrected z IMU platform alignmentla-34 Error in x IMU gyro bias estimatela-35 Error in y IMU gyro bias estimatela-36 Error in z IMU gyro bias estimatela-37 x INS/GPS position matching measurement residualla-38 y INS/GPS position matching measurement residualla-39 x IMU/INS position matching measurement residualla-40 y IMU/INS position matching measurement residualSimulation run la is performed with antenna boresight calibration disabled. The simulationserves three purposes. First, it demonstrates the operation of the current SARMCS Kalmanfilter design as implemented in the simulator data processing package. Second, with antennaboresight calibration disabled, it forms a basis for observing the effects of enabling antennacalibration. Third, it confirms the effectiveness of the S-turn and the 180 degree turn forazimuth misalignment estimation and it provides additional insight into manoeuvre requirements.To serve these purposes, an extensive set of plots (1a-1 through la-40) is included inAppendix A.Plots la-1 through la-12 show true and Kalman filter estimated INS errors on the same graph.In each graph, the total INS error and the relative magnitude of the corresponding estimationerror are readily observed. Plots la-13 to la-24 graph the difference between the estimated and108true INS errors and Plots la-25 and la-26 graph the difference between the estimated and trueGPS errors. In these plots, the Kalman filter estimated standard deviation bounds are indicatedby dashed lines. The corrected IMU navigation quantities are shown in Plots la-27 throughla-33 and the estimated IMU gyro biases are shown in Plots la-34 to la-36. These plots alsoshow the Kalman filter estimated standard deviation bounds as dashed lines. Comparison of theKalman filter estimation errors with the predicted error standard deviations, provides confidencethat the filter is adequately estimating INS errors and transferring position and alignment to theIMU navigator. In all of the plots, the estimation error is fairly consistent with the standarddeviation bounds indicating that the filter is performing well. Plots la-37 through la-40 showthe measurement residuals along with the Kalman filter predicted rms values. The residuals aresubstantially less than the predicted levels due to the absence of timing related measurementerrors in the simulator.The importance of accurate IMU position and azimuth alignment for estimation of the antennaboresight misalignment is indicated by Equation (3-9). Figure 5-4 shows that the Kalman filterpredicted IMU position error bounds are essentially dictated by the rms error in the GPS-indicated position. Observability and estimation of the of azimuth misalignment, on the otherhand, requires manoeuvres as described in subsection 5.1. Flight profile number one includesan S-turn manoeuvre at 2200 seconds and a 180 degree turn manoeuvre at 3800 seconds. Figure5-5 shows the error in the IMU platform misalignment about the z axis. Examination of theKalman filter predicted rms error verifies that both manoeuvres are effective for the intendedpurpose of estimating IMU azimuth misalignment. In Figure 5-6, the filter predicted rms errorin the z IMU gyro bias demonstrates that a single manoeuvre does not ensure adequateestimation of this gyro bias but that two separated manoeuvres do provide for adequate109estimation of the z IMU gyro bias. The importance of controlling z IMU gyro bias is indicatedin Figure 5-5 by the rapid deterioration of the z IMU platform misalignment estimate followingthe first manoeuvre.In this and subsequent plots, solid lines represent the actual quantity (positionerror, for example) and dashed lines lines represent the Kalman filter predictedrms error bounds.'----,z-f.-50^1000^1500^2000^2500^3000^3500^4000^4500^5000time [s]-50^1000^1500^2000^2500^3000^3500^4000^4500^5000time [s]Figure 5-4 Error in Corrected IMU Position for Simulation Run la788x5050110.................................. .........• .........................................IV42—2—43-3....................... .......................1000^1500^2000^2500^3000^3500^4000^4500^5000time [s]Figure 5-5 Error in Corrected z IMU Platform Alignment for Simulation Run lax10-31000^1500^2000^2500^3000^3500^4000^4500^5000time [s]Figure 5-6 Error in z IMU Gyro Bias Estimate for Simulation Run la1115.3.3 Simulation Run 2aFlight Profile:^#2 (see subsection 5.3.1)Target Location: 440 57' 34" N710 56' 57" WData Processing Start Time: 1000 SData Processing End Time: 5000 sCalibration Start Time:^3400 sCalibration End Time:^4300 sSpecial Conditions:^• Data processing initiated during straight and levelflight prior to manoeuvres.• Manoeuvres consist of 180 degree turns at 2200seconds and 3260 seconds.• Calibration performed during straight and level flightfollowing manoeuvres.• Antenna azimuth misalignment is 20 milliradians.Plots:^2a-1 Error in x INS position error estimate(see 2a-2 Error in y INS position error estimateAppendix^2a-3 Error in x INS velocity error estimateA)^2a-4 Error in y INS velocity error estimate2a-5 Error in x INS platform misalignment estimate2a-6 Error in y INS platform misalignment estimate2a-7 Error in z INS platform misalignment estimate2a-8 Error in x INS accelerometer bias estimate2a-9 Error in y INS accelerometer bias estimate2a-10 Error in x INS gyro bias estimate2a- 11 Error in y INS gyro bias estimate2a- 12 Error in z INS gyro bias estimate2a-13 Error in x GPS position error estimate2a-14 Error in y GPS position error estimate2a-l5 Error in corrected x IMU position2a-16 Error in corrected y IMU position2a-17 Error in corrected x IMU velocity1122a-18 Error in corrected y IMU velocity2a-19 Error in corrected x IMU platform alignment2a-20 Error in corrected y IMU platform alignment2a-21 Error in corrected z IMU platform alignment2a-22 Error in x IMU gyro bias estimate2a-23 Error in y IMU gyro bias estimate2a-24 Error in z IMU gyro bias estimate2a-25 Error in antenna azimuth misalignment estimate2a-26 Antenna azimuth matching measurement residual2a-27 Antenna azimuth matching Kalman gainThis simulation run represents a potential antenna boresight calibration flight profile andaircraft/target geometry. Two 180 degree turns separated by 1000 seconds of straight and levelflight constitute the manoeuvre segment of the profile. Calibration is initiated following thesecond manoeuvre at a distance from the target of just over 100 kilometres. During thecalibration segment, the aircraft flies directly toward the target and, after 900 seconds,calibration is terminated at just over 10 kilometres from the target.The error in the estimated antenna azimuth misalignment is shown in Figure 5-7. The Kalmanfilter predicted error is seen to fall to a nearly constant level near 0.6 milliradians rms(2 arc minutes rms) within about 500 seconds. Figure 5-8 shows the antenna azimuth matchingmeasurement residuals along with the Kalman filter predicted rms value. As previously seenfor the position matching measurement residuals in simulation run la, the azimuth matchingmeasurement residuals are somewhat less than predicted due to the absence of timing errors inthe simulator. The Kalman gain connecting the antenna azimuth matching measurementresiduals to the antenna azimuth estimate is shown in Figure 5-9. This plot indicates that theKalman filter rapidly estimates the antenna azimuth misalignment and begins ignoring the...........................................................................113azimuth matching measurements, relying instead on the assumed noise free, random constantprocess model.It is of interest to compare Plots 2a-1 through 2a-24 with the corresponding plots fromsimulation run la (that is, Plots la-13 to la-36). The one notable effect of the inclusion ofantenna azimuth matching measurements is a reduction in the rms position error as the aircraftnears the target. This can be seen in Figure 5-10. Here, the Kalman filter is making use of theassumed perfect knowledge of the target location. The reduced position error is of little benefitto the antenna azimuth calibration, however, because it is not significant until after the filter hasessentially completed estimation of the azimuth misalignment.7o":(131000^1500^2000^2500^3000^3500^4000^4500^5000time [s]Figure 5-7 Error in Antenna Azimuth Misalignment Estimate for Simulation Run 2a...........1000^1500^2000^2500^3000^3500^4000^4500^5000time [s]Figure 5-8 Antenna Azimuth Matching Measurement Residual0.5—0.51 ^1000^1500^2000^2500^3000^3500^4000^4500^5000time [s]Figure 5-9 Antenna Azimuth Matching Kalman Gain for Simulation Run 2a114500—501151000 1500 2000 2500 4000 4500 50003000^3500time [s]50—50 ^1000^1500^2000^2500^3000^3500^4000^4500^5000time [s]Figure 5-10 Error in Corrected IMU Position for Simulation Run 2a5.3.4 Simulation Run 3aFlight Profile:^#3 (see subsection 5.3.1)Target Location: 440 52' 06" N69° 30' 38" WData Processing Start Time: 1000 sData Processing End Time: 5000 sCalibration Start Time:^3700 sCalibration End Time:^4600 s116Special Conditions:^• Data processing initiated during straight and levelflight prior to manoeuvres.• Manoeuvres consist of S-turns at 2200 seconds and3420 seconds.• Calibration performed during straight and level flightfollowing manoeuvres.• Antenna azimuth misalignment is 20 milliradians.Plots:^3a-1 Error in x INS position error estimate(see 3a-2 Error in y INS position error estimateAppendix^3a-3 Error in x INS velocity error estimateA)^3a-4 Error in y INS velocity error estimate3a-5 Error in x INS platform misalignment estimate3a-6 Error in y INS platform misalignment estimate3a-7 Error in z INS platform misalignment estimate3a-8 Error in x INS accelerometer bias estimate3a-9 Error in y INS accelerometer bias estimate3a-10 Error in x INS gyro bias estimate3a-11 Error in y INS gyro bias estimate3a-12 Error in z INS gyro bias estimate3a-13 Error in x GPS position error estimate3a-14 Error in y GPS position error estimate3a-15 Error in corrected x IMU position3a-16 Error in corrected y IMU position3a-17 Error in corrected x IMU velocity3a-18 Error in corrected y IMU velocity3a-19 Error in corrected x IMU platform alignment3a-20 Error in corrected y IMU platform alignment3a-21 Error in corrected z IMU platform alignment3a-22 Error in x IMU gyro bias estimate3a-23 Error in y IMU gyro bias estimate3a-24 Error in z IMU gyro bias estimate3a-25 Error in antenna azimuth misalignment estimate3a-26 Antenna azimuth matching measurement residual3a-27 Antenna azimuth matching Kalman gainSimulation run 3a represents another potential antenna boresight calibration flight profile similarto simulation run 2a but with S-turns substituted for the 180 degree turns. For this simulation,117the S-turns were composed of two 180 degree turns in opposite directions and separated by 100seconds of straight flight. Two S-turns separated by 1000 seconds of straight and level flightconstitute the manoeuvre segment of the flight profile. Calibration is initiated following thesecond manoeuvre at a distance from the target of just over 100 kilometres. During thecalibration segment, the aircraft flies directly toward the target and, after 900 seconds,calibration is terminated at just over 10 kilometres from the target. Results for this simulationrun vary only slightly from the results for simulation run 2a. Figure 5-11 shows the error inthe estimated antenna azimuth misalignment. Other combinations of 180 degree turns and S-turns could be expected to provide similar results.71:1000^1500^2000^2500^3000^3500^4000^4500^5000time [s]Figure 5-11 Error in Antenna Azimuth Misalignment Estimate for Simulation Run 3a1185.3.5 SummaryAccurate IMU position and azimuth alignment are important for estimating the antenna boresightmisalignment. Simulation run la showed that Kalman filter estimated position error dependslargely on the error in the GPS-indicated position and that effective estimation of z IMUmisalignment can be achieved using S-turn or 180 degree turn manoeuvres. Furthermore, it wasobserved that minimizing z IMU gyro bias is important and that two separated manoeuvres areneeded to effectively estimate this bias. Several simulation runs with various target locationsand calibration start times showed that, for best calibration results, the calibration should beperformed at maximum range from the target where bearing error is minimum, and immediatelyfollowing the manoeuvres where z IMU platform misalignment is minimum. Simulation runs2a and 3a conform to these conditions and represent potential antenna boresight calibration flightprofiles and aircraft/target geometries. Simulation run 2a uses two 180 degree turns resultingin a compact flight profile and minimum time. Simulation run 3a uses two S-turns requiringslightly more time but maintaining a more constant direction of travel. Other combinations of180 degree turns and S-turns may also be used. A particularly attractive combination consistsof two 180 degree turns in opposite directions which minimizes both manoeuvre time andcumulative errors.In simulation runs 2a and 3a, the Kalman filter predicted error in the estimated antenna azimuthmisalignment falls to a nearly constant level near 0.6 milliradians rms (2 arc minutes rms) within500 seconds (50 TAU measurements at 10 second intervals). This is well within the requiredaccuracy of 5 arc minutes rms. The total of manoeuvre time plus calibration time for the runsis in the vicinity of 35 minutes. Further simulations indicate that the time between manoeuvrescan be reduced from 1000 seconds to as little as 500 or 600 seconds with some degradation in119the z IMU gyro bias estimate but little impact on the antenna boresight calibration performance.Furthermore, the predicted calibration error falls to within ten percent of the final value afteronly about 300 seconds of calibration (30 TAU measurements). By using 600 seconds betweenmanoeuvres and 300 to 500 seconds for azimuth misalignment estimation, the total time can bereduced to around 20 minutes.Several other simulation runs, testing simple variations of the flight profiles and aircraft/targetgeometries, were performed. Variations included a change of flight profile orientation fromeast/west to north/south, a change of ground speed from 210 to 290 nautical miles per hour, achange of altitude from 3300 to 5000 feet and a change of latitude from 45 degrees to test valuesof 0 and 70 degrees. The results indicate that antenna calibration performance is essentiallyinsensitive to the actual heading, ground speed, altitude and latitude within reasonable operatinglimits.These results suggest that the in-flight antenna boresight calibration technique is indeed feasible.It has the potential to achieve antenna boresight calibration accuracies in the vicinity of 2 arcminutes rms within 20 minutes. This assumes antenna azimuth matching measurements at 10second intervals (30 measurements in 300 seconds) with a measurement noise of 2 milliradiansrms (0.05 degrees rms TAU error plus 0.1 degrees rms timing related error). Similar accuracywould be expected for different measurement intervals provided that 1) at least 30 measurementsare used, and 2) the TAU range limitations are respected (maximum 100 kilometres, minimum10 kilometres). The assumption of 0.1 degrees rms of timing related measurement noise isprobably pessimistic (flight trials are required to refine this value). Simulations indicate that asmaller value might slightly improve the calibration accuracy and could substantially reduce the120number of measurements required to reach the steady-state. For example, a large reduction inthe noise, from 0.1 degrees rms to 0.05 degrees rms, improved the calibration accuracy by 10or 15 percent and required only about half as many measurements.The filter performance is not truly optimal with respect to the synthesized sensor data becausethe filter models include process and measurement noise sources not in the synthesized data(gravity anomalies, for example). However, the statistical behaviour of the errors which areincluded in the synthesized sensor data precisely matches the error models used in the Kalmanfilter. In this respect, the performance is optimal and represents a baseline for subsequentperformance evaluations. Because the filter retains error models to account for "real world"errors (even though not simulated in the synthesized sensor data), the calibration performanceachieved here represents a reasonable "best case" performance target.121SECTION SIXPERFORMANCE EVALUATIONThe objectives of the antenna boresight calibration performance evaluation are twofold. First,the effectiveness of the formulated Kalman filter and recommended flight profile andaircraft/target geometry needs to be verified using real (or at least more realistic) sensor data.Second, since the intent is to incorporate this antenna boresight calibration method into theSARMCS, verification of its performance in the current and anticipated future SARMCSenvironments is important.A full evaluation of the antenna boresight calibration performance requires real sensor dataobtained during actual flights. Such data was not available for this investigation. Theperformance evaluation presented here, therefore, is based upon synthesized sensor data providedby DREO. This data (described in subsection 6.1) represents real SARMCS sensor data morefaithfully than that used in the simulations described in Section 5.In evaluating the antenna boresight calibration Kalman filter performance, the DREO synthesizedsensor data is initially processed similarly to Section 5 for comparison with the previous results.Then the processing is modified to more closely match the anticipated SARMCS processing.This includes first, increasing the Kalman filter update interval and second, separating theINS/GPS part of the filter.1226.1 Synthesized SARMCS Sensor DataThe synthesized SARMCS sensor data provided by DREO includes INS, IMU and GPS sensordata as shown in Tables 6-1 through 6-3. Reference navigation data corresponding to thelocations of the INS, IMU and GPS sensors is also provided. The reference data includes thetrue position (latitude, longitude, altitude), true velocity (north, east, down) and, in the case ofthe INS and IMU, true attitude (roll, pitch, heading) required by the evaluation package.Table 6-1 INS Data (at 16 Hz)Parameter ResolutionTime 10 itsLatitude 180/2" degLongitude 180/2" degHeading 180/2" degPitch 180/2" degRoll 180/2" degBody pitch rate 128/2" deg/sBody roll rate 128/2" deg/sBody yaw rate 128/2" deg/sPlatform heading 180/2" degAltitude 0.125 ftUp velocity 1 ft/minNorth velocity 0.125 knotsEast velocity 0.125 knotsTable 6-2 IMU Data (at 50 Hz)Parameter ResolutionTime 10 ASX angular increment 3.3 arcsecy angular increment 3.3 arcsecz angular increment 3.3 arcsecx velocity increment 2.5 mm/sy velocity increment 2.5 mm/sz velocity increment 2.5 mm/sTable 6-3 GPS Data (at 1 Hz)Parameter ResolutionTime 10 AsLatitude 2-23 degLongitude 2-23 degAltitude 1 mNorth velocity 2-5 m/sEast velocity 2-5 m/sUp velocity 2-5 m/s123124The SARMCS data synthesis package models the sensor environment and sensor errors morefaithfully than the data synthesis package described in Section 4. Specifically:• Turbulence and flexible body dynamics are simulated.• Vertical deflections and gravity anomalies are simulated.• Accelerometer and gyro scale factor errors, misalignments and g-sensitive errorsare simulated.• Quantization of the synthesized sensor outputs is included.• An additional lever arm from the antenna roll/pitch intersection to the IMU isincluded.The lever arms used for the synthesized SARMCS sensor data are shown in Table 6-4. InitialINS position, velocity and attitude errors are shown in Table 6-5. Tables 6-6 and 6-7 show theaccelerometer and gyro errors used for the synthesized INS and IMU sensor data.Table 6-4 SARMCS Lever ArmsFrom To Lever Arm CoordinateFramex (m) y (m) z (m)INS Antenna roll/pitchintersection6.05 -0.61 -0.08 AircraftbodyAntenna roll/pitchintersectionIMU 0.09 -0.24 -0.10 Antennaring gearINS GPS -4.00 0.00 -2.00 AircraftbodyTable 6-5 Initial INS System ErrorsQuantity Initial Error(wander azimuth axes)x Y zPosition 0 0 0 mVelocity 0 0 0 m/sAttitude 1 1 6 arcminTable 6-6 INS Accelerometer and Gyro ErrorsParameter ValueAccelerometersBias 50 FigScale Factor 50 ppmNonlinearity 10 Agig2Misalignment 5 arcsecRandom Error Standard Deviation 5 kigRandom Error Correlation Time 3600 sGyrosDrift 0.01 deg/hrScale Factor 5 ppmMisalignment 3 arcsecRandom Drift 0.003 deg/../hr125126Table 6-7 IMU Accelerometer and Gyro ErrorsParameter ValueAccelerometersBias 100 ttgScale Factor 200 ppmNonlinearity 20 Agig2Misalignment 20 arcsecRandom Error Standard Deviation 15 i48Random Error Correlation Time 3600 sGyrosDrift -0.2 deg/hrScale Factor 150 ppmg-Sensitive Drift 0.2 deg/hr/gMisalignment 20 arcsecRandom Error Standard Deviation 0.2 deg/hrRandom Error Correlation Time 1800 sIn addition to the INS, IMU and GPS sensor data, the data processing package requires altitudedata and TAU antenna azimuth data. For these simulations, altitude data at two second intervalsis obtained by subsampling the INS altitude, and antenna azimuth data at ten second intervalsis synthesized using the IMU navigator reference data (corrected for the IMU to antennaroll/pitch intersection lever arm). The synthesized antenna azimuth data includes a fixed 20milliradian misalignment plus an rms error of 0.05 degrees which is uncorrelated frommeasurement to measurement.127The evaluation data flight profile (also referred to as flight profile number four) is shown inFigure 6-1. It is flown at a nominal altitude of 3000 metres and a nominal ground speed of 100metres per second. The profile includes two 180 degree turns in opposite directions andseparated by 600 seconds of straight and level flight (turns one and two) which allow the Kalmanfilter to observe and estimate the azimuth misalignment. Following these manoeuvres, theprofile offers two potential calibration segments. The first is the 300 second north pointing legfollowing turn two and the second is the 300 second south pointing leg following turn three.The " + " marks along the profile in Figure 6-1 indicate 100 second intervals.Figure 6-1 Performance Evaluation Flight Profile1286.2 Initial Performance EvaluationFor comparison purposes, it is appropriate to begin the antenna boresight calibrationperformance evaluation by processing the DREO synthesized sensor data similarly to Section 5.Three modifications to the data processing software are necessary. The first modification isminor. Since the earth model used in generating the evaluation data corresponds to the olderWGS-72 geodetic datum rather than WGS-84 some changes to the constants used in the dataprocessing software are necessary. It should be noted that real GPS-determined positioncoordinates refer to WGS-84 and that WGS-84 should be used for any real implementation ofthe antenna boresight calibration method.The IMU and the antenna roll/pitch intersection are not coincident in the evaluation data. Thesecond modification is necessary to compensate for this additional lever arm. In subsection 3.1,the expression for the IMU/INS position matching measurement is given asz,, =[c:aReti-{RekHc Oc edIctIribmul.But the lever arm raw is now split into two parts, rRp from the INS to the antenna roll/pitchintersection and rsD from the roll/pitch intersection to the IMU (see Table 6-4), so that thesecond term in the equation becomes{c ]({c ed jc .dmkil, 4c eds[cds[rJ;DUwhere129[4plAi = INS to antenna roll/pitch intersection lever arm vector expressed in aircraftbody coordinates= Antenna roll/pitch intersection to IMU lever arm vector expressed instabilized antenna ring gear coordinatesNote that the small misalignment of the IMU body frame with respect to the antenna ring gearframe is ignored in the expression.The third modification to the data processing software is due to the quantization of thesynthesized sensor data. Quantization of the INS latitude and longitude values adds to theKalman filter measurement noise associated with the INS/GPS position matching measurementsand the IMU/INS position matching measurements. The INS latitude and longitude quantizationis 180/220 degrees (see Table 6-1) or about 19 metres in the north-south direction and between0 and 19 metres in the east-west direction (depending on latitude). Because the wander azimuthframe is, in general, not aligned with the geographic frame, there is also a direction dependentcross-covariance between the x and y position matching measurement noise. Furthermore, theINS position quantization introduces a cross-covariance between the INS/GPS position matchingmeasurement noise and the IMU/INS position matching measurement noise.Quantization error is commonly assumed to be uncorrelated from measurement to measurementand uniformly distributed between -q/2 to q/2 resulting in a variance of g2/12 where q is thequantization step size. The smoothly varying nature of the latitude and longitude values in thesimulations makes this model less than perfect, with the quantization having correlations bothin time and with the data (the quantization has a "sawtooth" appearance). Nevertheless, it is a130simple and useful model and so the quantization noise is assumed here to be uncorrelated in timewith variance q2/12. As will be seen in subsection 6.2.2, the effect of the quantization noise onfilter performance is relatively small. Thus, there is little incentive to model the latitude anddirection dependence and the x and y position matching measurement quantization noises aresimply assumed to be independent with variances of 30 m2 each.In the absence of azimuth matching measurements, the INS/GPS and transfer-of-alignmentsegments of the Kalman filter formulated in Section 3 are decoupled. This is desirable becauseit prevents possible corruption of the INS/GPS filter states which might otherwise occur overa period of time due to slight mismodelling of the lower quality IMU. The presence of the INSposition quantization, however, introduces a coupling between the two parts of the optimalKalman filter due to the cross-correlation between the INS/GPS position matching measurementsand the IMU/INS position matching measurements. A suboptimal filter, with the cross-correlation ignored, is used here since tests showed the performance of the resulting decoupledINS/GPS and transfer-of-alignment Kalman filters to be nearly the same as the optimal filter.6.2.1 Simulation Run 4aFlight Profile:^#4 (see subsection 6.1)Target Location: 440 56' 57" N63° 24' 13" WData Processing Start Time: 0 sData Processing End Time: 1980 s131Calibration Start Time:^1320 sCalibration End Time:^1620 sSpecial Conditions:^• Calibrate during north pointing leg following turn two.• Target range is 50 kilometres at start of calibration.• One second Kalman filter update interval.• Quantized INS position used for position matchingmeasurements.The measure of the antenna boresight calibration Kalman filter performance is its ability toaccurately estimate the antenna azimuth misalignment. Figure 6-2 shows the error in the antennaazimuth misalignment estimate for this simulation run."Figure 6-2 Error in Antenna Azimuth Misalignment Estimate for Simulation Run 4a132The Kalman filter predicted error is observed to be slightly less than 1 milliradian rms (about3.3 arc minutes rms) at the completion of calibration. This is greater than the predicted errorsnear 2 arc minutes rms for the simulations in Section 5 but still within the required accuracy of5 arc minutes rms. Major factors contributing to the poorer predicted performance includedecreased initial target range, increased position matching measurement noise due to INSquantization, and increased process noise spectral density due to turbulence and vibration. Theeffects of the target range and the position matching measurement noise are examined moreclosely in simulation run 4h. Figure 6-3 shows the increased noise in Kalman filter positionmatching measurement residuals due to quantization of the INS latitude and longitude (compareSection 5, simulation run la).Figure 6-3 Quantization Noise in y IMU/INS Position Matching Measurement Residual....................... ................. •....................... ....^7 --133Accurate estimation of the antenna azimuth misalignment depends on how well the z IMUplatform alignment and IMU position are determined. The predicted error in corrected z IMUplatform alignment is increased by the added position matching measurement noise. This canbe seen by comparing Figure 6-4 with the results in Section 5 and with the results of simulationrun 4b in the next subsection. The filter predicted error in corrected IMU position is almostentirely due to the GPS position data rms error. This is shown in Figure 6-5 where it is seenthat the predicted IMU position error immediately falls to the GPS position data rms error (20metres) and does not change until calibration is essentially completed. In terms of antennaazimuth misalignment estimation, equation (3-9) shows that position error is more significant atshorter initial target ranges while the effect of z IMU platform misalignment is independent oftarget range.0f-a)0^200^400^600^800^1000 1200 1400 1600 1800 2000time [s]Figure 6-4 Error in Corrected z IMU Platform Alignment for Simulation Run 4a134200^400^600^800^1000 1200 1400 1600 1800 2000time [s]200^400^600^800^1000 1200 1400 1600 1800 2000time [s]Figure 6-5 Error in Corrected IMU Position for Simulation Run 4a6.2.2 Simulation Run 4bFlight Profile:^#4 (see subsection 6.1)Target Location: 450 23' 54" N63° 24' 13" WData Processing Start Time: 0 sData Processing End Time: 1980 sCalibration Start Time:^1320 sCalibration End Time:^1620 s135Special Conditions:^• Calibration during north pointing leg following turntwo.• Target range is 100 kilometres at start of calibration.• One second Kalman filter update interval.• Negligible quantization of INS position used forposition matching measurements.This simulation run demonstrates the effects of increased initial target range (from 50 kilometresto 100 kilometres) and decreased position matching measurement noise (INS latitude andlongitude quantization is reduced to a negligible 1 x 10-7 radians) on antenna boresight calibrationperformance. The error in the antenna azimuth misalignment estimate is shown in Figure 6-6.. .Figure 6-6 Error in Antenna Azimuth Misalignment Estimate for Simulation Run 4b136Filter predicted error for this simulation run is 0.75 milliradians rms or about 2.6 arc minutesrms versus 3.3 arc minutes rms for simulation run 4a. The errors in corrected z IMU platformalignment and corrected IMU position are shown in Figures 6-7 and 6-8 for comparison withsimulation run 4a.Simulation runs separating the target range and measurement noise effects indicate that thegreatest impact on calibration performance is due to the increased target range. In particular,increased target range alone resulted in a Kalman filter predicted error in the antenna azimuthmisalignment of less than 2.9 arc minutes rms while decreased position matching measurementnoise alone achieved a predicted error of only about 3.1 arc minutes rms.Figure 6-7 Error in Corrected z IMU Platform Alignment for Simulation Run 4b500—501370^200^400^600^800^1000 1200 1400 1600 1800 2000time [s]200^400^600^800^1000 1200 1400 1600 1800 2000time [s]Figure 6-8 Error in Corrected IMU Position for Simulation Run 4b6.3 Increased Kalman Filter Update IntervalFor the SARMCS to achieve real-time operation, it is important to minimize the computationalload on the small airborne computers. Currently, the SARMCS performs Kalman filter updatesat ten second intervals. To test the effect of increasing the interval between Kalman filterupdates on calibration performance, simulation run 4a is repeated here using ten second updatesinstead of one second updates. A simulation using the south pointing calibration leg of the flightprofile is also performed.1386.3.1 Simulation Run 4cFlight Profile:^#4 (see subsection 6.1)Target Location: 440 56' 57" N63° 24' 13" WData Processing Start Time: 9 sData Processing End Time: 1980 sCalibration Start Time:^1320 sCalibration End Time:^1620 sSpecial Conditions:^• Calibration during north pointing leg following turntwo.• Target range is 50 kilometres at start of calibration.• Ten second Kalman filter update interval.• Quantized INS position used for position matchingmeasurements.This simulation run is intended to approximate the operation of the antenna boresight calibrationKalman filter within the current SARMCS. The Kalman filter update interval is increased to10 seconds for the run. All other aspects of the simulation are the same as simulation run 4a.Figure 6-9 shows the error in the antenna azimuth misalignment estimate. The longer intervalbetween Kalman filter updates has increased the filter predicted error in the antennamisalignment estimate to 1.1 milliradians or about 3.8 arc minutes. Also, a slight increase inthe filter settling time is just observable between Figures 6-2 and 6-9. The 3.8 arc minute rmserror is still within the required accuracy of 5 arc minutes rms and, as suggested in subsection6.2.2, the figure could probably be slightly improved by increasing the initial target range.139.........0^200^400^600^800^1000 1200 1400 1600 1800 2000time Es]Figure 6-9 Error in Antenna Azimuth Misalignment Estimate for Simulation Run 4cThe error in the corrected IMU position and the error in the corrected z IMU platform alignmentare shown Figures 6-10 and 6-11. It can be seen that the longer update interval has resulted inonly a small increase in the filter predicted position error. The filter predicted error in correctedz IMU platform alignment, on the other hand, has been substantially increased by the longerupdate interval and accounts for much of the deterioration of the predicted error in the antennamisalignment estimate. Simulation run 4c best represents the operation of the calibrationKalman filter in the SARMCS and, as such, a full set of plots for the run are included inAppendix B.140500—500^200^400^600^800^1000 1200 1400 1600 1800 2000time [s]500—500^200^400^600^800^1000 1200 1400 1600 1800 2000time [s]Figure 6-10 Error in Corrected IMU Position for Simulation Run 4cFigure 6-11 Error in Corrected z IMU Platform Alignment for Simulation Run 4c1416.3.2 Simulation Run 4dFlight Profile:^#4 (see subsection 6.1)Target Location: 440 19' 17" N63° 21' 19" WData Processing Start Time: 9 sData Processing End Time: 1980 sCalibration Start Time:^1680 sCalibration End Time:^1980 sSpecial Conditions:^• Calibration during south pointing leg following turnthree.• Target range is 50 kilometres at start of calibration.• Ten second Kalman filter update interval.• Quantized INS position used for position matchingmeasurements.This simulation run is similar to simulation run 4c but calibration is performed during the southpointing leg of the flight profile. It is intended to provide some additional verification of theKalman filter performance. As Figure 6-12 shows, the filter predicted rms error in the antennaazimuth misalignment is comparable to simulation run 4c and the estimated misalignment iswithin the predicted rms error limits.77')142................ •0^200^400^600^800^1000 1200 1400 1600 1800 2000time [s]Figure 6-12 Error in Antenna Azimuth Misalignment Estimate for Simulation Run 4d6.4 Separated 1NS/GPS Kalman FilterThe INS and GPS sensors currently used in the SARMCS are expected to be replaced by anintegrated INS/GPS system in the future. In this scenario, the INS/GPS part of the Kalmanfilter is included in the INS/GPS system and is separate from the transfer-of-alignment andantenna azimuth calibration part of the filter. It is of interest, therefore, to evaluate the antennaboresight calibration performance in this cascaded Kalman filter configuration.For this simulation, it is assumed that the INS/GPS system outputs include not only the estimatedposition, velocity and attitude navigation information but also that both the uncorrected INS data143and the estimated INS errors are available. In particular, the uncorrected INS position is neededfor IMU/INS position matching measurements and the estimated INS errors are needed for theIMU navigator error control feedback and for the antenna azimuth matching measurements. Itis further assumed that the resolution of these outputs is high enough that quantization errors canbe neglected.To incorporate an approximation for a separate INS/GPS system into the simulation, theINS/GPS part of the Kalman filter must be completely independent. Coupling of the INS/GPSpart of the Kalman filter with the rest of the filter occurs through the antenna azimuth matchingmeasurement model. The antenna azimuth misalignment measurement is given byZ A -6 TAU-13 +0where 6 TAU is the TAU-determined antenna azimuth, fl is the target bearing computed from theIMU navigator position and the known target location, and is the IMU navigator heading. Thecorresponding measurement model is given in equation (3-9). For convenience, the complicatedcoefficient expressions in this equation are replaced here by the variables a and b and theequation is rewritten aszA =a(bRsx +Mtmx)-1-b(SRsy +SRO +(c5s, +00 +8 +vAwhere VA is the noise associated with the TAU measurement.144To decouple the INS/GPS portion of the Kalman filter, the measurement can be redefined asz'OrAu-13+0- mx-bAmy-ibm,where the hats indicate states estimated by the INS/GPS Kalman filter. Thus, the outputs of theINS/GPS Kalman filter become inputs to the calibration Kalman filter. The measurement modelis thenz=aOR +bait + (1) sz +8+Vwhere v is the noise associated with the TAU measurement plus the random errors associatedwith the estimated INS/GPS states. But the action of the INS/GPS filter makes the randomerrors in its outputs complicated time correlated sequences. A simplistic approach to thisproblem is just to assume that the outputs are not correlated and design a suboptimal calibrationKalman filter. Schlee et al. (1988) have successfully used this technique to integrate the outputsfrom a GPS receiver internal Kalman filter with INS data in a cascaded external Kalman filter.A more theoretically sound approach for cascading Kalman filters has been developed by Carlson(1988) but is beyond the scope of this thesis. The suboptimal approach is used here.One way to account for the additional antenna azimuth matching measurement noise, due to theerrors in the estimated INS/GPS states, is to assume that the errors are white sequences withvariances given by the error covariance matrix P. Assuming that the TAU measurement noise145is independent from the state estimation errors, the azimuth matching measurement noisevariance is+_apoR.14). +2bpsithk,„2 2^2„av =avA -a paR. b 2p- 512  +P95 . +2abpRm./6R 7where the p variables are the covariances of the INS/GPS state errors obtained from the errorcovariance matrix. A value for the additional measurement noise variance can be approximatedby ignoring the cross terms appearing in the equation and using typical aircraft/target geometryto specify values for a2 and b2, and typical values of the INS/GPS state error variances obtainedfrom previous simulation results. A value of 1.0 mrad2 is used for the simulation.The effect on calibration performance of ignoring the correlations can be predicted by modellingthe error in the relevant INS/GPS states as a bias plus uncorrelated noise. This is a reasonableapproximation assuming a relatively short time interval and straight and level flight, as is thecase when processing azimuth matching measurements. The calibration Kalman filter wouldthen be expected to obtain a biased estimate of the antenna azimuth misalignment. As anexample, consider initiating calibration at a distance of 50,000 metres from the target with amaster navigator position error bias of 20 metres perpendicular to the target direction and amaster navigator azimuth misalignment bias of 0.1 milliradians. The resulting bias in theestimated antenna azimuth misalignment is nearly 2 arc minutes which, due to the white noiseassumptions, the filter estimated error covariance does not account for. This also adverselyaffects the IMU navigator error states through the measurement model coupling. Partialcompensation for these problems can be achieved by adding a small white noise forcing function146to the antenna azimuth misalignment state and tuning the spectral density so that the filterestimated antenna azimuth error covariance reaches a steady-state value near that of the optimalfilter. A spectral density of 0.005 (mrad/s)2/Hz is used for the simulation.6.4.1 Simulation Run 4eFlight Profile:^#4 (see subsection 6.1)Target Location: 440 56' 57" N63° 24' 13" WData Processing Start Time: 9 sData Processing End Time: 1980 sCalibration Start Time:^1320 sCalibration End Time:^1620 sSpecial Conditions:^• Calibration during north pointing leg following turntwo.• Target range is 50 kilometres at start of calibration.• Ten second Kalman filter update interval.• Negligible quantization of INS position used forposition matching measurements.• Separated INS/GPS Kalman filter.Many simulation runs were performed to test the performance of this ad hoc design for acalibration Kalman filter incorporating a separated INS/GPS system. The simulations used boththe north pointing and south pointing calibration legs of the flight profile as well as synthesized147TAU data with differing noise components. The performance of the calibration filter wasadequate in all of the simulations with an error of less than 5 arc minutes in the antenna azimuthmisalignment estimation in every case and less than 3.3 arc minutes in most cases. Simulationrun 4e is a representative run. Figure 6-13 shows the error in the antenna azimuth misalignmentestimate for this run. The error in the corrected IMU position and the error in the corrected zIMU platform alignment are shown in Figures 6-14 and 6-15. In Figure 6-16, it is observed thatthe Kalman gain connecting the azimuth matching measurement residuals to the antenna azimuthmisalignment estimate levels out at a nonzero value. This is due to the addition of process noiseto the antenna azimuth misalignment state model.42"00tt0+—is?45240^200^400^600^800^1000 1200 1400 1600 1800 2000time [s]Figure 6-13 Error in Antenna Azimuth Misalignment Estimate for Simulation Run 4e148500—500^200^400^600^800^1000 1200 1400 1600 1800 2000time [s]200^400^600^800^1000 1200 1400 1600 1800 2000time [s]Figure 6-14 Error in Corrected IMU Position for Simulation Run 4eFigure 6-15 Error in Corrected z IMU Platform Alignment for Simulation Run 4e0.80.2149-^ -200^400^600^800^1000 1200 1400 1600 1800 2000time [s]Figure 6-16 Antenna Azimuth Matching Kalman Gain for Simulation Run 4eSince it is only the azimuth matching measurements and the antenna azimuth misalignment statethat have been modified to accommodate the separate INS/GPS system, filter operation duringthe manoeuvre portion of the flight profile is unaffected. In fact, even during calibration, theeffect of the antenna azimuth measurements on the navigation error and sensor error states ofthe calibration Kalman filter is very weak. This can be confirmed by comparing the plottedresults of a simulation run with calibration enabled to the same run with calibration disabled.Such plots are virtually identical. Thus, it can be concluded that the filter modifications are notadversely affecting the transfer-of-alignment part of the filter. Furthermore, even when theINS/GPS part of the filter is not separated, the azimuth matching measurements significantly150affect only the position error estimates and then only at close range to the target and only afterestimation of the antenna azimuth misalignment is essentially completed. It is, therefore, quitereasonable to consider the antenna azimuth misalignment estimation part of the Kalman filter inisolation from the rest. This is essentially what has been done to design the calibration Kalmanfilter for a separated INS/GPS system. The resulting performance appears to be quiteacceptable.151SECTION SEVENCONCLUSIONS AND RECOMMENDATIONSThis thesis has presented the theoretical background and description of a method for estimatingSAR antenna azimuth misalignment in-flight by means of a Kalman filter. The method, whichwas developed for the Canadian Department of National Defence Spotlight SAR program, offersa simple, quick and accurate operational procedure to replace cumbersome laser basedprocedures.7.1 Kalman Filter DesignA 25-state antenna boresight calibration Kalman filter was formulated. It is based on the DREOdesigned SARMCS Kalman filter. The SARMCS filter uses GPS-indicated position to damp theSchuler oscillations in a medium accuracy INS. Simultaneously, it transfers the resulting stableplatform alignment to an IMU navigator that provides position, velocity and heading informationat the SAR antenna. The antenna boresight calibration Kalman filter adds a new antennaazimuth matching measurement and a random constant state representing antenna azimuthmisalignment to the SARMCS filter.The existing SARMCS Kalman filter was chosen to form the basis for the antenna boresightcalibration Kalman filter because antenna boresight calibration is intended to become anoperational mode of the SARMCS and because performance of the SARMCS filter is wellproven in actual flight trials. It was shown that accurate position and heading error estimatesare prerequisites to accurate antenna azimuth calibration. In this respect, desirable152characteristics of the current SARMCS Kalman filter are its effective control of platform azimuthmisalignment and its bounding of position errors by GPS position measurements.The choice of the random constant model for the antenna azimuth misalignment state was basedon the assumption that the mechanical misalignment is fixed. It was argued that a constant isjustified for the calibration filter because the calibration time is short. A more complicatedmodel, such as a random walk process or an exponentially correlated process with a long timeconstant, is unnecessary.The calibration Kalman filter directly observes antenna azimuth misalignment using azimuthmatching measurements. These measurements are defined as the difference between the TAU-determined antenna azimuth and the antenna azimuth computed from IMU navigator informationand the known target location. In forming the azimuth matching measurements, it is importantnot to introduce errors by mixing geodetic datums. In particular, since the GPS-determinedposition coordinates refer to the WGS 84 geodetic datum, the target position coordinates and theearth model constants should also refer to the WGS 84 system.7.2 Test MethodsAn extensive computer simulation software package, consisting of approximately 9000 lines ofC+ + code and 2000 lines of MATLAB M code, was developed to test the antenna boresightcalibration Kalman filter. It is divided into a synthesis package, a processing package and anevaluation package. The synthesis package features the ability to quickly define and generatean arbitrary flight trajectory and to synthesize the corresponding sensor data with major sensorerror sources included. The processing package (listed in Appendix C) implements the IMU153strapdown navigator algorithm and the calibration Kalman filter. It inputs sensor data andoutputs time histories of various navigation and Kalman filter quantities for subsequent analysis.The evaluation package inputs truth data from the synthesis package and output data from theprocessing package and features the ability to automatically produce plots of selected navigationand Kalman filter quantities including the Kalman filter state estimation errors and predicted rmserror bounds.The synthesis package was used for flight profile and aircraft/target geometry determinationbecause it allowed many flight profiles to be quickly and conveniently generated. Simulationsusing this synthesized data also provided a "best case" performance baseline. The filterperformance is not truly optimal with respect to the synthesized sensor data because the filtermodels include process and measurement noise sources not in the synthesized data (gravityanomalies, for example). However, the statistical behaviour of the errors which are includedin the synthesized sensor data precisely matches the error models used in the Kalman filter. Inthis respect, the performance is optimal and represents a baseline for subsequent performanceevaluations.Error models in the synthesis package are limited to the major sensor error sources. Morerealistic synthesized sensor data provided by DREO was used for the Kalman filter performanceevaluation. Real sensor data was not available for the final evaluation but SARMCS experiencehas shown that sensor data generated using the DREO synthesis package compares favourablywith real data for testing purposes.1547.3 Flight Profile and Aircraft/Target GeometryRecommendations regarding appropriate flight profiles and aircraft/target geometry for antennaboresight calibration have been presented based on computer simulations and operationalconsiderations. The recommended flight profiles include manoeuvres designed to allow theKalman filter to observe and estimate heading errors. In particular, it was found that while asingle manoeuvre is sufficient for observation of the inertial platform azimuth misalignment, asecond manoeuvre is required to ensure observation of the azimuth gyro bias. A particularlyattractive combination of manoeuvres consists of two 180 degree turns in opposite directionsseparated by approximately 600 seconds of straight and level flight. This combination minimizesboth manoeuvre time and cumulative errors and was found to work well in the simulations.The described antenna boresight calibration method requires a radar target situated at a wellsurveyed, fixed location on the ground. Antenna azimuth misalignment estimation is mostconveniently performed while flying directly toward the target. It was found that a calibrationtime of 300 to 500 seconds is adequate given azimuth matching measurements at 10 secondintervals (30 to 50 measurements). Furthermore, it was found that calibration should beperformed at maximum TAU range from the target, where bearing error is minimum, andimmediately following the manoeuvres, where platform azimuth misalignment is minimum.Limited error models in the data synthesis package result in near optimal Kalman filterperformance when processing this data. The antenna azimuth calibration accuracy was foundto be about 2 arc minutes rms using the recommended flight profile and aircraft/target geometry.This assumes 30 antenna azimuth matching measurements at 10 second intervals and ameasurement noise of 2 milliradians rms. The calibration performance was found to be155insensitive to changes in the flight profile heading, ground speed, altitude and latitude, withinreasonable operating limits. These results form the "best case" performance baseline forsubsequent performance evaluations.7.4 Antenna Boresight Calibration PerformanceEvaluation of the Kalman filter calibration performance is based on simulations using synthesizedsensor data provided by DREO. The calibration Kalman filter is suboptimal since it is basedon the suboptimal SARMCS filter. However, extensive flight trials have validated the SARMCSdesign and provide grounds for confidence in the calibration Kalman filter performanceevaluation results. In the simulations, errors in the Kalman filter estimated states wereconsistent with predicted error bounds suggesting that the design is reasonable.The evaluation flight profile includes two 180 degree turns in opposite directions separated by600 seconds and followed by 300 seconds of flight directly toward a target from an initial rangeof 50 kilometres. Antenna azimuth misalignment estimation was performed during this last 300seconds with TAU measurements at 10 second intervals. The total of manoeuvre time pluscalibration time is less than 20 minutes.Calibration performance was found to be poorer than the previously achieved 2 arc minutes rms.Major factors contributing to the poorer performance were found to be increased process noisespectral densities due to turbulence and vibration, increased position matching measurementnoise due to INS output quantization, and decreased target range. Increased process noise alonebrought the filter predicted antenna azimuth misalignment error up to 2.6 arc minutes rms andthe addition of INS quantization noise further increased the predicted error to 2.9 arc minutes156rms. Decreasing the initial target range from 100 kilometres to 50 kilometres resulted in anotherjump in the predicted antenna azimuth misalignment error up to 3.3 arc minutes rms.For implementation of the calibration Kalman filter in the current SARMCS, it is necessary touse a ten second Kalman filter update interval. When simulations were performed using tensecond update intervals (previous results were based on one second updates), the filter predictedantenna azimuth misalignment error increased from 3.3 to 3.8 arc minutes rms. The SARMCSKalman filter actually obtains the transition matrix 4); at 2 second subintervals and forms 4)k forthe entire interval using the property 4)k = 4)J+44);,34)/,24);,14)./. This may slightly improve theresults obtained with the longer filter update interval.DREO is also interested in replacing the INS and GPS sensors used in the SARMCS with anintegrated INS/GPS system. A cursory examination of the problems associated with a separatedINS/GPS Kalman filter and simulations using a somewhat ad hoc calibration Kalman filter designsuggest that a calibration accuracy of better than 5 arc minutes rms is still feasible.In summary, extensive computer simulations have demonstrated the feasibility of the describedantenna boresight calibration approach for use in the Canadian Department of National DefenceSpotlight SAR program. Required calibration accuracy is 5 arc minutes rms or better. Theperformance evaluation results suggest that the Spotlight SAR antenna azimuth misalignment canbe estimated to an accuracy of better than 4 arc minutes rms within a 20 minute period. Itremains, however, to verify these results using real sensor data obtained during actual flighttrials.157REFERENCESBenson, D. 0., Jr. (1975): "A Comparison of Two Approaches to Pure-Inertial and Doppler-Inertial Error Analysis," IEEE Trans. Aerospace and Electronic Systems, Vol. AES-11,No. 4, July, pp. 447-455.Britting, K. R. (1971): Inertial Navigation Systems Analysis, Wiley-Interscience, New York.Brown, R. G. and P. Y. C. Hwang (1992): Introduction to Random Signals and Applied KalmanFiltering, 2nd ed., Wiley, New York.Carlson, N. A. (1988): "Federated Filter for Fault-Tolerant Integrated Navigation Systems,"Proceedings of IEEE Position, Location and Navigation Symposium, PLANS-88, pp. 110-119.Defense Mapping Agency (1987): "Department of Defense World Geodetic System 1984—ItsDefinition and Relationships with Local Geodetic Systems," DMA Technical Report8350.2, Defense Mapping Agency, Washington, D.C.DiFilippo, D. J. (July 10, 1992): Letter to the author including unpublished notes on inertialnavigation error analysis.DiFilippo, D. J. (Dec. 23, 1992): "Design Notes on SARMCS Kalman Filter," unpublished.DiFilippo, D. J., G. E. Haslam and W. S. Widnall (1988): "Evaluation of a Kalman Filter forSAR Motion Compensation," Proceedings of IEEE Position, Location and NavigationSymposium, PLANS-88, pp. 259-268.Dragomir, V. C., D. N. Ghitau, M. S. Mihailescu and M. G. Rotaru (1982): Theory of theEarth's Shape, Elsevier Scientific Publishing Co., New York.Ewing, C. E. and M. M. Mitchell (1970): Introduction to Geodesy, American ElsevierPublishing Co., New York.Farrell, J. L. (1976): Integrated Aircraft Navigation, Academic Press, New York.Gelb, A. ed. (1974): Applied Optimal Estimation, MIT Press, Cambridge, Mass.Haslam, G. E., M. R. Vant and D. DiFilippo (1988): "A Synthetic Aperture Radar Mode forthe AN/APS-506 Maritime Search Radar," IEEE National Radar Conference, pp. 106-110.Hepburn, J. S. A., D. B. Reid, W. S. Widnall, D. F. Liang and G. E. Haslam (1984): "MotionCompensation for High Resolution Spotlight SAR," Proceedings of IEEE Position,Location and Navigation Symposium, PLANS-84, pp. 59-65.158Heiskanen, W. A. and H. Moritz (1967): Physical Geodesy, W. H. Freeman and Co., SanFrancisco, Calif.Huddle, J. R. (1983): "Inertial Navigation System Error-Model Considerations in KalmanFiltering Applications," in Control and Dynamic Systems, Vol. 20 (C. T. Leondes, ed.),Academic Press, New York.MATLAB, The Mathworks, Inc., Cochituate Place, 24 Prime Parkway, Natick, MA 01760.Milliken, R. J. and C. J. Zoller (1978): "Principle of Operation of NAVSTAR and SystemCharacteristics," Navigation: Journal of the Institute of Navigation, Vol. 25, No. 2, pp.95-106.Von Mises, R. (1959): Theory of Flight, Dover Publications, New York.Poor, W. A. (1989): "A Geometric Description of Wander Azimuth Frames," Navigation:Journal of the Institute of Navigation, Vol. 36, No. 3, pp. 303-318.Press, W. H., B. P. Flannery, S. A. Teukolsky and W. T. Vetterling (1986): NumericalRecipes: The Art of Scientific Computing, Cambridge University Press, New York.Schlee, F. H., N. F. Toda, M. A. Islam and C. J. Standish (1988): "Use of an ExternalCascaded Kalman Filter to Improve the Performance of a Global Positioning System (GPS)Inertial Navigator," Proceedings of National Aerospace and Electronics Conference,NAECON, pp. 345-350.Zachmann, G. W. (1988): "GPS Accuracy for Civil Marine Navigation," presented at NationalMarine Electronics Assoc., Boston, Mass.159APPENDIX AFLIGHT PROFILE SIMULATION PLOTSThis appendix contains plots from the flight profile and aircraft/target geometry simulation runs(Section 5, simulation runs la, 2a and 3a). The plots were produced using the computersimulation evaluation package described in Section 4.dashed =true, solid=estimated(la-1) True and Estimated x INS Position Error16012001000800L. 600$."400200010001400120010008006004008."C) 2000-200-400-6%001500^2000^2500^3000^3500^4000time [s](1a-2) True and Estimated y INS Position Error4500^50001500^2000^2500^3000^3500^4000time [s]4500^50002500 3000^3500time [s]1.210.80.6E0.4° 0.2*0dashed =true, solid =estimated4000^4500^500015001u00 2000-0.2-0.4161(1a-3) True and Estimated x INS Velocity Error0.80.60.40.20f..° -0.2-0.4-0.6 dashed=true, solid =estimated-0.81500^2000^2500^3000^3500^4000^4500^5000time [s](1a-4) True and Estimated y INS Velocity Error0.150.1-0.1dashed =true, solid =estimated(1a-5) True and Estimated x INS Platform Misalignment1621500^2000^2500^3000^3500^4000^4500^5000time [s](1a-6) True and Estimated y INS Platform Misalignment-0. 0 I--1000time [s]163(1a-7) True and Estimated z INS Platform Misalignment2.51.50.5---------------------------------------------------------0.5dashed=true, solid =estimatedI^1^1^I^I^I-11000^1500^2000^2500^3000time [s]x10-4^(1a-8) True and Estimated x INS Accelerometer Bias1500 2000 2500 3000^3500Jashed=true, solid =estimatedtime [s]4000 500045003500 4000 4500 50001643000^3500 4000 4500 5000time [s](la-10) True and Estimated x INS Gyro Bias''''40,shed=truejolid=estimated-0.51500 2000 2500 4000 4500 5000dashed=true, solid=estimated3000^3500x10-4987654321011000(1a-9) True and Estimated y INS Accelerometer Biastime [s]165x10-5^(1a-11) True and Estimated y INS Gyro Bias10.50-0.5-2-2.5fodasifed=,trtie., ‘Splid‘=eitimate,d,„„,-315000^15100^2000^ 1^1^2500^3000time [s]time [s]-33500^4000^4500^500050403020100-10-20-30-40-501000(1a-13) Error in x INS Position Error Estimate1500^2000^2500^3000^3500^4000time [s](1a-14) Error in y INS Position Error Estimate1664500^500050403020100-10-20-30-401500^2000^2500^3000^3500^4000^4500^5000time [s](1a-15) Error in x INS Velocity Error EstimateI^1^I^I^1^1^I.13000^1500^2000^2500^3000time [s](1a-16) Error in y INS Velocity Error Estimatetime [s]1673500 4000 4500 5000time [s]-- -------------- --------------^■ -------------------------------------------- -------------------------------------------------------------40004315000^1500^2000^2500^3000^3500time [s](1a-18) Error in y INS Platform Misalignment Estimate(la-17) Error in x INS Platform Misalignment Estimate__----------------------------0.50.40.30.20.10-0.1-0.2-0.3-0.4--------------------------- --„ --- _ -------------- -------------- ------------^-------------- _ --------- _ ------015000 1500 2000 2500 3000^3500 4000 4500 50004500 50000.10-0.11680.50.40.30.2-0.2-0.3-0.40.20-0.2-0.4-0.6-0.80.80.60.41-11000 1500 2000 2500 3000^3500 4000 4500 5000(1a-19) Error in z INS Platform Misalignment Estimate169I^1^1^I^1^I^I-15000^1500^2000^2500^3000time [s]x103^(1a-20) Error in x INS Accelerometer Bias Estimate3500^4000^4500^5000time [s]170x10-310.80.60.40.20-0.2-0.4-0.6-0.8-11000x10410.80.60.40.20-0.2-0.4-0.6-0.8-11000(la-21) Error in y INS Accelerometer Bias Estimate1500^2000^2500^3000^3500^4000time [s](1a-22) Error in x INS Gyro Bias Estimate1500 2000 2500 3000^3500 40004500^5000-4500 5000time [s](1a-23) Error in y INS Gyro Bias Estimate0.8 -0.60.4-------^----- ------------------------------------------------------------^------------^- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -171il000 1500^2000^2500^3000^3500^4000^4500^5000time [s](1a-24) Error in z INS Gyro Bias Estimatetime [s]50403020100-10-20-30-40-501000(1a-25) Error in x GPS Position Error Estimate1500^2000^2500^3000^3500^4000time [s](la-26) Error in y GPS Position Error Estimate1724500^500050403020100-10-20-30-40 1500^2000^2500^3000^3500^4000^4500^5000time [s]50403020100-10-20-30-40-501000 1500^2000^2500^3000^3500 4000 4500 5000(1a-27) Error in Corrected x IMU Position173time [s](la-28) Error in Corrected y IMU Position50403020,-_,--.^10I.^0Ca.)^-10-20-30-40 1500^2000^2500^3000^3500^4000^4500^5000time [s](1a-29) Error in Corrected x IMU Velocity1740.30.20.1-0.1-0.21500^2000^2500^3000^3500^4000^4500^5000time [s](1a-30) Error in Corrected y IMU Velocity0.3 ^0.2 -0.1 --0.1-0.2-013000^1500^2000^2500^3000^3500^4000^4500^5000.13000time [s]-------------------------------------------- --------------0.50.40.30.20.10-0.1-0.2-0.3-0.4415000^1500^2000^2500^3000^3500^4000^4500^5000(1a-31) Error in Corrected x IMU Platform Alignmenttime [s](1a-32) Error in Corrected y IMU Platform Alignment0.50.40.30.276; 0.10-0.1a)-0.2-0.3-0.4 3000^3500^4000^4500time [s]175k./53(1a-33) Error in Corrected z IMU Platform Alignment-2-3-417624-15000 1500^2000^2500^3000^3500^4000^4500^5000time [s]x10-3^ (1a-34) Error in x IMU Gyro Bias Estimatetime [s]45004000 50003210-1-2I^1 -13000^1500^2000^2500^3000^3500time [s]x10-3^(1a-36) Error in z IMU Gyro Bias Estimate177x10-3^(la-35) Error in y IMU Gyro Bias Estimate3-13000^1500^2000^2500^3000^3500^4000^4500^5000time [s]-8 --1Q,^1u00178(1a-37) x INS/GPS Position Matching Measurement Residualtime [s](1a-38) y INS/GPS Position Matching Measurement Residualj,/"Of 1500^2000^2500^3000^3500^4000^4500^5000time [s]5432-2 ,-3-415000 1500 2000 2500 3000^3500 4000 4500 5000179(1a-39) x IMU/INS Position Matching Measurement Residual2 F-2^ - ,^ - - - - - ---3-4i5000 1500^2000^2500^3000^3500^4000^4500^5000time [s](1a-40) y IMU/INS Position Matching Measurement Residualtime [s]50403020100-10-20-30-40-501000(2a-1) Error in x INS Position Error Estimate1500^2000^2500^3000^3500^4000time [s](2a-2) Error in y INS Position Error Estimate1804500^500050403020--E—^10s-,^0Cc)^-10-20-30-401500^2000^2500^3000^3500^4000^4500^5000time [s]0.30.10.2(2a-3) Error in x INS Velocity Error Estimate11,181-0.1-0.21500^2000^2500^3000^3500^4000^4500^5000time [s](2a-4) Error in y INS Velocity Error Estimate0.30.20.1-0.1-0.2-013000^1500^2000^2500^3000^3500^4000^4500^5000time [s]5000-(115000 45001500 2000 3000^3500 400025000.50.40.30.2-- --------------- - ----------------------------------0.2-0.3-0.4182(2a-5) Error in x INS Platform Misalignment Estimate0.1 ---0--------------------------^-------------- _ -------------- ------------------------------- -------------- -------------- - -------------- - ----------------0.1-0.2-0.3-0.4 --015000 1500^2000^2500^3000^3500^4000^4500^5000time [s](2a-6) Error in y INS Platform Misalignment Estimatetime [s]0.50.40.30.2183(2a-7) Error in z INS Platform Misalignment Estimate3210-2-3-4-15000x10-310.80.60.40.20-0.2-0.4-0.6-0.8110001500^2000^2500^3000^3500^4000time [s](2a-8) Error in x INS Accelerometer Bias Estimate4500^5000--------------------------------------------^ -----------------------------1500 2000 2500 3000^3500 4000 4500 5000time [s]----------------------------------------------------------------------------------------------------------------10.80.60.4-----------------------------------\--------------------0.4 ---------------------------- -------------------------------------------------------0.6-0.811000 1500 2000 2500 3000^3500 4000 4500 5000184x10-3^(2a-9) Error in y INS Accelerometer Bias Estimate10.80.60.47-7 0.20-0.2-0.4-0.6-0.81000^1500^2000^2500^3000^3500^4000time [s]x10-4^(2a-10) Error in x INS Gyro Bias Estimate 4500 5000time [s]185x104^(2a-11) Error in y INS Gyro Bias Estimate10.80.6--------------------------^----------------------- --------------- - --------------^---------------------^----------------- ------------------------0.6-0.811000^1500^2000^2500^3000^3500time [s]x10-4^(2a-12) Error in z INS Gyro Bias Estimatetime [s]0.4-0.44000 4500 500050403020100-10-20-30-40-501000(2a-13) Error in x GPS Position Error Estimate1500^2000^2500^3000^3500^4000time [s](2a-14) Error in y GPS Position Error Estimate1864500^500050403020100-10-20-30-40------------------------------------------- ----------------------------------------1500^2000^2500^3000^3500 4000^4500^5000time [s]50403020100-10-20-30-40-501000(2a-15) Error in Corrected x IMU Position1500^2000^2500^3000^3500^4000time [s](2a-16) Error in Corrected y IMU Position1874500^500050403020100-10-20-30-401500^2000^2500^3000^3500^4000^4500^5000time [s](2a-17) Error in Corrected x IMU Velocity188I^1^I^1^I^I^i-1313000^1500^2000^2500^3000time [s](2a-18) Error in Corrected y IMU Velocity3500 4000 50004500-0.10.30.20.1-0.2-0'13000^1500^2000^2500^3000^3500^4000^4500^5000time [s]189(2a-19) Error in Corrected x IMU Platform Alignment0.5A0.30.20.10-0.1tu"-0.27:71-0.3-0.40.4-i---------------------------------------------------------------- --1^I^I^I^I^1-Cli5000^1500^2000^2500^3000^3500time [s](2a-20) Error in Corrected y IMU Platform Alignment-0.44115000 3000^3500 4500 500040001500 2000 25000.50.40.30.2-0.2-0.3time [s]4000 4500 5000V05^43210-1-2-3-4-15000x10-33210-1-2-130001500^2000^2500^3000^3500^4000^4500^50001500 2000time [s](2a-22) Error in x IMU Gyro Bias Estimate------ ----------------------------------------------------2500 3000^3500-----------------------------^---------4000 4500 5000(2a-21) Error in Corrected z IMU Platform Alignment190time [s]191x10-3^(2a-23) Error in y IMU Gyro Bias Estimate3210-1-21500^2000^2500^3000^3500^4000^4500^5000time [s]x10-3^(2a-24) Error in z IMU Gyro Bias Estimate3------------------------------ - -------------- - --------------- -----------^ ---- — -------------- — ----------------------------------------------------------------13000^1500^2000^2500^3000^3500^4000^4500^5000-?000time [s]-------------------------------------------32-2-3-42-------------------2-3 --4 -(2a-25) Error in Antenna Azimuth Misalignment Estimate1921500^2000^2500^3000^3500^4000^4500^5000time [s](2a-26) Antenna Azimuth Matching Measurement Residual1500^2000^2500^3000^3500^4000^4500^5000time [s]0.2t:4"75'010.80.60.4-0.2-0.4-0.6--0.8-11000^1500 2000^2500^3000^3500^4000^4500^5000(2a-27) Antenna Azimuth Matching Kalman Gaintime [s]19350403020100-10-20-30-40-501000(3a-1) Error in x INS Position Error Estimate1500^2000^2500^3000^3500^4000time [s](3a-2) Error in y INS Position Error Estimate1944500^500050403020100-10-20-30-40time [s]----- --- --- _ --- -------- --- _ ----------- ------------- -----------------------------------------(3a-3) Error in x INS Velocity Error Estimate1950.30.20.1-0.1-0.2I^i^1^I^I^I^I-0 13000^1500^2000^2500^3000^3500^4000time [s](3a-4) Error in y INS Velocity Error Estimate0.30.20.10-0.1-0.215004500^5000time [s]196(3a-5) Error in x INS Platform Misalignment Estimate0.50.40.30.2-0.2-0.3-0.41 -015000^1500^2000^2500^3000^3500time [s](3a-6) Error in y INS Platform Misalignment Estimate1500-015000 2000 2500 3000^3500 4000 4500 50000.50.40.30.2-0.2-0.3-0.4time [s]4000 4500 5000-----------------------------------3 --4197(3a-7) Error in z INS Platform Misalignment Estimate-15000^1500^2000^2500^3000^3500^4000^4500^5000time [s]x10-3^(3a-8) Error in x INS Accelerometer Bias Estimatetime [s]10.80.60.4-------------^--------------- -------------- ---------------- ---------------------------------------------0.4 ------------------------------0.6-0.82000 2500 3000^3500 4000 4500 5000il000 1500198x10-3^(3a-9) Error in y INS Accelerometer Bias Estimate2500^3000^3500^4000^4500^5000time [s]x104^(3a-10) Error in x INS Gyro Bias Estimatetime [s]199x10-4^(3a-11) Error in y INS Gyro Bias Estimate10.80.60.4---------- ------------- ------------- --------------- -------------- -^_ - ___ _ ____ -- ---- _ ------------- - _ ---- ---------- . ------ --------- ------ ----------------------------1500^2000^2500^3000^3500^4000^4500^5000time [s](3a-12) Error in z INS Gyro Bias Estimate2500time [s]50403020100-10-20-30-40-51U0^1500^2000^2500^3000^3500 4000^4500^500050403020100-10-20-30-40-501000(3a-13) Error in x GPS Position Error Estimate1500^2000^2500^3000^3500^4000time [s](3a-14) Error in y GPS Position Error Estimate2004500^5000time [s]10-20-30-4050403020-50 ^1000(3a-15) Error in Corrected x 1MU Position1500^2000^2500^3000^3500time [s](3a-16) Error in Corrected y IMU Position2014000^4500^5000time [s]202(3a-17) Error in Corrected x IMU Velocitytime [s](3a-18) Error in Corrected y IMU Velocitytime [s](3a-19) Error in Corrected x IMU Platform Alignment1^1^1^1^I^i^1-1315000^1500^2000^2500^3000time [s](3a-20) Error in Corrected y IMU Platform Alignment4000 500045001500^2000^2500^3000^3500time [s]2034000 500045003500(3a-21) Error in Corrected z IMU Platform Alignment2043-2-3-4------------ 7 -------------- _ ------------15000 1500^2000^2500^3000^3500^4000^4500^5000time [s]x10-3^(3a-22) Error in x IMU Gyro Bias Estimatetime [s]205x10-3^(3a-23) Error in y IMU Gyro Bias Estimate3210-1-2I^I^I^ I -13000^1500^2000^2500^3000^3500time [s]x10-3^(3a-24) Error in z IMU Gyro Bias Estimate34000 4500 5000-13000^1500^2000^2500^3000^3500^4000^4500^5000time [s]5-3-4-------------(3a-25) Error in Antenna Azimuth Misalignment Estimate1500^2000^2500^3000^3500time [s](3a-26) Antenna Azimuth Matching Measurement Residual2064000 4500 50005432-2-3-4-15000 1500^2000^2500^3000^3500^4000^4500^5000time [s](3a-27) Antenna Azimuth Matching Kalman Gain207-0.20.80.60.40.201-0.4-0.6-0.8-11000^1500^2000^2500^3000^3500^4000^4500^5000time [s]208APPENDIX BPERFORMANCE EVALUATION SIMULATION PLOTSThis appendix contains a full set of plots from a representative antenna boresight calibrationKalman filter performance evaluation simulation run (Section 6, simulation run 4c ).It will be noted that some plots have a saw-tooth appearance. In generating the plots, "true" INSerrors are computed from the synthesized INS data after subtracting the reference INS data.Quantization of the synthesized INS data results in "noise" in the computed true errors. Thisartifact of the quantization is most readily observed in the plots of true x and y INS platformmisalignment (plots 4c-5 and 4c-6). The effect on plots of the error in Kalman filter estimatedINS system states is also very apparent, especially in plots 4c-8, 4c-9, 4c-12 and 4c-13.dashed= true, solid =estimated500-500-2000-2500dashed =true, solid =estimated200^400^600^800^1000 1200 1400 1600 1800 2000-3000o(4c-1) True and Estimated x INS Position Error2090-500-1000-1500• -2000-25001) -3000-3500-4000-4500-5000o 200^400^600^800^1000 1200 1400 1600time [s](4c-2) True and Estimated y INS Position Error1800 2000time [s]0.5-1.5dashed=true, solid=estima1 200^400^600^800^1000 1200 1400 1600 1800 2000time [s]210(4c-3) True and Estimated x INS Velocity Error0.50-0.5-1E -1.5-2 -V-2.5-3dashed=true, solid=estim t200^400^600^800^1000 1200 1400^1600 1800 2000time [s](4c-4) True and Estimated y INS Velocity Error-3.5200 16001400 1800 2000I^Ifrldashed=true, solid=estimated0.30.2 -0.10-0.1-0.2-0.3.40 400^600^800^1000 12001400 1600 1800 2000200^400^600^800^1000 1200time [s](4c-6) True and Estimated y INS Platform Misalignment0.50.40.30.20.10-0.1-0.2 - dashed=true, solid=estimated-0.30(4c-5) True and Estimated x INS Platform Misalignment211time [s]dashed=true, solid =estimated(4c-7) True and Estimated z INS Platform Misalignment2122.52'•Ft1.5a)^1ot)0.5•0-0.5200^400^600^800^1000 1200 1400 1600 1800 2000-10time [s](4c-8) Error in x INS Position Error Estimate21350403020100-10-20-30-40-500 200^400^600^800^1000 1200 1400 1600 1800 2000time [s](4c-9) Error in y INS Position Error Estimate50403020100-10-20-30-40-500---------- ------------------------------------------------------^---------------------200^400^600^800^1000 1200 1400 1600 1800 2000time [s](4c-10) Error in x INS Velocity Error Estimate_ - - - -- -- - ------0.40.20.3s, ----- -200^400^600^800^1000 1200 1400 1600 1800 2000time [s](4c-11) Error in y INS Velocity Error Estimate-0.400.40.30.20.10-0.1-0.2-0.3200^400^600^800^1000 1200 1400 1600 1800 2000time [s]-0.40214Iv.10-0.2-0.30.50.40.30.20.10-0.1-0.2-0.3-0.4-0.50215(4c-12) Error in x INS Platform Misalignment EstimateI^I^1^I^1^I^1^1^1200^400^600^800^1000 1200 1400^1600^1800 2000time [s](4c-13) Error in y INS Platform Misalignment Estimate0.50.40.30.2''ciz 0.10-0.1-0.2-0.3-0.4-0.50 200^400^600^800^1000 1200 1400^1600 1800 2000time [s]216(4c-14) Error in z INS Platform Misalignment Estimate543210-1-2-3-4-5-----------------------------------^-----------------------------------------------------------------------^--------------------0^200^400^600^800^1000 1200 1400 1600 1800^2000time [s]x10-4^(4c-15) Estimated x INS Accelerometer Bias5200^400^600^800^1000 1200 1400 1600 1800 2000time [s]217x10-4^(4c-16) Estimated y INS Accelerometer Bias43.532.510.501600 18001400 2000- .50^200^400^600^800^1000^1200time [s](4c-17) Estimated x INS Gyro Bias1600 1800 2000200^400^600^800^1000 1200 1400time [s]x10-610^86420-2-4-6218(4c-18) Estimated y INS Gyro Bias-80^200^400^600^800^1000 1200 1400time [s]x10-6^(4c-19) Estimated z INS Gyro Bias20001600^18002200^400^600^800^1000 1200 1400^1600 1800 2000time [s](4c-20) Error in x GPS Position Error Estimate21950403020100-10-20-30-40-500 200^400^600^800^1000 1200 1400 1600 1800 2000time [s](4c-21) Error in y GPS Position Error Estimate50403020100- '-10--20-30 --40-500 200^400^600^800^1000 1200 1400 1600 1800 2000time [s]50403020100- 1 0-20-30-40200^400^600^800^1000 1200 1400^1600 1800 2000(4c-22) Error in Corrected x IMU Position22050403020100-10-20-30-40-5ooI^1^i^I^1^I^I^I 200^400^600^800^1000 1200 1400^1600time [s](4c-23) Error in Corrected y IMU Position1800 2000time [s]200^400^600^800^1000 1200time [s](4c-25) Error in Corrected y IMU Velocity200^400^600^800^1000 1200 1400 1600 1800 2000221(4c-24) Error in Corrected x IMU Velocitytime [s](4c-26) Error in Corrected x IMU Platform Alignment2220.50.40.30.20.10-0.1-0.2-0.3-0.4-0.50 200^400^600^800^1000 1200 1400 1600 1800 2000time [s](4c-27) Error in Corrected y IMU Platform Alignment0.50.40.30.20.10-0.1-0.2-0.3-0.4-0.50 I^1^i^1^,^1^I^I^i^1 I200^400^600^800^1000 1200 1400 1600 1800 2000time [s]time [s](4c-29) Estimated x IMU Gyro Bias200^400^600^800^1000 1200 1400^1600 1800 2000223(4c-28) Error in Corrected z IMU Platform Alignment- --- - ------- - --^ -------- -200^400^600^800^1000 1200 1400^1600^1800^2000time [s]20-2-4-10-12-14200^400^600^800^1000 1200 1400 1600 1800 2000-160224x10-4^(4c-30) Estimated y IMU Gyro Bias6420-4-6-8-10-121^1^I^i^1^I^I^1^I-140^200^400^600^800^1000^1200^1400time [s]x10-4^(4c-31) Estimated z IMU Gyro Biastime [s]1600^1800 2000---------------225(4c-32) Error in Antenna Azimuth Misalignment Estimate200^400^600^800^1000 1200 1400 1600 1800 2000time [s]20151050-5-10-15-2%226(4c-33) x INS/GPS Position Matching Measurement Residual200^400^600^800^1000 1200 1400 1600^1800 2000time [s](4c-34) y INS/GPS Position Matching Measurement Residual-----------------------^ -------------------j--------^ I------- - ! -----------------------200^400^600^800^1000 1200 1400^1600^1800 2000time [s]2015105+I-10-15-200227(4c-35) x IMU/INS Position Matching Measurement Residual20151050-5-10-15- --------- - --„______- -------- - -- is,-2%^200^400^600^800^1000 1200 1400 1600 1800 2000time [s](4c-36) y IMU/INS Position Matching Measurement Residual20151050a.)^-5---------------- 1 0-15-2%^200^400^600^800^1000 1200 1400 1600 1800 2000time [s]228(4c-37) Antenna Azimuth Matching Measurement Residual200^400^600^800^1000 1200 1400 1600 1800 2000time [s](4c-38) Antenna Azimuth Matching Kalman Gain0.80.60.40.2I^I^I^I^I^I 200^400^600^800^1000 1200time [s]•20 1400^1600^1800 2000229APPENDIX CSOFTWARE LISTINGSThis appendix contains listings of the software modules comprising the data processing packageof the simulator. The following modules are included:• Antenna boresight calibration main programCALPRO.HCALPRO.CPP• System model moduleCALMDL.HCALMDL.CPP• Generic Kalman filter moduleKFLT.HPHIQD.CPPEXTRAP.CPPUPDAT.CPP• Inertial navigation moduleNAVGTR.HNAVGTR.CPP• Navigation libraryNAVLIB.HERADII.CPPINVRADII.CPPGRAVITY. CPPRE2GEOD.CPPGEOD2RE.CPPEA2CBW.CPPEA2CWE.CPPEA2CEG.CPPEA2CGB.CPPCBW2EA.CPPCWE2EA.CPPDCM2Q.CPPQUPDT.CPPQ2DCM.CPP230CALPRO.H// CALPRO.H// IMU measurement record.typedef struct fdouble time;double Dvel[3];double Dang1[3];1 imumes_record;// INS measurement record.typedef struct {double time;double latitude;double longitude;double gndspeed;double track;double heading;double pitch;double roll;double bdy_pitch_rate;double bdy_roll_rate;double bdy_yaw_rate;double plat_hdg;double altitude;double vertspeed;double N_velocity;double E_velocity;} insmes_record;// Baroaltitude measurement record;typedef struct {double time;double baroalt;} altmes_record;// GPS measurement record.typedef struct fdouble time;double latitude;double longitude;double altitude;double N_velocity;double E_velocity;double D_velocity;1 gpsmes_record;// Azimuth encoder measurement record.typedef struct fdouble time;double azimuth;1 azmmes_record;// IMU navigation output record.typedef struct {double time;double latitude;double longitude;double altitude;double gndspeed;double track;double heading;double azimuth;double pitch;double roll;double vertspeed;231CALPRO.H1 NOO_record;// State vector output record.typedef struct {double time;double x[25];} NOl_record;// State error variance output record.typedef struct {double time;double Pdiag[25];double Poffd[7];1 NO2 record;// IMU gyro bias output record.typedef struct {double time;double gb[3];} NO3 record;// Measurement residual output record.typedef struct {double time;double r[5];} N04 record;// Measurement residual variance output record.typedef struct {double time;double Sdiag[5];} N05 record;// Kalman gain output record.typedef struct {double time;double K[5];} N06 record;// Filtered acceleration and filtered heading rate output record.typedef struct {double time;double gndspeed;double accel_f[3];double hdgrate_f;} N07 record;CALPRO.CPP^ 232// CALPRO.CPP (main)// Antenna azimuth calibration processing.I-II CALPRO does not use any input or output parameters. Instead, the required// input files must be present in the default directory with the default// names CALPRO.ext where ext is one of:I-II IMU = IMU measurement data.// INS = INS measurement data.// ALT = Baroaltitude measurement data.// GPS = GPS measurement data.// AZM = Azimuth encoder measurement data.I-II Output files are produced with the default names CALPRO.ext where ext is// one of:////^$00 = corrected IMU navigation data.//^$01 = state vector.//^$02 = state error variances.//^$03 = IMU gyro biases.//^$04 = measurement residual vector.//^$05 = measurement residual variances.//^$06 = Kalman gain (elements directly affecting the antenna azimuth// misalignment estimate).//^$07 = Filtered acceleration and filtered heading rate.#include <fstream.h>#include <stdlib.h>^// exit#include <math.h>#include "calpro.h"#include "navgtr.h"#include "navlib.h"#include "calmdl.h"#include "kflt.h"#define TRUE 1#define FALSE 0struct inswork_recorddouble Rxinv;double Ryinv;double Tiny;double gravity;double wander;double Vw[3];double wiew[3];double weww[3];double fw[3];double Cbw[3][3];double Cwe[3][3];void inscalc(long cycle_count, double Dtins, insmes_record insrec,inswork_record &inswrk, double accel_f[3], double &hdgrate_f);void navcor(double naverr[10], double Dtimu,navgtr_record &navrec, navwork_record &navwrk,double gyro_bias[3]);void main(void){ const double target lat=(44.0+52.0/60.0+6.0/3600.0)*M_PI/180.0;const double target—long=-(69.0+30.0/60.0+38.0/3600.0)*M_PI/180.0;const double target=alt=0.0*0.3048;233CALPRO.CPPconst double RRP[3]={6.0, -0.5, -0.11;const double RSD[3]={0.0, 0.0, 0.01;const double RGPS[3]={-3.0, -0.5, -2.01;const double Dtimu=1.0/50.0;const double Dtins=1.0/16.0;const double Dtalt=1.0/2.0;const double Dtgps=1.0;const double Dtazm=10.0;const double Dtupdt_min=0.75;const double Dtupdt_max=1.5;const int nTPHI=3;const int nTQd=6;if stream imu_file;if stream ins_file;if stream alt_file;if stream gps_file;if stream azm_file;of stream NOO_file;of stream NOl_file;of stream NO2 file;of stream NO3 file;of stream N04- file;of stream N05 file;of stream N06 file;of stream N07 file;imumes_record imurec;insmes_record insrec;altmes_record altrec;gpsmes_record gpsrec;azmmes_record azmrec;navgtr_record navrec;navwork_record navwrk;inswork_record inswrk;NOO_record NOOrec;NOl_record NOlrec;NO2 record NO2rec;NO3_record NO3rec;N04 record NO4rec;N05_record NO5rec;N06 record NO6rec;NO7_record NO7rec;struct ready_record {int imu;int ins;int alt;int gps;int azm;} rdyrec;double TIME 10;double TIME - hi;double start_ time;double end_time;double cal_start_time;double cal_end_time;double cur_time;double stamp_time;double tau time;CALPRO.CPP^ 234double kflt_time;double Dtkflt;long cycle_count;long imu_cycle_count;long ins_cycle_count;long kflt_cycle_count;int calibrate;int calibrate_last=FALSE;int gps_rdy;int tau_rdy;int kflt_go;double altref;double M;double N;double RN;double RP;double accel_f[3];double hdgrate_f;int insfw count;int imufw count;double inifw_avg[3];double imufw_avg[3];double Cgb[3][3];double naverr[10];double gyro_bias[3];int nsys;int mmes;Kflt_vector x;Kflt_vector z;Kflt_vector r;Kflt_matrix F;Kflt matrix Q;Kflt=matrix PHI;Kflt_matrix Qd;Kflt_matrix H;Kflt_matrix R;Kflt_matrix S;Kflt_matrix P;Kflt_matrix K;Kflt_tmp tmp;int open_fail;double a;double b;int i;int j;// Open the input files.open_fail=FALSE;imu_file.open("CALPRO.IMU", ios::nocreatelios::binary);if (!imu_file) {cerr << "Unable to open imu measurements file - CALPRO.IMU\n";open_fail=TRUE;}ins_file.open("CALPRO.INS", ios::nocreatelios::binary);if (line file) {cerr << "Unable to open ins measurements file - CALPRO.INS\n";open_fail=TRUE;}alt file.open("CALPRO.ALT", ios::nocreatelios::binary);if (lalt_file) {cerr << "Unable to open baroaltitude measurements file - CALPRO.ALT\n";open_fail=TRUE;CALPRO.CPP^ 2351gps file.open("CALPRO.GPS",if T!gps_file) fcerr << "Unable to open gopen_fail=TRUE;1azm file.open("CALPRO.AZM",if (lazm_file) {cerr << "Unable to open aopen_fail=TRUE;}if (open fail)exit(ERIT_FAILURE);// Create the output files.N00_file.open("CALPRO.$00",if (!N00 file) fcerr << "Unable to createexit(EXIT_FAILURE);1N01 file.open("CALPRO.$01",if TINOl_file) {cerr << "Unable to createexit(EXIT_FAILURE);}NO2 file.open("CALPRO.$02",if T!NO2_file) fcerr << "Unable to createexit(EXIT_FAILURE);1NO3 file.open("CALPRO.$03",if T1NO3_file) {cerr << "Unable to createexit(EXIT_FAILURE);}N04 file.open("CALPRO.$04",if T!NO4_file) fcerr << "Unable to createexit(EXIT_FAILURE);1N05 file.open("CALPRO.$05",if (!N05 file) {cerr <<- "Unable to createexit(EXIT_FAILURE);}N06 file.open("CALPRO.$06",if TIN06_file) {cerr << "Unable to createexit(EXIT_FAILURE);}N07 file.open("CALPRO.$07",if (!N07 file) {cerr << "Unable to createexit(EXIT_FAILURE);}ios::nocreatelios::binary);ios::binary);output file - CALPRO.$00\n";ios::binary);output file - CALPRO.$01\n";ios::binary);output file - CALPRO.$02\n";ios::binary);output file - CALPRO.$03\n";ios::binary);output file - CALPRO.$04\n";ios::binary);output file - CALPRO.$05\n";ios::binary);output file - CALPRO.$06\n";ios::binary);output file - CALPRO.$07\n";ps measurements file - CALPRO.GPS\n";ios::nocreatelios::binary);zimuth encoder measurements file - CALPRO.AZM\n";// Get the start and end times.azm_file.read((char *) &azmrec, sizeof(azmrec));TIME_lo=azmrec.time-Dtazm;azm_file.seekg(-1L*sizeof(azmrec), ios::end);azm_file.read((char *) &azmrec, sizeof(azmrec));TIME_hi=azmrec.time+Dtazm;azm file.clear();azm_file.seekg(OL);gps_file.read((char *) &gpsrec, sizeof(gpsrec));CALPRO.CPPif (gpsrec.time-Dtgps>TIME_lo)TIME_lo=gpsrec.time-Dtgps;gps_file.seekg(-1L*sizeof(gpsrec), ios::end);gps_file.read((char *) &gpsrec, sizeof(gpsrec));if (gpsrec.time+Dtgps<TIME_hi)TIME_hi=gpsrec.time+Dtgps;gps_file.clear();gps_file.seekg(OL);alt_file.read((char *) &altrec, sizeof(altrec));if (altrec.time-Dtalt>TIME_lo)TIME_lo=altrec.time-Dtalt;alt_file.seekg(-1L*sizeof(altrec), ios::end);alt_file.read((char *) &altrec, sizeof(altrec));if (altrec.time+Dtalt<TIME_hi)TIME_hi=altrec.time+Dtalt;alt_file.clear();alt_file.seekg(OL);ins_file.read((char *) &insrec, sizeof(insrec));if (insrec.time-Dtins>TIME_lo)TIME_lo=insrec.time-Dtins;ins_file.seekg(-1L*sizeof(insrec), ios::end);ins_file.read((char *) &insrec, sizeof(insrec));if (insrec.time+Dtins<TIME_hi)TIME_hi=insrec.time+Dtins;ins_file.clear();ins_file.seekg(OL);imu_file.read((char *) &imurec, sizeof(imurec));if (imurec.time>TIME_lo)TIME_lo=imurec.time;imu_file.seekg(-1L*sizeof(imurec), ios::end);imu_file.read((char *) &imurec, sizeof(imurec));if (imurec.time<TIME_hi)TIME_hi=imurec.time;imu_file.clear();imu_file.seekg(OL);cout «do {cout « "Enter processing start time between« TIME lo « " and " « TIME hi « ": ";cmn >> start:time;1 while (start_time<TIME_lo 11 start_time>=TIME_hi);cout « "\n";do {cout « "Enter processing end time between« start time « " and " « TIME hi « ": ";cm >> end time;} while (end_time<start_time 11 end_time>TIME_hi);cout «do fcout « "Enter calibration start time greater than or equal to« start_time^": ";cmn >> cal_start_time;1 while (cal_start_time<start_time);cout <<if (cal_start_time<end_time) {do {cout « "Enter calibration end time betweencal_start_time « " and "^end_time^": ";cmn >> cal_end_time;} while (cal_end_time<cal_start_time 11 cal_end_time>end_time);cout 1else {cal_start_time=end_time;cal end time=cal start time;236237CALPRO.CPP1// Main loop.imu_cycle_count=0L;ins_cycle_count=0L;kflt_cycle_count=0L;cycle_count=0L;cur_time=start_time;stamp_time=cur_time;calibrate=FALSE;while (cur_time<end_time) {if (cur_time>=stamp_time) {cout << cur_time << "\tTime stamp\n";stamp_time=stamp_time+100.0;}// Get the sensor measurement data.if (cycle count==0L) {rdyrec.rmu=FALSE;rdyrec.ins=FALSE;rdyrec.alt=FALSE;rdyrec.gps=FALSE;rdyrec.azm=FALSE;do {ins_file.read((char *) &insrec, sizeof(insrec));} while (insrec.time<start_time);do {imu_file.read((char *) &imurec, sizeof(imurec));} while (imurec.time<insrec.time);start_time=imurec.time;do {alt_file.read((char *) &altrec, sizeof(altrec));} while (altrec.time<=start_time);do {gps_file.read((char *) &gpsrec, sizeof(gpsrec));1 while (gpsrec.time<=start time);_1if (rdyrec.ins) {rdyrec.ins=FALSE;ins_file.read((char *) &insrec, sizeof(insrec));}if (rdyrec.alt) {rdyrec.alt=FALSE;alt _file.read((char *) &altrec, sizeof(altrec));1if (rdyrec.gps) {rdyrec.gps=FALSE;gps_file.read((char *) &gpsrec, sizeof(gpsrec));1if (rdyrec.azm) frdyrec.azm=FALSE;azm file.read((char *) &azmrec, sizeof(azmrec));_}1rdyrec.imu=TRUE;cur_time=imurec.time;if (insrec.time<=cur_time)do {azm_file.read((char *) &azmrec, sizeof(azmrec));} while (azmrec.time<=start_time);}else {if (rdyrec.imu) frdyrec.imu=FALSE;imu_file.read((char *) &imurec, sizeof(imurec));CALPRO.CPP^ 238rdyrec.ins=TRUE;if (altrec.time<=cur_time)rdyrec.alt=TRUE;if (gpsrec.time<=cur_time)rdyrec.gps=TRUE;if (azmrec.time<=cur_time)rdyrec.azm=TRUE;// Process the INS measurements.if (rdyrec.ins) {inscalc(ins_cycle_count, Dtins, insrec, inswrk, accel_f, hdgrate_f);if (kflt_cycle_count>OL) {insfw count=insfw_count+1;b=1.07insfw count;a=1.0-b;^—for (i=0; i<3; i++)insfw_avg[i]=a*insfw_avg[i]+b*inswrk.fw[i];1ins_cycle_count=ins_cycle_count+1L;1If Process the baroaltitude measurements.if (rdyrec.alt)altref=altrec.baroalt;// Process the IMU measurements.if (rdyrec.imu) {if (imu_cycle_count==0L) {Ea2Cgb(insrec.roll, insrec.pitch, insrec.heading, Cgb);eradii(insrec.latitude, M , N);RM=M+insrec.altitude;RP=(N+insrec.altitude)*cos(insrec.latitude);navrec.time=insrec.time;navrec.latitude=insrec.latitude+((Cgb[0][0]*RRP(0)+Cgb(1)(0)*RRP[1)+Cgb[2][0]*RRP(2))+(RSD(0)*cos(insrec.heading)-RSD[1]*sin(insrec.heading)))/RM;navrec.longitude=insrec.longitudefl(Cgb(0)(1)*RRP[0]+Cgb[1][1]*RRP[1]+Cgb(2)[1]*RRP[2])+(RSD[0]*sin(insrec.heading)+RSD[1]*cos(insrec.heading)))/RP;navrec.altitude=insrec.altitude-((Cgb[0][2)*RRP[0]+Cgb[1][2]*RRP[1]+Cgb[2][2]*RRP[2])+RSD[2]);navrec.gndspeed=insrec.gndspeed;navrec.track=insrec.track;navrec.heading=insrec.heading;navrec.azimuth=insrec.plat_hdg;navrec.pitch=0.0;navrec.roll=0.0;navrec.vertspeed=insrec.vertspeed;altref=navrec.altitude;for (i=0; i<3; i++)gyro_bias(i]=0.0;1for (i=0; i<3; i++)imurec.Dangl[i]=imurec.Dangl[i]-gyro_bias[i]*Dtimu;navgtr(imu_cycle_count, cur_time, Dtimu,imurec.Dvel, imurec.Dangl, altref, navrec, navwrk);if (kflt_cycle_count>OL) {imufw count=imufw_count+1;b=1.07imufw_count;a=1.0-b;for (i=0; i<3; i++)imufw_avg[i]=a*imufw_avg[i]+b*navwrk.fw[i];1imu_cycle_count=imu_cycle_count+1L;1CALPRO.CPP^ 239// Kalman filter timing.// - Allow use of all azimuth measurements that occur between the//^calibration start and end times.// - If not calibrating, allow use of all GPS measurements.// - If calibrating, ignore GPS measurements when an azimuth record is//^expected.// - Ensure Kalman filter updates are appropriately separated in time//^(not too close together and not too far apart).tau rdy=FALSE;if Trdy ec.azm)if (azmrec.time>=cal_start_time && azmrec.time<cal_end_time) {calibrate=TRUE;tau_rdy=TRUE;tau_time=azmrec.time;elsecalibrate=FALSE;gps_rdy=FALSE;if (rdyrec.gps)if (lcalibrate)gps_rdy=TRUE;elseif ((gpsrec.time-tau_time)<=(Dtazm-O.75*Dtgps) 11 tau_rdy)gps_rdy=TRUE;kflt_go=FALSE;if (kflt_cycle count==0L) {if (gps_rdy T1 tau_rdy) {kflt go=TRUE;if (Eau_rdy)kflt_time=azmrec.time;elsekflt_time=gpsrec.time;else {if ((gps_rdy 11 tau_rdy 11 (cur_time-kflt_time)>=Dtupdt_max) &&(cur time-kflt time)>=Dtupdt_min) {kflt_gT)=TRUE; —if (tau_rdy) {Dtkflt=azmrec.time-kflt_time;kflt_time=azmrec.time;1else if (gps_rdy) {Dtkflt=gpsrec.time-kflt_time;kflt_time=gpsrec.time;else {Dtkflt=cur_time-kflt_time;kflt_time=cur_time;}// Perform the antenna azimuth calibration Kalman filtering.if (kflt_go) {if (kflt_cycle_count==0L) {calmdl wrk(nsys, mmes, x, z, r, F, Q, PHI, Qd, H, R, S, P, K, tmp);calmd1=x0P0();else {if (!calibrate && calibrate_last) {for (i=0; i<14; i++)for (j=14; j<25; j++) {240CALPRO.CPPP[i][j]=0.0;P[j][i]=0.0;1for (i=0; i<24; i++) fP[i][24]=0.0;P[24] [i]=0.0;}}calibrate_last=calibrate;naverr[0]=x[0]+x[14];naverr[1]=x[1]+x[15];naverr[2]=x[2]+x[16];naverr[3]=x[3]+x[17];naverr[4]=x[4]+x[18];naverr[5]=x[5]+x[19];naverr[6]=x[6]+x[20];naverr[7]=x[21];naverr[8]=x[22];naverr[9]=x[23];navcor(naverr, Dtimu, navrec, navwrk, gyro_bias);calmdl_mes(insrec.latitude, insrec.longitude, insrec.altitude,inswrk.Cwe, inswrk.Cbw,navrec.latitude, navrec.longitude, navrec.altitude,navrec.heading, navwrk.wander,navwrk.Cwe, navwrk.Cbw,gps_rdy, gpsrec.latitude, gpsrec.longitude,gpsrec.altitude,tau_rdy, azmrec.azimuth,RRP, RSD, RGPS, target_lat, target_long, target_alt);calmdl FQ(inswrk.Rxinv, inswrk.Ryinv,Inswrk.Vw, inswrk.wiew, inswrk.weww,insfw_avg, inswrk.Cbw,navwrk.Rxinv, navwrk.Ryinv,navwrk.Vw, navwrk.wiew, navwrk.weww,imufw_avg, navwrk.Cbw,insrec.gndspeed, accel_f, hdgrate_f);PHIQd(nsys, nTPHI, nTQd, Dtkflt, F, Q, PHI, Qd, tmp);extrap(nsys, PHI, Qd, x, P, tmp);x[14]=x[14]-naverr[0];x[15]=x[15]-naverr[1];x[16]=x[16]-naverr[2];x[17]=x[17]-naverr[3];x[18]=x[18]-naverr[4];x[19]=x[19]-naverr[5];x[20]=x[20]-naverr[6];x[21]=x[21]-naverr[7];x[22]=x[22]-naverr[8];x[23]=x[23]-naverr[9];updat(nsys, mines, z, H, R, r, S, K, x, P. tmp);1// Output the results.NOOrec.time=kflt_time;NOOrec.latitude=navrec.latitude;NOOrec.longitude=navrec.longitude;NOOrec.altitude=navrec.altitude;NOOrec.gndspeed=navrec.gndspeed;NOOrec.track=navrec.track;NOOrec.heading=navrec.heading;NOOrec.azimuth=navrec.azimuth;NOOrec.pitch=navrec.pitch;NOOrec.roll=navrec.roll;NOOrec.vertspeed=navrec.vertspeed;if (INOO_file.write((char *) &NOOrec, sizeof(NOOrec))) {cerr << "Write error on output file - CALPRO.$00\n";CALPRO.CPPexit(EXIT_FAILURE);}NOlrec.time=kflt_time;for (i=0; i<nsys; i++)NOlrec.x[i]=x[i];if (INOl_file.write((char *) &NOlrec, sizeof(NOlrec))) {cerr << "Write error on output file - CALPRO.$01\n";exit(EXIT_FAILURE);}NO2rec.time=kflt time;for (i=0; i<nsys7 i++)NO2rec.Pdiag[i]=P[i][i];for (i=0; i<7; i++)NO2rec.Poffd[i]=P[O+i)(14+i];if (INO2_file.write((char *) &NO2rec, sizeof(NO2rec))) {cerr << "Write error on output file - CALPRO.$02\n";exit(EXIT_FAILURE);}NO3rec.time=kflt time;for (i=0; i<3; iT+)NO3rec.gb[i]=gyro_bias[i);if (INO3 file.write((char *) &NO3rec, sizeof(NO3rec))) {cerr <Z "Write error on output file - CALPRO.$03\n";exit(EXIT_FAILURE);}NO4rec.time=kflt time;for (i=0; i<mmes7 i++)NO4rec.r[i]=r[i];if (INO4 file.write((char *) &NO4rec, sizeof(NO4rec))) {cerr <Z "• Write error on output file - CALPRO.$04\n";exit(EXIT FAILURE);}^(EX _for (i=0; i<mmes; i++)NO5rec.Sdiag[i]=S[i][i];if (INO5 file.write((char *) &NO5rec, sizeof(NO5rec))) {cerr <7c- " Write error on output file - CALPRO.$05\n";exit(EXIT_FAILURE);}NO6rec.time=kflt time;for (i=0; i<mmes7 i++)NO6rec.K[i]=K[24][i];if (INO6 file.write((char *) &NO6rec, sizeof(NO6rec))) {cerr <7Z "Write error on output file - CALPRO.$06\n";exit(EXIT_FAILURE);}NO7rec.time=kflt_time;NO7rec.gndspeed=insrec.gndspeed;for (i=0; i<3; i++)NO7rec.accel_f[i)=accel_f[i];NO7rec.hdgrate_f=hdgrate_f;if (IN07_file.write((char *) &NO7rec, sizeof(NO7rec))) {cerr << "Write error on output file - CALPRO.$07\n";exit(EXIT_FAILURE);}241insfw_count=0;imufw_count=0;kflt_cycle_count=kflt_cycle_count+1L;1cycle_count=cycle_count+1L;1// Normal exit.imu_file.close();}ins_file.close();alt_file.close();gps_file.close();azm_file.close();NO0 file.close();_NO1 file.close();NO2:file.close();NO3 file.close();_NO4 file.close();NO5 -file.close();_NO6 file.close();NO7 -file.close();exit(EXIT_SUCCESS);CALPRO.CPP 242// Local function to calculate some required INS navigation quantities.//void inscalc(long cycle count, double Dt, insmes record n, inswork_record &w,double acce-l_f[3], double &hdgrate_Y){const double tau=10.0;const double b=Dt/tau;const double a=1.0-b;double DVw[3];double vectmp[3];Ea2Cbw(n.roll, n.pitch, n.plat_hdg, w.Cbw);w.wander=n.plat_hdg-n.heading;if (w.wander>=M_PI)w.wander=w.wander-2.0*M_PI;else if (w.wander<-M_PI)w.wander=w.wander+2.0*M PI;Ea2Cwe(n.latitude, n.longitude, w.wander, w.Cwe);vectmp[0]=w.Vw[0];vectmp[1]=w.Vw[1];vectmp[2]=w.Vw[2];w.Vw[0]=n.gndspeed*cos(n.track+w.wander);w.Vw[1]=-n.gndspeed*sin(n.track+w.wander);w.Vw[2]=n.vertspeed;if (cycle_count==0L) {DVw[0]=0.0;DVw[1]=0.0;DVw[2]=0.0;1else {DVw[0]=w.Vw[0]-vectmp[0];DVw[1]=w.Vw[1]-vectmp[1];DVw[2]=w.Vw[2]-vectmp[2];1invradiil(w.Cwe, n.altitude, w.Rxinv, w.Ryinv, w.Tinv);w.weww[0]=-w.Vw[1]*w.Ryinv+w.Vw[0]*w.Tinv;w.weww[1]=w.Vw[0]*w.Rxinv-w.Vw[1]*w.Tinv;w.weww[2]=0.0;w.wiew[0]=w.Cwe[0][0]*E_wie;w.wiew[1]=w.Cwe[0][1]*E_wie;w.wiew[2]=w.Cwe[0][2]*E_wie;w.gravity=gravityl(w.Cwe, n.altitude);CALPRO.CPPvectmp[0]=2.0*w.wiew[0]+w.weww[0];vectmp[1]=2.0*w.wiew[1]+w.weww[1];vectmp[2]=2.0*w.wiew[2]+w.weww[2];w.fw[0]=DVw[0]/Dt+(vectmp[1]*w.Vw[2]-vectmp[2]*w.Vw[1]);w.fw[1]=DVw[1]/Dt+(vectmp[2]*w.Vw[0]-vectmp[0]*w.Vw[2]);w.fw[2]=DVw[2]/Dt+w.gravity+(vectmp[0]*w.Vw[1]-vectmp[1]*w.Vw[0]);if (cycle count==0L) {accel_f[0]=w.fw[0];accel_f[1]=w.fw[1];accel f[2]=w.fw[2]-w.gravity;hdgrate_f=(n.bdy_pitch rate*sin(n.roll)+n.bdy_yaw_rite*cos(n.roll))/cos(n.pitch);1else {accel_f[0]=a*accel_f[0]+b*w.fw[0];accel_f[1]=a*accel_f[1]+b*w.fw[1];accel f[2]=a*accel f[2]+b*(w.fw[2]-w.gravity);hdgrate f=a*hdgrat-e- f+b*(n.bdy_pitch rate*sin(n.roll)+n.bdy_yaw_rae*cos(n.roll))/cos(n.pitch);return;1243// Local function to correct// estimated errors.//void navcor(double naverr[10navgtr_record &nthe IMU navigation information using the], double Dt,, navwork_record &w, double gb[3])double q[4];double u[4];double vectmp[3];// Correct the wander to earth quaternion (and related quantities// q=q+qu where q is the quaternion qwe and u is the quaternion// corresponding to the vector -0.5*delta_theta.q[0]=w.qwe[0];q[1]=w.qwe[1];q[2]=w.qwe[2];q[3]=w.qwe[3];u[0]=0.0;u[1]=0.5*naverr[1)*w.Ryinv;u[2]=-O.5*naverr[0]*w.Rxinv;u[3]=0.0;w.qwe[0]=q[0]-q[1]*u[1]-q[2]*u[2]-q[3]*u[3];w.qwe[1]=q[1]+q[0]*u[1]+q[2]*u[3]-q[3]*u[2];w.qwe[2]=q[2]+q[0]*u[2]-q[1]*u[3]+q[3]*u[1];w.qwe[3]=q[3]+q[0]*u[3]+q[1]*u[2]-q[2]*u[1];q2dcm(w.qwe, w.Cwe);w.wiew[0]=w.Cwe[0][0]*E_wie;w.wiew[1]=w.Cwe[0][1]*E_wie;w.wiew[2]=w.Cwe[0][2]*E_wie;invradiil(w.Cwe, n.altitude, w.Rxinv, w.Ryinv, w.Tinv);w.gravity=gravityl(w.Cwe, n.altitude);Cwe2Ea(w.Cwe, n. latitude, n. longitude, w.wander);// Correct the velocity (and related quantities).CALPRO.CPP^ 244w.Vw[0]=w.Vw[0]-naverr[2];w.Vw[1]=w.Vw(1)-naverr[3];w.weww[0]=-w.Vw[1]*w.Ryinv+w.Vw[0]*w.Tinv;w.weww[1]=w.Vw[0]*w.Rxinv-w.Vw[1]*w.Tinv;w.weww[2]=0.0;n.gndspeed=sqrt(w.Vw[0]*w.Vw[0]+w.Vw[1]*w.Vw[1]);n.track=atan2(-w.Vw[1], w.Vw[0])-w.wander;if (n.track>=M_PI)n.track=n.track-2.0*M_PI;else if (n.track<-M_PI)n.track=n.track+2.0*M PI;_// Correct the body to wander quaternion (and related quantities).// q=q+usg where q is the quaternion qbw and u is the quaternion// corresponding to the vector 0.5*phi.q[0]=w.gbw[0];q[1]=w.gbw[1];q[2]=w.gbw[2];q[3]=w0gbw[3];u[0]=0.0;u[1]=(0.5*naverr[4])/1000.0;u[2]=(0.5*naverr[5])/1000.0;u[3]=(0.5*naverr[6])/1000.0;woabw(0)=q[0]-u[1]*q[1]-u[2]*q[2]-u[3]*q[3];w.gbw[1]=q[1]+u[1]*q[0]+u[2]*q[3]-u[3]*q[2];w.gbw[2]=q[2]-u(1)*q[3]+u[2]*q[0]+u[3]*q[1];w.gbw[3]=q[3]+u[1]*q(2)-u[2]*q[1]+u[3]*q[0];q2dcm(w.gbw, w.Cbw);vectmp[0]=(w.wiew[0]+w.weww[0])*Dt;vectmp[1]=(w.wiew[1]+w.weww[1])*Dt;vectmp[2]=(w.wiew[2]+w.weww[2])*Dt;w.Dthiwb[0]=w.Cbw(0)[0]*vectmp[0]+w.Cbw[l][0]*vectmp[1]+w.Cbw[2][0]*vectmp[2];w.Dthiwb(1)=w.Cbw[0][1]*vectmp(0)+w.Cbw(].][1]*vectmp[1]+w.Cbw[2][1]*vectmp(2);w.Dthiwb[2]=w.Cbw[0][2]*vectmp[0]+w.Cbw[l][2]*vectmp[1]+w.Cbw[2][2]*vectmp[2];Cbw2Ea(w.Cbw, n.roll, n.pitch, n.azimuth);n.heading=n.azimuth-w.wander;if (n.heading>=M_PI)n.heading=n.heading-2.0*M_PI;else if (n.heading<-M_PI)n.heading=n.heading+2.0*M_PI;// Update the gyro bias.gb[0]=gb(0)+naverr[7]/1000.0;gb(1)=gb[1]+naverr[8]/1000.0;gb(2)=gb[2]+naverr[9]/1000.0;return;1CALMDL . H^245II CALMDL.H#ifndef KFLT H#include "kflt7h"#endif//// Provide access to the working data store.// * ****void calmdl_wrk(int &nays, int &mmes,Kflt vector &x, Kflt vector &z, Kflt_vector &r,Kflt—matrix &F, Kflt—matrix &Q,Kflt matrix KPHI,flt matrix &Qd,Kflt—matrix &H, Kflt matrix &R, Kflt matrix &S,Kflt=matrix &P, Kflt=matrix &K, Kflt=tmp &tmp);// *****// Initialize the state estimate vector (x) and the error covariance// matrix (P).// *****'-void calmdl_x0P0(void);// *****// Construct the system dynamics matrix (F) and the process noise spectral// density matrix (Q).// *****//void calmdl_FQ(double insRxinv, double insRyinv,double insVw[3], double inswiew[3], double insweww[3],double insfw[3], double insCbw[3][3],double imuRxinv, double imuRyinv,double imuVw[3], double imuwiew[3], double imuweww[3],double imufw[3], double imuCbw[3][3],double gndspeed, double accel[3], double hdgrate);// *****// Construct the measurement vector (z), the measurement matrix (H) and the// measurement noise covariance matrix (R).// *****'-void calmdl mes(double inslat, double inslong, double insalt,double insCwe[3][3], double insCbw[3][3],double imulat, double imulong, double imualt,double imuhdg, double imuwan,double imuCwe[3][3], double imuCbw[3][3],int gpsrdy, double gpslat, double gpslong, double gpsalt,int taurdy, double tauazm,const double RRP[3], const double RSD[3],const double RGPS[3],double tgtlat, double tgtlong, double tgtalt);* * * * *CALMDL.CPP^ 246// CALMDL.CPPI-II The following variables are defined and allocated storage:I-II^x = State estimate vector.// z = Measurement vector.//^r = Residual vector.// F = System dynamics matrix.//^Q = Process noise spectral density matrix.//^PHI = State transistion matrix.//^Qd = Process noise covariance matrix.// H = Measurement matrix.//^R = Measurement noise covariance matrix.// S = Residual covariance matrix.//^P = Error covariance matrix.// K = Kalman gain matrix.// tmp = Temporary storage required by the Kalman filter functions.I-II// The following functions are provided:I-II^_wrk - Provides access to the working data store.// _x0P0 - Initializes x and P.//^FQ - Constructs F and Q.//^ities - Constructs z, H and R.#include <math.h>#include "calmdl.h"#include "navlib.h"#define NSYS 25 // Number of states.#define MMES 5 // Maximum number of measurements.static double xl[NSYS];static double zl[MMES];static double rl[MMES];static double F2[NSYS][NSYS];static double *Fl[NSYS];static double Q2[NSYSUNSYS];static double *Q1[N5Y5];static double PHI2[NSYS][NSYS];static double *PHIl[NSYS];static double Qd2[NSYS][NSYS];static double *Qd1[NSY5];static double H2[MMESUNSYS];static double *Hl[MMES];static double R2[MMES][MMES];static double *R1[MMES];static double S2[MMES][MMES];static double *51[4MES];static double P2[NSYS](NSYS];static double *Pl[NSYS];CALMDL.CPP^ 247static double K2[NSYS][MMES];static double *Kl[NSYS];#if NSYS>MMESstatic doublestatic doublestatic doublestatic doublestatic doublestatic double#elsestatic doublestatic doublestatic doublestatic doublestatic doublestatic double#endifvltmpl[NSYS];v2tmpl[NSYS];mltmp2[NSYS][NSYS];*mltmpl[NSYS];m2tmp2[NSYS][NSYS];*m2tmpl[NSYS];vltmpl[MMES];v2tmpl[MMES];mltmp2[MMES][MMES];*mltmpl[MMES];m2tmp2[MMES][MMES];*m2tmp1[MMES];// * * * * *If Provide access to the working data store.// * ****void calmdl_wrk(int &nsys, int &mmes,Kflt_matrix KPHI, Kflt_matrix &Qd,Kflt_vector &x, Kflt_vector &z, Kflt_vector &r,Kflt matrix &F, Kflt_matrix &Q,Kflt_matrix &H, Kflt_matrix &R, Kflt_matrix &S,Kflt_matrix &P, Kflt_matrix &K, Kflt_tmp &trap){ int i;nsys=NSYS;mmes=MMES;x=&xl[0];z=&z1[0];r=&r1[0];for (i=0; i<NSYS; i++)Fl[i]=&F2[i][0];F=&F1[0];for (i=0; i<NSYS; i++)Ql[i]=&Q2[i][0];Q=&Q1[0];for (i=0; i<NSYS; i++)PHI1[i]=&PHI2[i][0];PHI=&PHI1[0];for (i=0; i<NSYS; i++)Qdl[i]=&Qd2[i][0];Qd=&Qd1[0];for (i=0; i<MMES; i++)Hl[i]=&H2[i][0];H=&H1[0];for (i=0; i<MMES; i++)Rl[i]=&R2[i][0];R=&R1[0];CALMDL.CPP^ 248for (i=0; i<MMES; i++)S1[i]=&S2[i][0];S=&S1[0];for (i=0; i<NSYS; i++)P1[i]=&P2[i][0];P=&P1[0];for (i=0; i<NSYS; i++)K1[i]=&K2[i][0];K=&K1[0];tmp.vector1=&vltmpl[0];tmp.vector2=&v2tmpl[0];NSYS>MMESfor (i=0; i<NSYS; i++)mltmpl[i]=&mltmp2[i][0];tmp.matrix1=&mltmpl[0];for (i=0; i<NSYS; i++)m2tmpl[i]=&m2tmp2[i][0];tmp.matrix2=&m2tmpl[0];#elsefor (i=0; i<MMES; i++)mltmpl[i]=&mltmp2[i][0];tmp.matrix1=&mltmpl[0];for (i=0; i<MMES; i++)m2tmpl[i]=&m2tmp2[i][0];tmp.matrix2=&m2tmpl[0];#endifreturn;1// *****// Initialize the state estimate vector (x) and the error covariance// matrix (P).// *****//void calmdl_x0P0(void){ Kflt_vector x;Kflt_matrix P;int i, j;x=&xl[0];P=&P1[0];for (i=0; i<NSYS; i++) {x[i]=0.0;for (j=0; j<NSYS; j++)P[i][j]=0.0;}// Initial INS inertial navigation error state covariance.P[0][0]=1.0e6;^// x position; 1000 m rmsP[1][1]=1.0e6; // y position; 1000 m rmsP[2][2]=4.0;^// x velocity; 2 m/s rmsP[3][3]=4.0; // y velocity; 2 m/s rmsP[4][4]=0.09;^// x misalignment; 0.3 mrad rmsP[5][5]=0.09; // y misalignment; 0.3 mrad rmsP[6][6]=25.0;^// z misalignment; 5 mrad rms// Initial INS instrument error state covariance.P[7][7]=2.4e-7;^// x accelerometer bias; 50 micro_g (4.9e-4 m/s^2) rmsCALMDL.CPP^ 249P[8][8]=2.4e-7;^// y accelerometer bias; 50 micro_g (4.9e-4 m/s^2) rmsP[9][9)=2.4e-9;^// x gyro bias; 0.01 deg/hr (4.8e-5 mrad/s) rmsP[10][10)=2.4e-9; // y gyro bias; 0.0]. deg/hr (4.8e-5 mrad/s) rmsP[11][11]=2.4e-9; // z gyro bias; 0.01 deg/hr (4.8e-5 mrad/s) rms// Initial GPS position error state covariance.P[12][12)=400.0;^// x position; 20 m rmsP[].3][13]=400.0;^// y position; 20 m rms// Initial IMU inertial navigation error state covariance.P[14][14]=100.0;^// x position; 10 m rmsP[15][15)=100.0;^// y position; 10 m rmsP[16][16]=4.0e-4; // x velocity; 0.02 m/s rmsP[17][17]=4.0e-4; // y velocity; 0.02 m/s rmsP[18][18]=400.0;^// x misalignment; 20 mrad rmsP[19][19]=400.0;^// y misalignment; 20 mrad rmsP[20][20]=225.0;^// z misalignment; 15 mrad rms// Initial IMU instrument error state covariance.P[21][21]=2.1e-6; // x gyro bias; 0.3 deg/hr (1.5e-3 mrad/s) rmsP[22][22]=2.1e-6; // y gyro bias; 0.3 deg/hr (1.5e-3 mrad/s) rmsP[23][23]=2.1e-6; // z gyro bias; 0.3 deg/hr (1.5e-3 mrad/s) rms// Initial antenna azimuth error state covariance.P[24][24)=400.0; // antenna azimuth error; 20 mrad rmsreturn;1// *****// Construct the system dynamics matrix (F) and the process noise spectral// density matrix (Q).// *****'-void calmdl FQ(double insRxinv, double insRyinv,double insVw[3], double inswiew[3], double insweww[3],double insfw[3], double insCbw[3][3],double imuRxinv, double imuRyinv,double imuVw[3], double imuwiew[3], double imuweww[3],double imufw[3], double imuCbw[3][3],double gndspeed, double accel[3], double hdgrate){double Vgnd;double amag;double wzabs;double Rxinv;double Ryinv;double Vx;double Vy;double Vz;double wiex;double wiey;double wiez;double wewx;double wewy;double fx;double fy;double fz;Kflt_matrix F;Kflt matrix Q;int I, j;F=&F1[0];CALMDL.CPP^ 250Q=&Q1[0];for (i=0; i<NSYS; i++)for (j=0; j<NSYS; j++) {F[i][j]=0.0;Q[i][j]=0.0;1// Compute some quatities used in constructing the spectral density matrix.if (gndspeed>70.0)Vgnd=gndspeed;elseVgnd=70.0;amag=sqrt(pow(accel[0],2.0)+pow(accel[1],2.0)+pow(accel[2],2.0));wzabs=fabs(hdgrate);// INS inertial navigation error dynamics and random forcing function// spectral density.Rxinv=insRxinv;Ryinv=insRyinv;Vx=insVw[0];Vy=insVw[1];Vz=insVw[2];wiex=inswiew[0];wiey=inswiew[1];wiez=inswiew[2];wewx=insweww[0];wewy=insweww[1];fx=insfw[0];fy=insfw[1];fz=insfw[2];F[0][0]=Vz*Rxinv;F[0][2]=1.0;F[1][1]=Vz*Ryinv;F[1][3]=1.0;F[2][0]=(2.0*wiex+wewx)*Vy*Rxinv;F[2][1]=((2.0*wiey+wewy)*Vy+2.0*wiez*Vz)*Ryinv;F[2][2]=-Vz*Rxinv;F[2][3]=2.0*wiez;F[2][5]=(-fz)/1000.0;F[2][6]=(fy)/1000.0;F[3][0]=-((2.0*wiex+wewx)*Vx+2.0*wiez*Vz)*Rxinv;F[3][1]=-(2.0*wiey+wewy)*Vx*Ryinv;F[3][2]=-2.0*wiez;F[3][3]=-Vz*Ryinv;F[3][4]=(fz)/1000.0;F[3][6]=(-fx)/1000.0;F[4][0]=(-wiez*Rxinv)*1000.0;F[4][3]=(-Ryinv)*1000.0;F[4][5]=wiez;F[4][6]=-wiey-wewy;F[5][1]=(-wiez*Ryinv)*1000.0;F[5][2]=(Rxinv)*1000.0;F[5][4]=-wiez;F[5][6]=wiex+wewx;F[6][0]=((wiex+wewx)*Rxinv)*1000.0;F[6][1]=((wiey+wewy)*Ryinv)*1000.0;F[6][4]=wiey+wewy;F[6][5]=-wiex-wewx;Q[2][2]=6 .0e-3/Vgnd+3.2e-9*Vgnd*amag;4[3][3]=6 .0e-3/Vgnd+3.2e-9*Vgnd*amag;Q[4][4]=7. 6e-7;4[5][5]=7 .6e-7;Q[6][6]=7. 6e-7;CALMDL.CPP^ 251If INS instrument error dynamics and random forcing function spectral// density.^F[7][7]=-1.0/360000.0;^// x accelerometer bias correlation time = 100 hrF[8][8]=-1.0/360000.0;^// y accelerometer bias correlation time = 100 hrF[9][9]=-1.0/360000.0;^// x gyro bias correlation time = 100 hrF[10][10]=-1.0/360000.0; // y gyro bias correlation time = 100 hrF[11][11]=-1.0/360000.0; // z gyro bias correlation time = 100 hrQ[7][7]=2.0*2.4e-7/360000.0; // x accel biasQ[8][8]=2.0*2.4e-7/360000.0; // y accel biasQ[9][9]=2.0*2.4e-9/360000.0; // x gyro biasQ[10)[10]=2.0*2.4e-9/360000.0; // y gyro biasQ[11][11]=2.0*2.4e-9/360000.0; // z gyro biasvariance = 2.4e-7 (m/s^2)^2variance = 2.4e-7 (m/s^2)^2variance = 2.4e-9 (mrad/s)^2variance = 2.4e-9 (mrad/s)^2variance = 2.4e-9 (mrad/s)A2// INS inertial/instrument coupling.F[2][7]=insCbw[0][0];F[2][8]=insCbw[0][1];F[3][7]=insCbw[l][0];F[3][8]=insCbw[l][1];F[4][9]=-insCbw[0][0];F[4][10]=-insCbw[0][1];F[4][11]=-insCbw[0][2];F[5][9)=-insCbw[].][0];F[5][10]=-insCbw[1][1];F[5][11]=-insCbw[1][2];F[6][9]=-insCbw[2][0];F[6][10]=-insCbw[2][1];F[6][11]=-insCbw[2][2];If GPS position error dynamics and random forcing function spectral// density.F[12][12]=-1.0/10800.0; If x position error correlation time = 3 hrF[13][13]=-1.0/10800.0; // y position error correlation time = 3 hrQ[12][12]=2.0*400.0/10800.0; // x position error variance = 400 m^2Q[13][13]=2.0*400.0/10800.0; // y position error variance = 400 m^2// IMU inertial navigation error dynamics and random forcing function// spectral density.Rxinv=imuRxinv;Ryinv=imuRyinv;Vx=imuVw[0];Vy=imuVw[1];Vz=imuVw[2];wiex=imuwiew[0];wiey=imuwiew[1];wiez=imuwiew[2];wewx=imuweww[0];wewy=imuweww[1];fx=imufw[0];fy=imufw[1];fz=imufw[2];F[14][14]=Vz*Rxinv;F[14][16]=1.0;F[15][15]=Vz*Ryinv;F[15][17]=1.0;F[16][14]=(2.0*wiex+wewx)*Vy*Rxinv;F[16][15)=((2.0*wiey+wewy)*Vy+2.0*wiez*Vz)*Ryinv;F[16][16]=-Vz*Rxinv;F[16][17]=2.0*wiez;F[16][19]=(-fz)/1000.0;F[16][20]=(fy)/1000.0;F[17][14]=-((2.0*wiex+wewx)*Vx+2.0*wiez*Vz)*Rxinv;F[17][15)=-(2.0*wiey+wewy)*Vx*Ryinv;F[17][16)=-2.0*wiez;CALMDL.CPP^ 252F[17][17]=-Vz*Ryinv;F[17][18]=(fz)/1000.0;F[17][20]=(-fx)/1000.0;F[18][14]=(-wiez*Rxinv)*1000.0;F[18][17]=(-Ryinv)*1000.0;F[18][19]=wiez;F[18][20)=-wiey-wewy;F[19][15]=(-wiez*Ryinv)*1000.0;F[19][16]=(Rxinv)*1000.0;F[19)[18]=-wiez;F[19][20]=wiex+wewx;F[20][14]=((wiex+wewx)*Rxinv)*1000.0;F[20][15]=((wiey+wewy)*Ryinv)*1000.0;F[20][18]=wiey+wewy;F[20][19]=-wiex-wewx;Q[16][16]=(5.1e-8+3.2e-9)*Vgnd*amag;Q[17][17]=(5.1e-8+3.2e-9)*Vgnd*amag;Q[18][18]=1.2e-8*Vgnd*amag+(1.2e-1+1.2e-2)*wzabs+7.6e-7;Q[19][19]=1.2e-8*Vgnd*amag+(1.2e-1+1.2e-2)*wzabs+7.6e-7;Q[20][20]=1.2e-8*Vgnd*amag+7.1e-2*wzabs+7.6e-7;// IMU instrument error dynamics and random forcing function spectral// density.F[21][21]=-1.0/36000.0; // x gyro bias correlation time = 10 hrF[22][22]=-1.0/36000.0; // y gyro bias correlation time = 10 hrF[23][231=-1.0/36000.0; // z gyro bias correlation time = 10 hrQ[21][21]=2.0*2.1e-6/36000.0; // x gyro bias variance = 2.1e-6 (mrad/s)^2Q[22][22]=2.0*2.1e-6/36000.0; // y gyro bias variance = 2.1e-6 (mrad/s)A2Q[23][23]=2.0*2.1e-6/36000.0; // z gyro bias variance = 2.1e-6 (mrad/s)^2// IMU inertial/instrument coupling.F[18][21]=-imuCbw[0][0];F[18][22]=-imuCbw[0][1];F[18][23]=-imuCbw[0][2];F[19][21]=-imuCbw[l][0];F[19][22]=-imuCbw[1][1];F[19][23]=-imuCbw[1][2];F[20][21]=-imuCbw[2][0];F[20][22]=-imuCbw[2][1];F[20][23]=-imuCbw[2][2];return;1// * ****// Construct the measurement vector (z), the measurement matrix (H) and the// measurement noise covariance matrix (R).// *****//void calmdl_mes(double inslat, double inslong, double insalt,double insCwe[3][3], double insCbw[3][3],double imulat, double imulong, double imualt,double imuhdg, double imuwan,double imuCwe[3][3], double imuCbw[3][3],int gpsrdy, double gpslat, double gpslong, double gpsalt,int taurdy, double tauazm,const double RRP[3], const double RSD[3],const double RGPS[3],double tgtlat, double tgtlong, double tgtalt)double insRe[3];double imuRe[3];CALMDL.CPP^ 253double gpsRe[3];double RRPe[3];double RSDe[3];double Cbe[3][3];double rplat;double rplong;double rpalt;double Dlat;double Dlong;double slat;double cat;double slatl;double clatl;double sDlat;double sDlong;double cDlong;double swan;double cwan;double shdg;double chdg;double M;double N;double N1;double RN;double RN1;double RN;double RP;double D;double rg[2];double rg2;double brg;double vectmp[3];Kflt_vector z;Kflt matrix H;Kflt=matrix R;double sum;int i, j, k;z=&z1[0];H=&H1[0];R=&R1[0];for (i=0; i<MMES; i++)z[i]=0.0;for (i=0; i<MMES; i++)for (j=0; j<NSYS; j++)H[i][j]=0.0;for (i=0; i<MMES; i++) {for (j=0; j<MMES; j++)R[i][j]=0.0;R[i][i]=1.0;1geod2Re(inslat, inslong, insalt, insRe);geod2Re(imulat, imulong, imualt, imuRe);geod2Re(gpslat, gpslong, gpsalt, gpsRe);// INS/GPS position matching measurements.if (gpsrdy) {vectmp[0]=insRe[0]-gpsRe[0];vectmp[1]=insRe[1]-gpsRe[1];vectmp[2]=insRe[2]-gpsRe[2];CALMDL.CPP^ 254z[0]=insCwe[0][0]*vectmp[0]+insCwe[1][0]*vectmp[1]+insCwe[2][0]*vectmp[2]+insCbw[0][0]*RGPS[0]+insCbw[0][1]*RGPS[1]+insCbw[0][2]*RGPS[2];z[1]=insCwe[0][1]*vectmp[0]+insCwe[1][1]*vectmp[1]+insCwe[2][1]*vectmp[2]+insCbw[1][0]*RGPS[0]+insCbw[1][1]*RGPS[1]+insCbw[1][2]*RGPS[2];H[0][0]=1.0;H[0][12]=-1.0;H[1][1]=1.0;H[1][13)=-1.0;R[0][0]=25.0; // x INS/GPS position matching measurement noise; 5 m rmsR[1][1]=25.0; // y INS/GPS position matching measurement noise; 5 m rms}// IMU/INS position matching measurements.for (j=0; j<3; j++)for (i=0; i<3; i++) fsum=0.0;for (k=0; k<3; k++)sum=sum+insCwe[i][k]*insCbw[k][j];Cbe[i][j]=sum;}RRPe[0]=Cbe[0][0]*RRP[0]+Cbe[0][1]*RRP[1]+Cbe[0][2]*RRI,[2];RRPe[1]=Cbe[1][0]*RRP[0]+Cbe[1][1]*RRP[1]+Cbe[1][2]*RRP[2];RRPe[2]=Cbe[2][0]*RRP[0]+Cbe[2][1]*RRP[1]+Cbe[2][2]*RRP[2];for (j=0; j<3; j++)for (i=0; i<3; i++) {sum=0.0;for (k=0; k<3; k++)sum=sum+imuCwe[i][k]*imuCbw[k][j];Cbe[i][j]=sum;1RSDe[0]=Cbe[0][0]*RSD[0]+Cbe[0][1]*RSD[1]+Cbe[0][2]*RSD[2];RSDe[1]=Cbe[1][0]*RSD[0]+Cbe[1][1]*RSD[1]+Cbe[1][2]*RSD[2];RSDe[2]=Cbe[2][0]*RSD[0]+Cbe[2][1]*RSD[1]+Cbe[2][2]*RSD[2];vectmp[0]=imuRe[0]-(insRe[0]+RRPe[0]+RSDe[0]);vectmp[1]=imuRe[1]-(insRe[1]+RRPe[].]+RSDe[1]);vectmp[2]=imuRe[2]-(insRe[2]+RRPe[2]+RSDe[2]);z[2]=imuCwe[0][0]*vectmp[0]+imuCwe[1][0]*vectmp[1]+imuCwe[2][0]*vectmp[2];z[3]=imuCwe[0][1]*vectmp[0]+imuCwe[1][1]*vectmp[1]+imuCwe[2][1]*vectmp[2];H[2][14]=1.0;H[3][15]=1.0;R[2][2]=4.0; // x IMU/INS position matching measurement noise; 2 m rmsR[3][3]=4.0; // y IMU/INS position matching measurement noise; 2 m rms// Antenna azimuth error measurement.if (taurdy) feradii(imulat, M, N);RM=M+imualt;RP=(N+imualt)*cos(imulat);shdg=sin(imuhdg);chdg=cos(imuhdg);rplat=imulat-(RSD[0]*chdg-RSD[1]*shdg)/RM;rplong=imulong-(RSD[0]*shdg+RSD[1]*chdg)/RP;rpalt=imualt+RSD[2];Dlat=tgtlat-rplat;Dlong=tgtlong-rplong;slat=sin(rplat);clat=cos(rplat);slatl=sin(tgtlat);clatl=cos(tgtlat);sDlat=sin(Dlat);sDlong=sin(Dlong);CALMDL.CPP^ 255cDlong=cos(Dlong);swan=sin(imuwan);cwan=cos(imuwan);eradii(rplat, M, N);eradii(tgtlat, M, Ni);RN=N+rpalt;RN1=Nl+tgtalt;D=E_e2*(Nl*slatl-N*slat);rg[0]=RN1*clatl*sDlong;rg(1)=RN1*(sDlat+clatl*slat*(1.0-cDlong))-D*clat;rg2=rg[0]*rg[0]+rg[1]*rg[1);brg=atan2(rg[0],rg[1]);z[4]=tauazm-(brg-imuhdg);if (z[4]>=M PI)z(4)=z[4].-72.0*M PI;else if (z[4]<-MiiI)z[4]=z(4)+2.0*M PI;z[4]=z[4]*1000.0;-H[4][0]=(-(rg[0]*cwan+rg[1]*swan)/rg2-(slat*swan)/(RN*clat))*1000.0;H[4][14]=H[4][0];H[4][1]=((rg[Oreswan-rg[1]*cwan)/rg2-(slat*cwan)/(RN*clat))*1000.0;H[4][15]=H[4][1];H[4][6]=1.0;H[41](20]=13(4][6];H[4][24]=1.0;R[4][4]=4.0; // Antenna azimuth error measurement noise; 2 mrad rms}return;1KFLT.H^ 256// KFLT.H#ifndef KFLT_H#define KFLT H__ _typedef double *Kflt vector;typedef double *consE *Kflt_matrix;typedef struct {double *vectorl;double *vector2;double **matrixl;double **matrix2;} Kflt_tmp;// *****// Calculate the state transition matrix and the process noise covariance// matrix.// *****'-void PHIQd(int nsys, int nTPHI, int nTQd, double Dt,Kflt_matrix F, Kflt_matrix Q, Kflt_matrix PHI, Kflt_matrix Qd,Kflt_tmp tmp);// *****// Perform the Kalman filter state estimate and error covariance// extrapolation.// *****'-void extrap(int nsys, Kflt matrix PHI, Kflt matrix Qd,Kflt_vector x,—Kflt_matrix P, KYlt_tmp tmp);// *****// Perform the Kalman filter state estimate and error covariance update.// *****'-void updat(int nsys, int mmes, Kflt vector z, Kflt matrix H, Kflt _ matrix R,Kflt vector r, Kflt matrix S, Kflt matrix K,Kflt_vector^Kflt_matrix 15, Kflt_tmp^ tmpj;#endifPHIQD.CPP^ 257// PHIQD.CPPI-II Function to compute the state transistion matrix (PHI) and the process// noise covariance matrix (Qd) by Taylor series expansion. The system// dynamics matrix (F) and the process noise spectral density matrix (Q) are// assumed to be constant over the interval (Dt). nTPHI and nTQd are the// degrees of the truncated series expansions for PHI and Qd respectively.#include "kflt.h"// *****// Calculate the state transition matrix and the process noise covariance// matrix.// *****'-void PHIQd(int nsys, int nTPHI, int nTQd, double Dt,Kflt_matrix F, Kflt_matrix Q, Kflt_matrix PHI, Kflt_matrix Qd,Kflt_tmp tmp){ double sum;int n;double Dtn;int i, j, k;// Calculate the state transition matrix.for (j=0; j<nsys; j++) {for (i=0; i<nsys; i++) {tmp.matrixl[i][j]=F[i][j]*Dt;PHI[i][j]=tmp.matrixl[i][j];1PHI[j][j]=1.0+PHI[j][j];1for (n=2; n<=nTPHI; n++) {for (j=0; j<nsys; j++)for (i=0; i<nsys; i++) {sum=0.0;for (k=0; k<nsys; k++)sum=sum+F[i][k]*tmp.matrixl[k][j];tmp.matrix2[i][j]=sum;1Dtn=Dt/n;for (j=0; j<nsys; j++)for (i=0; i<nsys; i++) {tmp.matrixl[i][j]=tmp.matrix2[i][j]*Dtn;PHI[i][j]=PHI[i][j]+tmp.matrixl[i][j];}1// Calculate the process noise covariance matrix.for (j=0; j<nsys; j++)for (i=0; i<nsys; i++)tmp.matrixl[i][j]=Q[i][j]*Dt;Qd[i][j]=tmp.matrixl[i][j];}for (n=2; n<=nTQd; n++) {for (j=0; j<nsys; j++)for (i=0; i<nsys; i++) {sum=0.0;for (k=0; k<nsys; k++)sum=sum+F[i][k]*tmp.matrixl[k][j];tmp.matrix2[i][j]=sum;1Dtn=Dt/n;PHIQD.CPPfor (j=0; j(nsys; j++)for (i=0; i<=j; i++) {sum=(tmp.matrix2[i][j]+tmp.matrix2[j][i])*Dtn;tmp.matrixl[i][j]=sum;tmp.matrixl[j][i]=sum;sum=Qd[i][j]+sum;Qd[i][j]=sum;Qd[j][i]=sum;}}return;1258EXTRAP.CPP^ 259// EXTRAP.CPPI-II Function to extrapolate the Kalman filter state estimate vector (x) and// the error covariance matrix (P) given the state transition matrix (PHI)// and the process noise covariance matrix (Qd). The Kalman filter// extrapolation equations are directly implemented except that only the// upper triangular portion of P is computed. The symmetry property of P is// used to determine the lower triangular portion.#include "kflt.h"// *****// Perform the Kalman filter state estimate and error covariance// extrapolation.// *****'-void extrap(int nsys, Kflt matrix PHI, Kflt matrix Qd,Kflt_vector x,—Kflt_matrix P, Kilt_tmp tmp){ double sum;int i, j, k;// Extrapolate the state estimate.for (i=0; i<nsys; i++) {sum=0.0;for (k=0; k<nsys; k++)sum=sum+PHI[i][k]*x[k];tmp.vectorl[i]=sum;for (i=0; i<nsys; i++)x[i]=tmp.vectorl[i];// Extrapolate the error covariance.for (j=0; j<nsys; j++)for (i=0; i<nsys; i++) {sum=0.0;for (k=0; k<nsys; k++)sum=sum+PHI[i][k]*P[k][j];tmp.matrixl[i][j]=sum;1for (j=0; j<nsys; j++)for (i=0; i<=j; i++) {sum=0.0;for (k=0; k<nsys; k++)sum=sum+tmp.matrixl[i][k]*PHI[j][k];sum=sum+Qd[i][j];P[i][j]=sum;P[j][i]=sum;1return;1UPDAT.CPP^ 260// UPDAT.CPPI-II Function to update the Kalman filter state estimate vector (x) and// the error covariance matrix (P) given the measurement vector (z), the// measurement matrix (H) and the measurement noise covariance matrix (R).// The Kalman filter update equations are directly implemented except that// only the upper triangular portion of P is computed. The symmetry property// of P is used to determine the lower triangular portion. The measurement// residual vector (r), the residual covariance matrix (S) and the Kalman// gain matrix (K) are also output.#include <stdlib.h>#include <iostream.h>#include <math.h>#include "kflt.h"void inv(int n, Kflt_tmp tmp);// *****// Perform the Kalman filter state estimate and error covariance update.// *****'-void updat(int nsys, int mines, Kflt vector z, Kflt matrix H, Kflt_matrix R,Kflt_vector r, Kflt_matrix S, Kflt_matrix K,Kflt_vector x, Kflt_matrix P, Kflt_tmp tmp){double sum;int i, j, k;// Compute the Kalman gain matrix.for (j=0; j<nsys; j++)for (i=0; i<mmes; i++) {sum=0.0;for (k=0; k<nsys; k++)sum=sum+H[i](k)*P(k)(j);tmp.matrix2[i][j]=sum;1for (j=0; j<mmes; j++)for (i=0; i<mmes; i++) {sum=0.0;for (k=0; k<nsys; k++)sum=sum+tmp.matrix2(i][k]*H[jUk];S[i][j]=sum+R[i][j];tmp.matrixl(i)(j]=S[i][j];1inv(mmes, tmp);for (j=0; j<mmes; j++)for (i=0; i<nsys; i++) {sum=0.0;for (k=0; k<mmes; k++)sum=sum+H[k][i]*tmp.matrixl[k][j];tmp.matrix2(i)(j]=sum;1for (j=0; j<mmes; j++)for (i=0; i<nsys; i++) {sum=0.0;for (k=0; k<nsys; k++)sum=sum+P[i][k]*tmp.matrix2[k][j];K[i][j)=sum;1// Update the state estimate.UPDAT.CPP^ 261for (i=0; i<mmes; i++) {sum=0.0;for (k=0; k<nsys; k++)sum=sum+H[i][k]*x[k];r[i]=z[i]-sum;1for (i=0; i<nsys; i++) {sum=0.0;for (k=0; k<mmes; k++)sum=sum+K[i][k]*r[k];x[i)=x[i]+sum;}// Update the error covariance.for (j=0; j<nsys; j++) ffor (i=0; i<nsys; i++) •sum=0.0;for (k=0; k<mmes; k++)sum=sum-K[i][k]*H(k][j];tmp.matrixl(i)(j)=sum;tmp.matrix2(i][j]=P[i][j];1tmp.matrixl(j)(j]=1.0+tmp.matrixl(j][j];1for (j=0; j<nsys; j++)for (i=0; i<=j; i++) {sum=0.0;for (k=0; k<nsys; k++)sum=sum+tmp.matrixl[i][k]*tmp.matrix2[k][j];P[i][j]msumiP[j][i]=sum;}return;}// Local function to perform a matrix inversion using LU decomposition.// On entry, an mxm matrix is supplied in tmp.matrixl.// On exit, the matrix inverse is provided in tmp.matrixl.// The original matrix is destroyed as are the contents of tmp.vectorl,//^tmp.vector2 and tmp.matrix2.'-void inv(int m, Kflt_tmp tmp){Kflt_matrix A;Kflt_matrix LU;double *s;int *ix;double amax;double sum;double atmp;int imax;int ii;int i, j, k;A=tmp.matrix1;LU=tmp.matrix2;s=tmp.vectorl;ix=(int *) tmp.vector2;// Perform the LU decomposition.for (i=0; i<m; i++) {amax=0.0;for (j=0; j<m; j++) {262UPDAT.CPPif (fabs(A[i][j])>amax)amax=fabs(A[i][j));LU[i][j]=A[i][j];1if (amax==0.0) {cerr << "Singular matrix in function inv\n";exit(EXIT_FAILURE);}s[i]=1.0/amax;for (j=0; j<m; j++) {for (i=0; i<j; i++) {sum=LU[i][j];for (k=0; k<i; k++)sum=sum-LU[i][k]*LU[k][j];LU[i][j]=sum;1amax=0.0;for (i=j; i<m; i++) {sum=LU[i][j];for (k=0; k<j; k++)sum=sum-LU[i][k]*LU[k][j];LU[i][j]=sum;atmp=s[i]*fabs(sum);if (atmp>=amax) {imax=i;amax=atmp;11if (j!=imax) {for (k=0; k<m; k++) fatmp=LU[imax][k];LU[imax][k]=LU[j][k];LU[j][k]=atmp;1s[imax)=s[j];1ix[j]=imax;if (LU[j][j]==0.0) {cerr << "Singular matrix in function inv\n";exit(EXIT_FAILURE);}if (j!=m-1) {atmp=1.0/LU[j][j];for (i=j+1; i<m; i++)LU[i][j]=LU[i][j]*atmp;11// Perform the matrix inversion column by column.for (j=0; j<m; j++) {for (i=0; i<m; i++)A[i][j]=0.0;A[j][j]=1.0;1for (j=0; j<m; j++) {ii=-1;for (i=0; i<m; i++) {imax=ix[i];sum=A[imax][j];A[imax][j]=A[i][j];if (ii!=-1)for (k=ii; k<i; k++)sum=sum-LU[i][k]*A[k][j];else if (suml=0.0)UPDAT.CPPii=i;A[i][j]=sum;}for (i=m-1; i>=0; i--) {sum=A[ii(j);if (il=m-1)for (k=i+1; k<m; k++)sum=sum-LU[i][kj*A[k][j];A[i][j]=sum/LU[i] (ii;}}return;}263264NAVGTR.H// NAVGTR.Hstruct navgtr_record {double time;double latitude;double longitude;double altitude;double gndspeed;double track;double heading;double azimuth;double pitch;double roll;double vertspeed;struct navwork_record {double vertaccelbias;double vertaccelfb;double vertvelfb;double Rxinv;double Ryinv;double Tiny;double gravity;double wander;double Dthiwb[3];double Dthwbb[3];double Dtheww[3];double Vw[3];double wiew[3];double weww[3];double fw[3];double qbw[4];double qwe[4];double Cbw[3][3];double Cwe[3][3];// *****// Perform inertial navigation calculations.// *****//void navgtr(long cycle count, double cur time, double Dt,double DVeib[3], double DthiEb[3], double altref,navgtr_record &navrec, navwork_record &wrkrec);NAVGTR.CPP^ 265// NAVGTR.CPP// Perform inertial navigation calculations.#include <math.h>#include "navlib.h"#include "navgtr.h"static void vertfb(double Dt, double altitude, double altref,double &vertaccelbias,double &vertaccelfb, double &vertvelfb);// *****// Perform inertial navigation calculations.// *****//void navgtr(long cycle count, double cur time, double Dt,double DveIb[3], double DthiEb[3], double altref,navgtr_record &n, navwork_record &w){double roll;double pitch;double azimuth;double heading;double latitude;double longitude;double wander;double altitude;double gndspeed;double vertspeed;double track;double Dvelw[3];double DVw[3];double Dthiwb[3];double Dthwbb[3];double Dtheww[3];double Vw[3];double wiew[3];double weww[3];double qbw[4];double qwe[4];double Cbw[3][3];double Cwe[3][3];double vectmp[3];int i, j;if (cycle_count==0L) {Ea2Cbw(n.roll, n.pitch, n.azimuth, w.Cbw);dcm2q(w.Cbw, w.qbw);Dvelw[0]=w.Cbw[0][0]*Dvelb[0]+w.Cbw[0][1]*Dvelb[1]+w.Cbw[0][2]*Dvelb[2];Dvelw[1]=w.Cbw[l][0]*Dvelb[0]+w.Cbw[l][1]*Dvelb[1]+w.Cbw[1][2]*Dvelb[2];Dvelw[2]=w.Cbw[2][0]*Dvelb[0]+w.Cbw[2][1]*Dvelb[1]+w.Cbw[2][2]*Dvelb[2];w.fw[0]=Dvelw[0]/Dt;w.fw[1]=Dvelw[1]/Dt;w.fw[2]=Dvelw[2]/Dt;w.wander=n.azimuth-n.heading;Ea2Cwe(n.latitude, n.longitude, w.wander, w.Cwe);dcm2q(w.Cwe, w.qwe);w.Vw[0]=n.gndspeed*cos(n.track+w.wander);w.Vw[1]=-n.gndspeed*sin(n.track+w.wander);266NAVGTR.CPPw.Vw[2)=n.vertspeed;invradiil(w.Cwe, n.altitude, w.Rxinv, w.Ryinv, w.Tinv);w.weww[0)=-w.Vw[1]*w.Ryinv+w.Vw[0]*w.Tinv;w.weww[1]=w.Vw[0]*w.Rxinv-w.Vw[1]*w.Tinv;w.weww[2]=0.0;w.Dtheww[0]=w.weww[0)*Dt;w.Dtheww[1]=w.weww[1]*Dt;w.Dtheww[2]=w.weww[2)*Dt;w.wiew[0]=w.Cwe[0)[0]*E_wie;w.wiew[1]=w.Cwe[0][1]*E_wie;w.wiew[2]=w.Cwe[0][2]*E_wie;vectmp[0]=(w.wiew[0]+w.weww[0])*Dt;vectmp[1)=(w.wiew[1]+w.weww[1])*Dt;vectmp[2]=(w.wiew[2]+w.weww[2])*Dt;w.Dthiwb[0]=w.Cbw[0][0]*vectmp[0]+w.Cbw[1][0]*vectmp[1)+w.Cbw[2][0]*vectmp[2];w.Dthiwb[1]=w.Cbw[0][1]*vectmp[0]+w.Cbw[l][1]*vectmp[1]+w.Cbw[2][1]*vectmp[2];w.Dthiwb[2]=w.Cbw[0][2]*vectmp[0]+w.Cbw[l][2]*vectmp[1]+w.Cbw[2][2]*vectmp[2];w.Dthwbb[0]=Dthibb[0]-w.Dthiwb[0];w.Dthwbb[1]=Dthibb[1)-w.Dthiwb[1];w.Dthwbb[2)=Dthibb[2]-w.Dthiwb[2];w.gravity=gravityl(w.Cwe, n.altitude);w.vertaccelbias=0.0;w.vertaccelfb=0.0;w.vertvelfb=0.0;1else {Dthwbb[0]=Dthibb[0)-w.Dthiwb[0];Dthwbb[1]=Dthibb[1]-w.Dthiwb[1];Dthwbb[2]=Dthibb[2]-w.Dthiwb[2);qbw[0]=w.gbw[0];qbw[1]=w.gbw[1];qbw[2]=w.gbw[2];qbw[3)=w0gbw[3];qupdt(Dthwbb, w.Dthwbb, qbw);q2dcm(gbw, Cbw);Cbw2Ea(Cbw, roll, pitch, azimuth);Dvelw[0]=Cbw[0][0]*Dvelb[0]+Cbw[0][1]*Dvelb[1]+Cbw[0][2]*Dvelb[2];Dvelw[1]=Cbw[1)(0)*Dvelb[0]+Cbw[l][1]*Dvelb[].)+Cbw[l][2]*Dvelb[2];Dvelw[2]=Cbw[2][0]*Dvelb[0]+Cbw[2][1]*Dvelb[1]+Cbw[2][2)*Dvelb[2];vectmp[0)=2.0*w.wiew[0]+w.weww[0];vectmp[1]=2.0*w.wiew[1)+w.weww[1];vectmp[2]=2.0*w.wiew[2]+w.weww[2];DVw[0]=Dvelw[0]-(vectmp[1]*w.Vw[21-vectmp[2]*w.Vw[1])*Dt;DVw[1)=Dvelw[1]-(vectmp[2]*w.Vw[0]-vectmp[0]*w.Vw[2])*Dt;DVw[2]=Dvelw[2]-(vectmp[0]*w.Vw[1]-vectmp[1]*w.Vw[0])*Dt-(w.gravity+w.vertaccelbias+w.vertaccelfb)*Dt;Vw[0]=w.Vw[0)+DVw[0);Vw[1]=w.Vw[1]+DVw[1];Vw[21=w.Vw[2)+DVw[2);weww[0)=-Vw[1]*w.Ryinv+Vw[0]*w.Tinv;NAVGTR.CPP^ 267weww[1]=Vw[0]*w.Rxinv-Vw[].]*w.Tinv;weww[2]=0.0;Dtheww[0]=0.5*(weww[0]+w.weww[0])*Dt;Dtheww[1]=0.5*(weww[1]+w.weww[1])*Dt;Dtheww[2]=0.5*(weww[2]+w.weww[2])*Dt;qwe[0]=w.qwe[0];qwe[1]=w.qwe[1];qwe[2]=w.qwe[2];qwe[3]=w.qwe[3];qupdt(Dtheww, w.Dtheww, qwe);q2dcm(qwe, Cwe);Cwe2Ea(Cwe, latitude, longitude, wander);altitude=n.altitude+(0.5*(Vw[2]+w.Vw[2])-w.vertvelfb)*Dt;wiew[0]=Cwe[0][0]*E_wie;wiew[1]=Cwe[0][1]*E_wie;wiew[2]=Cwe[0][2]*E_wie;vectmp[0]=(wiew[0]+weww[0])*Dt;vectmp[1]=(wiew[1]+weww[1])*Dt;vectmp[2]=(wiew[2]+weww[2])*Dt;Dthiwb[0]=Cbw[0][0]*vectmp[0)+Cbw[l][0]*vectmp[1]+Cbw[2][0]*vectmp[2];Dthiwb[1]=Cbw[0][1]*vectmp[0]+Cbw[l][1]*vectmp[1]+Cbw[2][1]*vectmp[2];Dthiwb[2]=Cbw[0][2]*vectmp[0]+Cbw[l][2]*vectmp[1]+Cbw[2][2]*vectmp[2];gndspeed=sqrt(Vw[0]*Vw[0]+Vw[].]*Vw[1]);vertspeed=Vw[2];track=atan2(-Vw[1],Vw[0])-wander;if (track>=M_PI)track=track-2.0*M_PI;else if (track<-M_PI)track=track+2.0*M PI;heading=azimuth-wander;if (heading>=M_PI)heading=heading-2.0*M_PI;else if (heading<-M_PI)heading=heading+2.0*M_PI;n.time=cur_time;n.latitude=latitude;n.longitude=longitude;n.altitude=altitude;n.gndspeed=gndspeed;n.track=track;n.heading=heading;n.azimuth=azimuth;n.pitch=pitch;n.roll=roll;n.vertspeed=vertspeed;vertfb(Dt, altitude, altref,w.vertaccelbias, w.vertaccelfb, w.vertvelfb);invradiil(Cwe, altitude, w.Rxinv, w.Ryinv, w.Tinv);w.gravity=gravityl(Cwe, altitude);w.wander=wander;for (i=0; i<=2; i++) {w.Dthiwb[i]=Dthiwb[i];w.Dthwbb[i]=Dthwbb[i];w.Dtheww[i]=Dtheww[i];w.Vw[i]=Vw[i];w.wiew[i]=wiew[i];w.weww[i]=weww[i];268NAVGTR.CPPw.fut(i)=Dvelw(i)/Dt;1for (i=0; i<=3; i++) {w.qbw[i]=gbw(i);w.alwe(i)=qwe(i);1for (1=0; i<=2; i++)for (j=0; j<=2; j++) {w.Cbur[i](j)=Cbw(i)[j];w.Cwe(i)(j)=Cwe[i][j];1Ireturn;}// Local function to compute vertical channel error control feedback.//static void vertfb(double Dt, double altitude, double altref,double &vertaccelbias,double &vertaccelfb, double &vertvelfb){static const double k1=3.0e-2;static const double k2=3.0e-4;static const double k3=1.0e-6;double altdiff;altdiff=altitude-altref;vertvelfb=kl*altdiff;vertaccelfb=k2*altdiff;vertaccelbias=vertaccelbias+k3*altdiff*Dt;return;1NAVLIB.H^ 269// NAVLIB.H// Definitions for the navigation library.I-II The functions defined in this library use the following coordinate// systems:I-II Earth-fixed coordinates:// x-axis through North pole// y-axis for right-hand system// z-axis through Greenwich meridianI-II Geographic coordinates:// x-axis north// y-axis east// z-axis down//// Wander azimuth coordinates:// x-axis displaced counterclockwise from north through wander angle// y-axis for right-hand system//^z-axis upI-II Body coordinates:// x-axis forward// y-axis right//^z-axis down#ifndef NAVLIB H#define NAVLIB—H_// *****// Compute the radius of curvature in the plane of the meridian (M) and the// radius of curvature in the plane of the prime vertical (N) given the// latitude.// *****'-void eradii(double latitude, double &M, double &N);// *****// Compute the radius of curvature in the plane of the meridian (M) and the// radius of curvature in the plane of the prime vertical (N) given the// wander azimuth to Earth DCM.// *****'-void eradiil(const double Cwe[3][3], double &M, double &N);// *****// Compute the inverse radii of curvature given the latitude, altitude and// wander angle.// *****'-void invradii(double latitude, double altitude, double wander,double &Rxinv, double &Ryinv, double &Tinv);// *****// Compute the inverse radii of curvature given the wander azimuth to Earth// DCM and the altitude.// *****'-void invradiil(const double Cwe[3][3], double altitude,double &Rxinv, double &Ryinv, double &Tinv);NAVLIB.H^ 270// *****// Compute the normal gravity given the latitude and altitude.// *****'-double gravity(double latitude, double altitude);// *****// Compute the normal gravity given the wander azimuth to Earth DCM and the// altitude.// *****'-double gravityl(const double Cwe[3][3], double altitude);// *****// Compute latitude, longitude and altitude from Earth-fixed coordinates.// *****'-void Re2geod(const double Re[3],double &latitude, double &longitude, double &altitude);// *****// Compute Earth-fixed coordinates from latitude, longitude and altitude.// *****'-void geod2Re(double latitude, double longitude, double altitude,double Re[3]);// *****// Compute the body to wander azimuth DCM from roll, pitch and platform// azimuth (Euler angles).// *****'-void Ea2Cbw(double roll, double pitch, double azimuth,double Cbw[3][3]);// *****// Compute the wander azimuth to Earth DCM from latitude, longitude and// wander angle (Euler angles).// *****'-void Ea2Cwe(double latitude, double longitude, double wander,double Cwe[3][3]);// *****// Compute the Earth to geographic (NED) DCM from latitude and longitude.// *****'-void Ea2Ceg(double latitude, double longitude, double Ceg[3][3]);// *****// Compute the geographic (NED) to body DCM from roll, pitch and heading// (Euler angles).// *****'-void Ea2Cgb(double roll, double pitch, double heading, double Cgb[3][3]);271NAVLIB.H// *****// Compute roll, pitch and platform azimuth (Euler angles) from the// body to wander azimuth DCM.// *****'-void Cbw2Ea(const double Cbw[3][3],double &roll, double &pitch, double &azimuth);// *****// Compute latitude, longitude and wander angle (Euler angles) from the// wander azimuth to Earth DCM.// *****'-void Cwe2Ea(const double Cwe[3][3],double &latitude, double &longitude, double &wander);// *****// Obtain a quaternion from a direction cosine matrix.// *****'-void dcm2q(const double C[3][3], double q[4]);// *****// Update a quaternion using third order quaternion integration.// *****'-void qupdt(const double Dang1[3], const double Dangl_L[3], double q[4]);// *****// Obtain a direction cosine matrix from a quaternion.// *****//void q2dcm(const double q[4], double C[3][3]);// * ****// Earth constants (WGS 84).//^a^semi-major axis [m]//^f^flattening// e2^first eccentricity squared//^wie angular velocity (rad/s]//^GM gravitational constant [m^3/sA2]//^ge normal gravity at equator [m/s^2]// k^constant used in gravity formula// *//#define E a^6.378137e6#define E—f^3.35281066474e-3#define E:e2 6.69437999013e-3#define E _wie 7.292115e-5#define E GM 3.986005e14#define E:ge 9.7803267714#define E k^1.93185138639e-3_#endifERADII.CPP^ 272// ERADII.CPP// Functions to compute the radius of curvature in the plane of the meridian// and the radius of curvature in the plane of the prime vertical.#include <math.h>#include "navlib.h"// *****// Compute the radius of curvature in the plane of the meridian (M) and the// radius of curvature in the plane of the prime vertical (N) given the// latitude.// *****//void eradii(double latitude, double &M, double &N){ double slat2;slat2=pow(sin(latitude), 2.0);M=E_a*(1.0-E_e2)/pow(1.0-E_e2*slat2, 1.5);N=E_a/sgrt(1.0-E_e2*slat2);return;}// *****// Compute the radius of curvature in the plane of the meridian (M) and the// radius of curvature in the plane of the prime vertical (N) given the// wander azimuth to Earth DCM.// *****'-void eradiil(const double Cwe[3][3], double &M, double &N){double slat2;slat2=Cwe[0][2]*Cwe[0][2];M=E a*(1.0-E e2)/pow(1.0-E e2*slat2, 1.5);N=E—a/sgrt(170-E_e2*slat2)Treturn;}INVRADII.CPP^ 273// INVRADII.CPP// Functions to compute the inverse radii of curvature.#include <math.h>#include "navlib.h"// *****// Compute the inverse radii of curvature given the latitude, altitude and// wander angle.// *****'-void invradii(double latitude, double altitude, double wander,double &Rxinv, double &Ryinv, double &Tinv){double swan;double cwan;double swan2;double cwan2;double M;double N;double R1;double R2;double R1R2;swan=sin(wander);cwan=cos(wander);swan2=swan*swan;cwan2=cwan*cwan;eradii(latitude, M, N);R1=M+altitude;R2=N+altitude;R1R2=R1*R2;Rxinv=(R1*swan2+R2*cwan2)/R1R2;Ryinv=(R1*cwan2+R2*swan2)/R1R2;Tinv=swan*cwan*(R2-R1)/R1R2;return;}// *****// Compute the inverse radii of curvature given the wander azimuth to Earth// DCM and the altitude.// *****'-void invradiil(const double Cwe(3)(3), double altitude,double &Rxinv, double &Ryinv, double &Tinv){ double u;double v;double u2;double v2;double w2;double swan2;double cwan2;double swancwan;double M;double N;double R1;double R2;double R1R2;u=Cwe[0][0];v=-Cwe[0](1);1u2=u*u;v2=v*v;w2=u2+v2;swan2=v2/w2;cwan2=u2/w2;swancwan=u*v/w2;eradiil(Cwe, M, N);R1=M+altitude;R2=N+altitude;R1R2=R1*R2;Rxinv=(R1*swan2+R2*cwan2)/R1R2;Ryinv=(R1*cwan2+R2*swan2)/R1R2;Tinv=swancwan*(R2-R1)/R1R2;return;INVRADII.CPP 274GRAVITY.CPP^ 275If GRAVITY.CPP// Functions to compute normal gravity.#include <math.h>#include "navlib.h"static const double m=E_wie*E_wie*E_a*E_a*E_a*(1.0-E_f)/E_GM;static const double cl=(2.0/E a)*(1.0+E f+m);_static const double c2=4.0*E Y/E a;static const double c3=3.0/(f_a*f_a);// *****// Compute the normal gravity given the latitude and altitude.// *****'-double gravity(double latitude, double altitude){ double slat2;double g;slat2=pow(sin(latitude), 2.0);g=E_ge*(1.0+Ek*slat2)/sgrt(1.0-E_e2*slat2);g=g*(1.0-(cl-c2*slat2)*altitude+c3*altitude*altitude);return g;}// * ****// Compute the normal gravity given the wander azimuth to Earth DCM and the// altitude.// *****'-double gravityl(const double Cwe[3][3], double altitude){double slat2;double g;slat2=Cwe[0][2]*Cwe[0][2];g=E_ge*(1.0+E_k*slat2)/sqrt(1.0-E_e2*slat2);g=g*(1.0-(cl-c2*slat2)*altitude+c3*altitude*altitude);return g;1RE2GEOD.CPP// RE2GEOD.CPP// Function to compute geodetic coordinates (latitude, longitude and// altitude) given the Earth-fixed coordinates.//// Earth-fixed coordinates://// x-axis through North pole// y-axis for right-hand system// z-axis through Greenwich meridian#include <math.h>#include "navlib.h"static const double c=1.0-E_e2;// *****// Compute latitude, longitude and altitude from Earth-fixed coordinates.// *****'-void Re2geod(const double Re[3],double &latitude, double &longitude, double &altitude){double x, y, z;double w;double N;double slat;x=Re[0];y=Re[1];z=Re[2];longitude=atan2(-y,z);w=sqrt(y*y+z*z);latitude=atan2(x,c*w);if (fabs(latitude)<M_PI_4)for (int i=1; i<=6; i++) {slat=sin(latitude);N=E_a/sqrt(1.0-E_e2*slat*slat);altitude=w/cos(latitude)-N;latitude=atan2(x*(N+altitude),w*(c*N+altitude));1elsefor (int i=1; i<=6; i++) {slat=sin(latitude);N=E a/sqrt(1.0-E e2*slat*slat);altitude=x/slat-C-*N;latitude=atan2(x*(N+altitude),w*(c*N+altitude));Ireturn;}276GEOD2RE.CPP^ 277// GEOD2RE.CPP// Function to compute Earth-fixed coordinates given the geodetic coordinates// (latitude, longitude and altitude).I-II Earth-fixed coordinates:I-II x-axis through North pole// y-axis for right-hand system// z-axis through Greenwich meridian#include <math.h>#include "navlib.h"// *****// Compute Earth-fixed coordinates from latitude, longitude and altitude.// *****'-void geod2Re(double latitude, double longitude, double altitude,double Re[3]){ double slat;double cat;double N;slat=sin(latitude);clat=cos(latitude);N=E_a/sqrt(1.0-E_e2*slat*slat);Re[0]=(N*(1.0-E_e2)+altitude)*slat;Re[1]=-(N+altitude)*clat*sin(longitude);Re[2]=(N+altitude)*clat*cos(longitude);return;}EA2CBW.CPP^ 278// EA2CBW.CPP// Function to compute the DCM which transforms a vector in body coordinates// to wander azimuth coordinates.I-II Body coordinates:I-II x-axis forward// y-axis right//^z-axis downI-II Wander azimuth coordinates://// x-axis displaced counterclockwise from north through wander angle// y-axis for right-hand system//^z-axis up#include <math.h>// *****// Compute the body to wander azimuth DCM from roll, pitch and platform// azimuth (Euler angles).// *****'-void Ea2Cbw(double roll, double pitch, double azimuth,double Cbw[3][3]){double sroll;double croll;double sptch;double cptch;double saz;double caz;sroll=sin(roll);croll=cos(roll);sptch=sin(pitch);cptch=cos(pitch);saz=sin(azimuth);caz=cos(azimuth);Cbw[0][0]=cptch*caz;Cbw[0][1]=-croll*saz+sroll *sptch*caz;Cbw[0][2]=sroll*saz+croll* sptch*caz;Cbw[1][0]=-cptch*saz;Cbw[l][1]=-croll*caz-sroll *sptch*saz;Cbw[l][2]=sroll*caz-croll* sptch*saz;Cbw[2][0]=sptch;Cbw[2][1]=-sroll*cptch;Cbw[2][2]=-croll*cptch;return;}EA2CWE.CPP^ 279// EA2CWE.CPP// Function to compute the DCM which transforms a vector in wander azimuth// coordinates to Earth-fixed coordinates.I-II Wander azimuth coordinates:I-II x-axis displaced counterclockwise from north through wander angle// y-axis for right-hand system//^z-axis upI-II Earth-fixed coordinates:I-II x-axis through North pole// y-axis for right-hand system// z-axis through Greenwich meridian#include <math.h>// *****// Compute the wander azimuth to Earth DCM from latitude, longitude and// wander angle (Euler angles).// *****'-void Ea2Cwe(double latitude, double longitude, double wander,double Cwe[3][3]){double slat;double cat;double slong;double clong;double swan;double cwan;slat=sin(latitude);clat=cos(latitude);slong=sin(longitude);clong=cos(longitude);swan=sin(wander);cwan=cos(wander);Cwe[0][0]=clat*cwan;Cwe[0][1]=-clat*swan;Cwe[0][2]=slat;Cwe[1][0]=clong*swan+slat*slong*cwan;Cwe[1][1]=clong*cwan-slat*slong*swan;Cwe[1][2]=-clat*slong;Cwe[2][0]=slong*swan-slat*clong*cwan;Cwe[2][1]=slong*cwan+slat*clong*swan;Cwe[2][2]=clat*clong;return;}EA2CEG.CPP^ 280II EA2CEG.CPP// Function to compute the DCM which transforms a vector in Earth-fixed// coordinates to geographic coordinates.I-II Earth-fixed coordinates://// x-axis through North pole// y-axis for right-hand system// z-axis through Greenwich meridianI-II Geographic coordinates:I-II x-axis north// y-axis east//^z-axis down#include <math.h>// *****// Compute the Earth to geographic (NED) DCM from latitude and longitude.// *****//void Ea2Ceg(double latitude, double longitude, double Ceg[3][3]){ double slat;double cat;double slong;double clong;slat=sin(latitude);clat=cos(latitude);slong=sin(longitude);clong=cos(longitude);Ceg[0][0]=clat;Ceg[0][1]=slat*slong;Ceg[0][2]=-slat*clong;Ceg[1][0]=0.0;Ceg[1][1]=-clong;Ceg[1][2]=-slong;Ceg[2][0]=-slat;Ceg[2][1]=clat*slong;Ceg[2][2]=-clat*clong;return;}EA2CGB.CPP^ 281// EA2CGB.CPP// Function to compute the DCM which transforms a vector in geographic (NED)// coordinates to body coordinates.I-II Geographic coordinates:I-II x-axis north// y-axis east// z-axis downI-II Body coordinates://// x-axis forward// y-axis right// z-axis down#include <math.h>// * ****// Compute the geographic (NED) to body DCM from roll, pitch and heading.// *****'-void Ea2Cgb(double roll, double pitch, double heading, double Cgb[3][3]){double sroll;double croll;double sptch;double cptch;double shdg;double chdg;sroll=sin(roll);croll=cos(roll);sptch=sin(pitch);cptch=cos(pitch);shdg=sin(heading);chdg=cos(heading);Cgb[0][0]=cptch*chdg;Cgb[0][1]=cptch*shdg;Cgb[0][2]=-sptch;Cgb[1][0]=sroll*sptch*chdg-croll*shdg;Cgb[1][1]=sroll*sptch*shdg+croll*chdg;Cgb[1][2]=sroll*cptch;Cgb[2][0]=croll*sptch*chdg+sroll*shdg;Cgb[2][1]=croll*sptch*shdg-sroll*chdg;Cgb[2][2]=croll*cptch;return;}CBW2EA.CPP^ 282// CBW2EA.CPP// Function to compute the Euler angles (roll, pitch and platform azimuth)// from the body to wander azimuth DCM.I-II Body coordinates:I-II x-axis forward// y-axis right//^z-axis downI-II Wander azimuth coordinates:I-II x-axis displaced counterclockwise from north through wander angle// y-axis for right-hand system//^z-axis up#include <math.h>// *****// Compute roll, pitch and platform azimuth (Euler angles) from the// body to wander azimuth DCM.// *****'-void Cbw2Ea(const double Cbw[3][3],double &roll, double &pitch, double &azimuth)roll=atan2(-Cbw[2][1],-Cbw[2][2]);pitch=atan2(Cbw[2][0],sqrt(pow(Cbw(0][0],2.0)+pow(Cbw[l][0],2.0)));azimuth=atan2(-Cbw[1][0],Cbw[0][0]);return;}CWE2EA.CPP// CWE2EA.CPP// Function to compute the Euler angles (latitude, longitude and wander// angle) from the wander azimuth to Earth DCM.I-II Wander azimuth coordinates://// x-axis displaced counterclockwise from north through wander angle// y-axis for right-hand system//^z-axis up//// Earth-fixed coordinates://// x-axis through North pole// y-axis for right-hand system// z-axis through Greenwich meridian/include <math.h>// *****// Compute latitude, longitude and wander angle (Euler angles) from the// wander azimuth to Earth DCM.// *****'-void Cwe2Ea(const double Cwe[3][3],double &latitude, double &longitude, double &wander){latitude=atan2(Cwe[0][2],sqrt(pow(Cwe[0][0],2.0)+pow(Cwe[0][1],2.0)));longitude=atan2(-Cwe[1][2],Cwe[2][2]);wander=atan2(-Cwe[0][1],Cwe[0][0]);return;1283284DCM2Q.CPP// DCM2Q.CPP// Function to compute a quaternion from a direction cosine matrix.#include <math.h>// *****// Obtain a quaternion from a direction cosine matrix.// *****'-void dcm2q(const double C[3][3], double q(41){q[0]=0.5*sqrt(1.0+C(0)(0)+C[1][1]+C[2][2]);if (q[0]>=0.0002) {q(1)=0.25*(CpUlj-C(1)(2))/q(0);q(2)=0.25*(C(0)(2]-C(2)[0])/q(0);q[3]=0.25*(C(1)(0)-C(0)[1])/q[0];1else {if ((q0)(0]>C(1)[1]) && (C[0][0]>C[2][2])) {if (C[2][1]<C(1)(2))q[0]=-q[0];q[1]=sqrt(0.5*(1.0+C(0)[0]));q[2]=0.25*(C(0)[1]+C(1)(0))/q(1);q(3)=0.25*(C[0][2]-1-C[2][0])/q[1];1else if ((C(11[1]>C[0](0]) && (C[1][1]>C[2][2])) {if (C[0][2]<C[2][0])q(0)=-q(0);q[2]=sqrt(0.5*(1.0+C(1](1)));q[1]=0.25*(C[0] [11+C[1][0])/q[2];q[3]=0.25*(C[1][2]+C[2](1))/q[2];1else {if (C(1)(0j<C[0][1])q(0)=-q[0];q[3]=sqrt(0.5*(1.0+C(2)[2]));q(1)=0.25*(C[0][2]+C[2][0])/q[3];q(2)=0.25*(C[1][2]+C[2][1])/q[3];1Ireturn;1QUPDT.CPP^ 285// QUPDT.CPP// Function to update a quaternion using third order quaternion integration.#include <math.h>// *****// Update a quaternion using third order quaternion integration.// *****'-void qupdt(const double Dang1[3], const double Dangl_L[3], double q[4]){double L;double chi[4];double qq[4];L=pow(Dang1[0],2.0)+pow(Dang1[1],2.0)+pow(Dangl[2],2.0);chi[0]=1.0-0.125*L;chi[1]=(0.5-L/48.0)*Dang1[0]+(Dang1[2]*Dangl_L[1]-Dangl[1]*Dangl_L[2])/24.0;chi[2]=(0.5-L/48.0)*Dang1[1]+(Dang1[0]*Dangl_L[2]-Dangl[2]*Dangl_L[0])/24.0;chi[3]=(0.5-L/48.0)*Dang1[2]+(Dang1[1]*Dangl_L[0]-Dangl[0]*Dangl_L[1])/24.0;qq[0]=q[0]*chi[0]-q[1]*chi[1]-q[2]*chi[2]-q[3]*chi[3];qq[1]=q[0]*chi[1]+q[1]*chi[0]+q[2]*chi[3]-q[3]*chi[2];qq[2]=q[0]*chi[2]-q[1]*chi[3]+q[2]*chi[0]+q[3]*chi[1];qq[3]=q[0]*chi[3]+q[1]*chi[2]-q[2]*chi[1]+q[3]*chi[0];L=sqrt(pow(qq[0],2.0)+pow(qq[1],2.0)+pow(qq[2],2.0)+pow(qq[3],2.0));q[0]=qq[0]/L;q[1]=qq[1]/L;q[2]=qq[2]/L;q[3]=qq[3]/L;return;1Q2DCM.CPP// Q2DCM.CPP// Function to compute a direction cosine matrix from a quaternion.#include <math.h>// *****// Obtain a direction cosine matrix from a quaternion.// *****'-void q2dcm(const double q(4), double C(3)(3)){ C[0][0]=1.0-2.0*(q(2]*q[2]+q[3]*q(3]);C(0)(1)=2.0*(q[1]*q[2]-q[0]*q(31);C(0112)=2.0*(q(1)*q(3)+q[0]*q(2));C(1)(0]=2.0*(q(1]*q(21+q(0]*q[3]);C[1][1]=1.0-2.0*(q[1]*q[1]+q[3]*q(3));C(1)(2)=2.0*(q[3]*q(2)-q(0)*q(1));C[2][0]=2.0*(q(3)*q(11-q[0]*q(2]);C(2)(1)=2.0*(q[3]*q(2]+q[0]*q(1]);C[2][2]=1.0-2.0*(q(1)*q[1]+q(2)*q(2));return;}286

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

Customize your widget with the following options, then copy and paste the code below into the HTML of your page to embed this item in your website.
                        
                            <div id="ubcOpenCollectionsWidgetDisplay">
                            <script id="ubcOpenCollectionsWidget"
                            src="{[{embed.src}]}"
                            data-item="{[{embed.item}]}"
                            data-collection="{[{embed.collection}]}"
                            data-metadata="{[{embed.showMetadata}]}"
                            data-width="{[{embed.width}]}"
                            async >
                            </script>
                            </div>
                        
                    
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0065098/manifest

Comment

Related Items