diff --git a/doc/diagrams/architecture.puml b/doc/diagrams/architecture.puml index 884001d8ea..ff1c837d74 100644 --- a/doc/diagrams/architecture.puml +++ b/doc/diagrams/architecture.puml @@ -65,7 +65,6 @@ frame localization { frame gnc { [CTL] [FAM] - [EKF] () "/gnc/control\nff_msgs::ControlAction" <> as ControlAction () "/gnc/ekf/set_input\nff_msgs::SetEkfInput" <> as EkfSetInput () "/gnc/ekf\nff_msgs::EkfState" <> as EkfState @@ -94,7 +93,7 @@ EXECUTIVE <<-->> ArmAction EXECUTIVE --> InertiaStamped InertiaStamped --> FAM InertiaStamped --> CTL -InertiaStamped --> EKF +InertiaStamped --> LOC DockAction -- DOCK @@ -103,14 +102,14 @@ DOCK <<-->> SwitchAction PERCH <<-->> SwitchAction SwitchAction -- LOCALIZATION_MANAGER LOCALIZATION_MANAGER --> EkfSetInput -EkfSetInput --|> EKF +EkfSetInput --|> LOC DOCK <<-->> MotionAction PERCH <<-->> MotionAction PERCH <<-->> ArmAction MotionAction -- CHOREOGRAPHER CHOREOGRAPHER <<-->> ControlAction CHOREOGRAPHER --> SpeedGain -SpeedGain --> EKF +SpeedGain --> LOC SpeedGain --> FAM ControlAction -- CTL CTL --> WrenchStamped @@ -123,9 +122,9 @@ ArmAction -- ARM WrenchStamped --> FAM FAM --> PmcCommand ARM --> JointGoals -EKF --> PoseStamped -EKF --> TwistStamped -EKF --> EkfState +LOC --> PoseStamped +LOC --> TwistStamped +LOC --> EkfState TwistStamped --> CTL PoseStamped --> CTL JointStates --> ARM diff --git a/doc/diagrams/gnc_overview.puml b/doc/diagrams/gnc_overview.puml index 573e4036db..ecf7bb313f 100644 --- a/doc/diagrams/gnc_overview.puml +++ b/doc/diagrams/gnc_overview.puml @@ -11,7 +11,6 @@ frame mob { } frame gnc { - gnc_node(EKF) gnc_node(CTL) gnc_node(FAM) action(Control, ff_msgs::ControlAction, /gnc/control) @@ -31,20 +30,20 @@ message(Landmarks, ff_msgs::VisualLandmarks, "/loc/{ml,ar,of}/features") latching_msg(Inertia, geometry_msgs::InertiaStamped, /mob/inertia) latching_msg(FlightMode, ff_msgs::FlightMode, /mob/flight_mode) -Landmarks -r-> EKF : " " +Landmarks -r-> LOC : " " FlightMode --> FAM FlightMode --> choregrapher -Inertia -up-> EKF +Inertia -up-> LOC Inertia -up-> CTL Inertia -up-> FAM -'EKF -r-> Pose -'EKF -r-> Vels +'LOC -r-> Pose +'LOC -r-> Vels 'Pose -r-> CTL 'Vels -r-> CTL -EKF -r-> CTL : "/loc/{pose,twist}\n[geometry_msgs::{Pose,Twist}Stamped]" +LOC -r-> CTL : "/loc/{pose,twist}\n[geometry_msgs::{Pose,Twist}Stamped]" CTL -r-> FAM : "/gnc/ctl/command\n[ff_msgs::FamCommand]" 'CTL -r-> Wrench : " " 'Wrench -r-> FAM : " " diff --git a/doc/diagrams/nodes.puml b/doc/diagrams/nodes.puml index 4d50d29f26..96dcb8a92a 100644 --- a/doc/diagrams/nodes.puml +++ b/doc/diagrams/nodes.puml @@ -110,7 +110,6 @@ node LLP { end_manager() !endif start_manager("GN&C") - gnc_node([ekf]) gnc_node([ctl]) gnc_node([fam]) end_manager() diff --git a/doc/diagrams/notations.puml b/doc/diagrams/notations.puml index 0116078556..6127e547a3 100644 --- a/doc/diagrams/notations.puml +++ b/doc/diagrams/notations.puml @@ -27,10 +27,8 @@ node "Processor (LLP,MLP,HLP)" { folder "Interfaces (Messages, Services, Actions) Legend" as l2 { - gnc_node(EKF) gnc_node(CTL) message(EkfState, ff_msgs::EkfState, /gnc/ekf) - EKF --> EkfState EkfState --> CTL action(DockAction, ff_msgs::DockAction, /proc/dock) diff --git a/doc/diagrams/sim_concepts_robot.puml b/doc/diagrams/sim_concepts_robot.puml index e60af48c92..15f435236c 100644 --- a/doc/diagrams/sim_concepts_robot.puml +++ b/doc/diagrams/sim_concepts_robot.puml @@ -27,13 +27,12 @@ folder "Astrobee Robot" { node LLP <> COLOR_CPU { 'frame GNC { - gnc_node(EKF) gnc_node(CTL) gnc_node(FAM) '} drv_node(epson_imu) drv_node(pmc_actuator) - EKF -> CTL + LOC -> CTL CTL -> FAM message(PmcCommand, ff_hw_msgs::PmcCommand, /hw/pmc/command) message(ImuTelem, sensor_msgs::Imu, /hw/imu) @@ -52,12 +51,12 @@ folder "Astrobee Robot" { NavImage --> image_sampler sparse_mapping --> Features - Features ---> EKF + Features ---> LOC image_sampler --> ImgSample epson_imu --> ImuTelem - ImuTelem -> EKF + ImuTelem -> LOC FAM -> PmcCommand PmcCommand --> pmc_actuator diff --git a/doc/diagrams/sim_concepts_simulator.puml b/doc/diagrams/sim_concepts_simulator.puml index 86623f3901..debc5e5334 100644 --- a/doc/diagrams/sim_concepts_simulator.puml +++ b/doc/diagrams/sim_concepts_simulator.puml @@ -25,11 +25,10 @@ folder "Development Computer" { mgt_node(image_sampler) loc_node("real robot\nsparse_mapping\nnode not running") 'frame GNC { - gnc_node(EKF) gnc_node(CTL) gnc_node(FAM) '} - EKF -> CTL + LOC -> CTL CTL -> FAM } @@ -43,12 +42,12 @@ folder "Development Computer" { NavImage --> image_sampler sparse_mapping --> Features - Features ---> EKF + Features ---> LOC image_sampler --> ImgSample IMU --> ImuTelem - ImuTelem --> EKF + ImuTelem --> LOC FAM -up-> PmcCommand PmcCommand -up-> PMC diff --git a/doc/general_documentation/subsystems.md b/doc/general_documentation/subsystems.md index 0e61042165..32dbeabbda 100644 --- a/doc/general_documentation/subsystems.md +++ b/doc/general_documentation/subsystems.md @@ -25,7 +25,7 @@ Definition of topic names are maintained in \subpage shared:
- \subpage localization - directory: `localization` - topic prefix: `loc` - - We keep this name, everything necessary for localization, pose estimation is performed in the GNC EKF... + - Everything necessary for localization - include: - Sparse Mapping Features - AR Tags @@ -52,8 +52,6 @@ Definition of topic names are maintained in \subpage shared:
- directory: `gnc` - topic prefix: `gnc` - include: - - EKF - - Visual Odometry - Control ### Behaviors diff --git a/gnc/ekf/CMakeLists.txt b/gnc/ekf/CMakeLists.txt deleted file mode 100644 index 5a74453bb7..0000000000 --- a/gnc/ekf/CMakeLists.txt +++ /dev/null @@ -1,85 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -cmake_minimum_required(VERSION 3.0) -project(ekf) - -## Compile as C++14, supported in ROS Kinetic and newer -add_compile_options(-std=c++14) - -## Find catkin macros and libraries -find_package(catkin2 REQUIRED COMPONENTS - roscpp - ff_msgs - ff_util - config_reader - gnc_autocode - camera -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES ekf - CATKIN_DEPENDS roscpp ff_msgs ff_util config_reader gnc_autocode camera -) - -########### -## Build ## -########### - -# Specify additional locations of header files -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -# Declare C++ libraries -add_library(ekf - src/ekf.cc - src/ekf_nodelet.cc - src/ekf_wrapper.cc -) -add_dependencies(ekf ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ekf ${catkin_LIBRARIES}) - -############# -## Install ## -############# - -# Mark libraries for installation -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -# Mark nodelet_plugin for installation -install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -# Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE -) - -# Mark launch files for installation -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE) diff --git a/gnc/ekf/include/ekf/ekf.h b/gnc/ekf/include/ekf/ekf.h deleted file mode 100644 index 6283d66db0..0000000000 --- a/gnc/ekf/include/ekf/ekf.h +++ /dev/null @@ -1,191 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef EKF_EKF_H_ -#define EKF_EKF_H_ - -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace ekf { - -typedef struct { - float x; - float y; -} OFObservation; - -typedef struct { - uint32_t feature_id; - std::vector obs; - int missing_frames; // number of frames skipped -} OFFeature; - -/** - * @brief Ekf implementation using GNC module - * @details Ekf implementation using GNC module - */ -class Ekf { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Ekf(void); - ~Ekf(); - - void Reset(); - - kfl_msg* GetOutput(void) {return &gnc_.kfl_;} - - void ReadParams(config_reader::ConfigReader* config); - void SetBias(Eigen::Vector3f gyro_bias, Eigen::Vector3f accel_bias); - - void PrepareStep(const sensor_msgs::Imu & imu, const geometry_msgs::Quaternion & quat); - int Step(ff_msgs::EkfState* state); - - void SparseMapUpdate(const ff_msgs::VisualLandmarks & vl); - bool ARTagUpdate(const ff_msgs::VisualLandmarks & vl); // returns true if reset transform - bool HRTagUpdate(const ff_msgs::DepthLandmarks & dl); // returns true if reset transform - void OpticalFlowUpdate(const ff_msgs::Feature2dArray & of); - void HandrailUpdate(const ff_msgs::DepthLandmarks & dl); - - // transform by from dock coordinates to world coordinates - Eigen::Affine3d GetDockToWorldTransform(void) {return dock_to_world_;} - - // transform by from perch coordinates to world coordinates - Eigen::Affine3d GetHandrailToWorldTransform(void) {return handrail_to_world_;} - - void SparseMapRegister(const ff_msgs::CameraRegistration & reg); - void ARTagRegister(const ff_msgs::CameraRegistration & reg); - void OpticalFlowRegister(const ff_msgs::CameraRegistration & reg); - void HandrailRegister(const ff_msgs::CameraRegistration & reg); - - void SetSpeedGain(const uint8_t gain); - - void SetResetCallback(std::function callback); - void ResetAR(void); // called when switching to AR mode - void ResetHR(void); // called when switching to HR (HandRail) mode - - Eigen::Affine3d GetNavCamToBody(void) const {return nav_cam_to_body_;} - - bool HasPoseEstimate() const { return has_pose_estimate_; } - - protected: - /** Functions **/ - /** - * If we're totally lose, this function reinitializes - * the EKF with a pose gained from visual landmarks. - **/ - void ResetPose(const Eigen::Affine3d & camera_to_body, geometry_msgs::Pose const& pose); - void ApplyReset(void); - - void DepthLandmarksUpdate(const ff_msgs::DepthLandmarks & dl); - void VisualLandmarksUpdate(const ff_msgs::VisualLandmarks & vl); - void VisualLandmarksRegister(const ff_msgs::CameraRegistration & reg); - - /** - * Writes the inputs for one time step to a file - * that can later be parsed and tested in matlab. - **/ - void WriteToFile(void); - - void UpdateState(ff_msgs::EkfState* state); - - /** Variables **/ - // this calls the simulink code that actually runs the EKF - gnc_autocode::GncEkfAutocode gnc_; - - // we write to these, then copy to gnc_ - cvs_landmark_msg vis_; - cvs_registration_pulse reg_; - cvs_optical_flow_msg of_; - cvs_handrail_msg hand_; - imu_msg imu_; - int cmc_mode_; - - // EKF wrapper needs to capture the first landmark message to do PnP before - // it can initialize the EKF autocode and let it run. This variable holds - // that state and is atomic so that it doesn't require a mutex. - bool reset_ekf_; - // remember pose to reset to do in step function to avoid race conditions - Eigen::Affine3d reset_camera_to_body_; - geometry_msgs::Pose reset_pose_; - bool reset_ready_; - - // optional: called when a reset occurs - std::function reset_callback_; - - // vector of feature ids and observations, observations are features ids and positions - // this list is sorted by feature id - std::map optical_flow_features_; - // the number of features for each augmentation - std::vector optical_flow_augs_feature_counts_; - std::vector deleting_augs_; - // the times of each augmentation - std::vector optical_flow_augs_times_; - bool processing_of_reg_, of_inputs_delayed_; - - // in output to file mode, the file to output to - FILE* output_file_; - - // only save this for writing to a file later - geometry_msgs::Pose last_estimate_pose_; - bool has_pose_estimate_ = false; - - /** Configuration Constants **/ - - // transform from camera frame to body frame - Eigen::Affine3d camera_to_body_, nav_cam_to_body_, perch_cam_to_body_, dock_cam_to_body_, imu_to_body_; - float nav_cam_focal_length_, dock_cam_focal_length_; - int ml_max_features_; - int min_of_observations_; - unsigned int of_history_size_, of_max_features_; - int dl_max_features_; - - // camera ids currently stored in the EKF's augmented state - uint32_t vl_camera_id_; - uint32_t of_camera_id_; - uint32_t dl_camera_id_; - - /** ar offset related **/ - bool reset_dock_pose_; - Eigen::Affine3d dock_to_world_; - - /** hr offset relatedd **/ - bool reset_handrail_pose_; - Eigen::Affine3d handrail_to_world_; -}; -} // end namespace ekf - -#endif // EKF_EKF_H_ diff --git a/gnc/ekf/include/ekf/ekf_wrapper.h b/gnc/ekf/include/ekf/ekf_wrapper.h deleted file mode 100644 index e392e1b5d4..0000000000 --- a/gnc/ekf/include/ekf/ekf_wrapper.h +++ /dev/null @@ -1,216 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef EKF_EKF_WRAPPER_H_ -#define EKF_EKF_WRAPPER_H_ - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include // NOLINT -#include -#include -#include -#include -#include -#include - -namespace ekf { - -/** - * @brief ROS wrapper around EKF GNC code. - * @details ROS wrapper around EKF GNC code. - */ -class EkfWrapper { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - explicit EkfWrapper(ros::NodeHandle* nh, std::string const& name); - ~EkfWrapper(); - - void Run(std::atomic const& killed); - - /** - * This moves the EKF forward one time step - * and publishes the EKF pose. - * - * Returns zero if EKF was not run, non-zero if it was. - */ - int Step(); - - protected: - void ReadParams(void); - /** - * Initialize services and topics besides IMU. - **/ - void InitializeEkf(void); - /** - * Publishes a ROS message containing the state of the EKF. - **/ - void PublishState(const ff_msgs::EkfState & state); - - /** - * Resets the EKF. - **/ - bool ResetService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); //NOLINT - - bool ResetHandrailService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); - - /** - - * Initializes the bias and saves the results to a file. - **/ - bool InitializeBiasService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); //NOLINT - /** - * Set the input mode between mapped landmarks, AR tags, and handrail. - **/ - bool SetInputService(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res); //NOLINT - - /** - * Actually does the bias estimation, called from IMU callback. - **/ - void EstimateBias(sensor_msgs::Imu::ConstPtr const& imu); - - /** - * Callback functions. These all record the information - * and then pass it on to the EKF the next time the Step function - * is called. - **/ - void ImuCallBack(sensor_msgs::Imu::ConstPtr const& imu); - void OpticalFlowCallBack(ff_msgs::Feature2dArray::ConstPtr const& of); - void VLVisualLandmarksCallBack(ff_msgs::VisualLandmarks::ConstPtr const& vl); - void ARVisualLandmarksCallBack(ff_msgs::VisualLandmarks::ConstPtr const& vl); - void DepthLandmarksCallBack(ff_msgs::DepthLandmarks::ConstPtr const& vl); - void VLRegisterCamera(ff_msgs::CameraRegistration::ConstPtr const& cr); - void ARRegisterCamera(ff_msgs::CameraRegistration::ConstPtr const& cr); - void RegisterDepthCamera(ff_msgs::CameraRegistration::ConstPtr const& cr); - void RegisterOpticalFlowCamera(ff_msgs::CameraRegistration::ConstPtr const& cr); - void GroundTruthCallback(geometry_msgs::PoseStamped::ConstPtr const& truth); - void GroundTruthTwistCallback(geometry_msgs::TwistStamped::ConstPtr const& truth); - void FlightModeCallback(ff_msgs::FlightMode::ConstPtr const& mode); - - /** - * Callback when a subscriber joins or disconnects from the feature topic - **/ - void SubscriberCallback(); - void PublishFeatures(ff_msgs::VisualLandmarks::ConstPtr const& l); - void PublishFeatures(ff_msgs::DepthLandmarks::ConstPtr const& l); - - /** - * Callback when the EKF resets - **/ - void ResetCallback(); - - /** Variables **/ - // the actual EKF - Ekf ekf_; - ff_msgs::EkfState state_; - - bool ekf_initialized_; - - // most recent imu message - sensor_msgs::Imu imu_; - geometry_msgs::Quaternion quat_; - int imus_dropped_; - - // Truth messages - geometry_msgs::PoseStamped truth_pose_; - geometry_msgs::TwistStamped truth_twist_; - - // EKF wrapper needs to capture the first landmark message to do PnP before - // it can initialize the EKF autocode and let it run. This variable holds - // that state and is atomic so that it doesn't require a mutex. - std::atomic have_imu_; - std::atomic input_mode_; - - /** Configuration Constants **/ - - /** Ros **/ - config_reader::ConfigReader config_; - ff_util::PerfTimer pt_ekf_; - ros::Timer config_timer_; - - ros::NodeHandle* nh_; - // topic subscribers - ros::Subscriber imu_sub_, of_sub_, vl_sub_, ar_sub_, dl_sub_, truth_sub_, truth_twist_sub_; - ros::Subscriber vl_reg_sub_, ar_reg_sub_, of_reg_sub_, dl_reg_sub_; - ros::Subscriber flight_mode_sub_; - - // publisher - ros::Publisher state_pub_; - ros::Publisher feature_pub_; - ros::Publisher pose_pub_, twist_pub_; - ros::Publisher reset_pub_; - tf2_ros::TransformBroadcaster transform_pub_; - ros::ServiceServer reset_srv_, bias_srv_, input_mode_srv_, reset_hr_srv_; - - /** Threading **/ - - // mutex for msgs, concurrency protection - std::mutex mutex_act_msg_; - std::mutex mutex_imu_msg_; - std::mutex mutex_of_msg_; - std::mutex mutex_vl_msg_; - std::mutex mutex_dl_msg_; - std::mutex mutex_truth_msg_; - - // cv to wait for an imu reading - std::condition_variable cv_imu_; - - /** IMU Bias reset variables **/ - std::string bias_file_; - bool estimating_bias_; - float bias_reset_sums_[6]; - int bias_reset_count_; - int bias_required_observations_; - - /** Feature drawing **/ - std::string platform_name_; - bool disp_features_; - sensor_msgs::PointCloud2 features_; - - // Prevents needing to call ros::ok() from a thread - std::atomic killed_; -}; - -} // end namespace ekf - -#endif // EKF_EKF_WRAPPER_H_ diff --git a/gnc/ekf/launch/ekf.launch b/gnc/ekf/launch/ekf.launch deleted file mode 100644 index 573e1f0308..0000000000 --- a/gnc/ekf/launch/ekf.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/gnc/ekf/nodelet_plugins.xml b/gnc/ekf/nodelet_plugins.xml deleted file mode 100644 index d57eb9774e..0000000000 --- a/gnc/ekf/nodelet_plugins.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - This nodelet wraps GNC's implementations of the EKF module. - - - diff --git a/gnc/ekf/package.xml b/gnc/ekf/package.xml deleted file mode 100644 index 0062940cee..0000000000 --- a/gnc/ekf/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - ekf - 0.0.0 - Calls the GNC simulink auto-generated EKF code, and passes - inputs and outputs to and from ros messages. - - - Apache License, Version 2.0 - - - Astrobee Flight Software - - - Astrobee Flight Software - - catkin - roscpp - ff_msgs - ff_util - config_reader - gnc_autocode - camera - roscpp - ff_msgs - ff_util - ff_msgs - config_reader - gnc_autocode - camera - - - - diff --git a/gnc/ekf/readme.md b/gnc/ekf/readme.md deleted file mode 100644 index 33af094847..0000000000 --- a/gnc/ekf/readme.md +++ /dev/null @@ -1,76 +0,0 @@ -\page ekf GNC EKF Wrapper - -The extended kalman filter tracks the robot's pose. - -# Overview - -The EKF is based on the augmented state extended kalman filter by Mourikis et. al. -We localize from IMU readings and visual odometry, together with one of sparse map features, -AR tags, or handrail detections. Only one of these last features can be used at a time. -The filter has three main steps: - -* **Predict**: With every IMU measurement, the EKF forward predicts the robot's motion based on the IMU measurement (linear acceleration and angular velocity). The predict step must -be run a constant rate (currently 62.5 Hz). Every iteration -of the loop must run at this rate, so the performance of the code is critical. -* **Registration**: Camera images cannot be process within one iteration of the predict step. -Hence, we have a registration step when an image is captured. When a registration pulse is -sent to GNC, the EKF state is *augmented*, and we remember the robot's pose when the image -was captured, and capture the covariance to the rest of the state. -* **Update**: When an image is processsed, an observation is used to update the state. - -For more details on how the EKF works, see our paper, *Localization from Visual Landmarks on a Free-Flying Robot. -Brian Coltin, Jesse Fusco, Zack Moratto, Oleg Alexandrov, and Robert Nakamura. IROS 2016*. - -# Inputs - -* IMU Readings, `/hw/imu`: The IMU message. Must be received at a constant rate. -* Sparse Map Features, `/loc/ml/features` and `/loc/ml/registration`: -The features and registration pulses from the sparse map. The features include image coordinates and corresponding -3D feature positions from the map. -* AR Tag Features, `/loc/ar/features` and `/loc/ar/registration`: The same as the -sparse map features, but for AR tags. These are assumed to come from the dock camera. -* Optical Flow Features: `/loc/of/features` and `/loc/of/registration`: These -features are used for visual odometry. They are not associated with a 3D position, but are tracked over time -with a unique associated ID. Inside the EKF wrapper we keep track of these points over time and -send appropriate features to the EKF. -* Handrail Features: `/loc/handrail/features` and `/loc/handrail/registration`: The features for -localization with respect to the handrail. These are point features like with the sparse map features, but also -include a direction of the handrail axis and a boolean indicating whether the position along this axis has been observed. - -# Outputs - -* `/gnc/ekf`, the body state. See the EkfState message documentation for details. -* The body tf2 transform. - -# Services - -* `/gnc/ekf/reset`: Resets the EKF, setting the pose based on the next incoming visual landmarks. -* `/gnc/ekf/initialize_bias`: Observes the accelerometer readings over the next several seconds, -sets the bias, and resets the EKF. This must only be called when the robot is stationary! -* `/gnc/ekf/set_input`: Switches the input mode of the EKF between sparse map features, AR tags, -and handrail features. Only one of these inputs can be used at a time. However, visual -odometry is always active. - -# Threading Model - -For good pose tracking, it is key to eliminate latency in IMU measurements and camera -registrations. That is why we use a multi-threaded ROS node, in which all of the -subscriptions record incoming data in a non-blocking manner. A separate thread loops continuously -and runs the GNC autocode step function whenever a new IMU measurement is received -(informed via a condition variable). This allows us to guarantee that all received -registration pulses and updates will be processed on the next tick. - -Note that each step of the EKF must run within one IMU tick, or serious problems will arise. -All of the ROS subscriptions and threading code is put in `ekf_wrapper.cc`, while the core -functionality is presented in `ekf.cc`. This way the class in `ekf.cc` can be used in -other ways, for example, when processing a bag file. - -# Selecting Visual Odometry Features - -Largely, the EKF wrapper simply passes the inputs directly to the GNC matlab code. The one -exception is the visual odometry features. Here, the wrapper keeps track of which augmentations are -stored in the EKF, and the feature observations during those observations. It chooses which -augmentations to send to the EKF when, and then deletes the sent features and chooses new -augmentations. The goals are to send features spanning as wide as possible a period of time, -to drive the covariance down, and to never send the same observation twice (because this results in overconfidence). - diff --git a/gnc/ekf/src/ekf.cc b/gnc/ekf/src/ekf.cc deleted file mode 100644 index 7c44c193bc..0000000000 --- a/gnc/ekf/src/ekf.cc +++ /dev/null @@ -1,735 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -DEFINE_bool(save_inputs_file, false, "Save the inputs to a file."); - -namespace ekf { - -Ekf::Ekf(void) : - reset_ekf_(true), reset_ready_(false), reset_callback_(nullptr), - processing_of_reg_(false), of_inputs_delayed_(false), output_file_(NULL), - vl_camera_id_(0), of_camera_id_(0), dl_camera_id_(0), reset_dock_pose_(true) { - gnc_.cmc_.speed_gain_cmd = 1; // prevent from being invalid when running bags - of_history_size_ = ASE_OF_NUM_AUG; - of_max_features_ = ASE_OF_NUM_FEATURES; - ml_max_features_ = ASE_ML_NUM_FEATURES; - dl_max_features_ = ASE_ML_NUM_FEATURES; - memset(&vis_, 0, sizeof(cvs_landmark_msg)); - memset(®_, 0, sizeof(cvs_registration_pulse)); - memset(&of_, 0, sizeof(cvs_optical_flow_msg)); - memset(&hand_, 0, sizeof(cvs_handrail_msg)); - memset(&imu_, 0, sizeof(imu_msg)); - - - bool output = FLAGS_save_inputs_file; - if (output) { - output_file_ = fopen("ekf_inputs.csv", "w"); - assert(output_file_); - ROS_WARN("Recording EKF inputs. EKF *NOT* running."); - } -} - -Ekf::~Ekf() { - if (output_file_) { - fclose(output_file_); - output_file_ = NULL; - } -} - -void Ekf::ReadParams(config_reader::ConfigReader* config) { - gnc_.ReadParams(config); - - if (!config->GetInt("min_of_observations", &min_of_observations_)) - ROS_FATAL("Unspecified min_of_observations."); - - // get camera transform - Eigen::Vector3d trans; - Eigen::Quaterniond rot; - if (!msg_conversions::config_read_transform(config, "nav_cam_transform", &trans, &rot)) - ROS_FATAL("Unspecified nav_cam_transform."); - nav_cam_to_body_ = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * - Eigen::Affine3d(rot); - if (!msg_conversions::config_read_transform(config, "dock_cam_transform", &trans, &rot)) - ROS_FATAL("Unspecified dock_cam_transform."); - dock_cam_to_body_ = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * - Eigen::Affine3d(rot); - if (!msg_conversions::config_read_transform(config, "perch_cam_transform", &trans, &rot)) - ROS_FATAL("Unspecified perch_cam_transform."); - perch_cam_to_body_ = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * - Eigen::Affine3d(rot); - if (!msg_conversions::config_read_transform(config, "imu_transform", &trans, &rot)) - ROS_FATAL("Unspecified imu_transform."); - imu_to_body_ = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * - Eigen::Affine3d(rot); - - std::string imu_filename; - if (!config->GetStr("imu_bias_file", &imu_filename)) { - ROS_FATAL("IMU bias file not specified."); - } - - // Modifying here some initial bias estimates. Also setting gravity subtract - // to happen in the body frame instead of the world frame. Removing in the - // world frame is fragile because vision could possibly rotate the - // body_q_world slightly, making subtraction way off. - auto& p = gnc_.est_->defaultParam; - Eigen::Map gyro_fixed_bias(p->ase_gyro_fixed_bias), - accel_fixed_bias(p->ase_accel_fixed_bias); - std::vector new_bias(3); - FILE* f = fopen(imu_filename.c_str(), "r"); - if (f) { - int ret = fscanf(f, "%g %g %g\n", &new_bias[0], &new_bias[1], &new_bias[2]); - if (ret == 3) - gyro_fixed_bias = Eigen::Vector3f(new_bias[0], new_bias[1], new_bias[2]); - ret = fscanf(f, "%g %g %g\n", &new_bias[0], &new_bias[1], &new_bias[2]); - if (ret == 3) { - accel_fixed_bias = Eigen::Vector3f(new_bias[0], new_bias[1], new_bias[2]); - } - fclose(f); - } else { - ROS_WARN("No bias file found at %s.", imu_filename.c_str()); - } -} - -void Ekf::SetBias(Eigen::Vector3f gyro_bias, Eigen::Vector3f accel_bias) { - Eigen::Map gyro_fixed_bias( - gnc_.est_->defaultParam->ase_gyro_fixed_bias), - accel_fixed_bias(gnc_.est_->defaultParam->ase_accel_fixed_bias); - gyro_fixed_bias = gyro_bias; - accel_fixed_bias = accel_bias; -} - -void Ekf::OpticalFlowUpdate(const ff_msgs::Feature2dArray & of) { - // check that the camera id matches our registration - if (of_camera_id_ != of.camera_id) { - // ROS_DEBUG_THROTTLE(1, "Registered optical flow camera not found."); - return; - } - - // put new features in tree to make it faster - std::map new_features; - for (size_t i = 0; i < of.feature_array.size(); i++) { - new_features.insert(std::pair(of.feature_array[i].id, - {of.feature_array[i].x, of.feature_array[i].y})); - } - - // delete all observations that we didn't see - std::map::iterator of_iter = optical_flow_features_.begin(); - while (of_iter != optical_flow_features_.end()) { - if (new_features.count((*of_iter).first) == 0) { - OFFeature & f = (*of_iter).second; - f.missing_frames++; - } - of_iter++; - } - - // now, add new features to the list of existing features - for (size_t i = 0; i < of.feature_array.size(); i++) { - uint16_t feature_id = of.feature_array[i].id; - float x = of.feature_array[i].x; - float y = of.feature_array[i].y; - // add new feature to list - if (optical_flow_features_.count(feature_id) == 0) { - optical_flow_features_.insert(std::pair( - feature_id, {feature_id, std::vector(), 0})); - } - OFFeature & f = optical_flow_features_[feature_id]; - // add the new observations - f.obs.insert(f.obs.begin(), {x, y}); - optical_flow_augs_feature_counts_[0]++; - } - of_camera_id_ = 0; - - if (deleting_augs_.size() > 0) - return; - if (optical_flow_augs_feature_counts_.size() >= of_history_size_ && - optical_flow_augs_feature_counts_[of_history_size_ - 1] > 3) { - deleting_augs_.push_back(0); - if (optical_flow_augs_feature_counts_[of_history_size_ - 3] < 10) { - deleting_augs_.push_back(of_history_size_ - 3); - deleting_augs_.push_back(of_history_size_ - 2); - deleting_augs_.push_back(of_history_size_ - 1); - } else { - deleting_augs_.push_back(2); - if (optical_flow_augs_feature_counts_[of_history_size_ - 2] < 10) { - deleting_augs_.push_back(of_history_size_ - 2); - deleting_augs_.push_back(of_history_size_ - 1); - } else { - deleting_augs_.push_back(6); - if (optical_flow_augs_feature_counts_[of_history_size_ - 1] < 10) { - deleting_augs_.push_back(of_history_size_ - 1); - } else { - ros::Time now = of.header.stamp; - double age_1 = (now - optical_flow_augs_times_[of_history_size_ - 1]).toSec(); - double age_2 = (now - optical_flow_augs_times_[of_history_size_ - 2]).toSec(); - double age_3 = (now - optical_flow_augs_times_[of_history_size_ - 3]).toSec(); - double age_4 = (now - optical_flow_augs_times_[of_history_size_ - 4]).toSec(); - if (age_1 < 2 * age_2) - deleting_augs_.push_back(of_history_size_ - 2); - else if (age_2 < 2 * age_3) - deleting_augs_.push_back(of_history_size_ - 3); - else if (age_3 < 2 * age_4) - deleting_augs_.push_back(of_history_size_ - 4); - else - deleting_augs_.push_back(of_history_size_ - 5); - } - } - } - } else { - return; - } - - // clear all valid flags - unsigned int index = 0; - memset(of_.cvs_valid_flag, 0, of_history_size_ * of_max_features_); - // we have to wait until the next timestep - if (!processing_of_reg_) { - of_.cvs_timestamp_sec = of.header.stamp.sec; - of_.cvs_timestamp_nsec = of.header.stamp.nsec; - } else { - of_inputs_delayed_ = true; - } - std::map::iterator iter = optical_flow_features_.begin(); - while (iter != optical_flow_features_.end()) { - OFFeature & f = (*iter).second; - if (f.missing_frames > 0) { - // We are no longer using these observations because we can greatly speed up - // computation in the EKF with a sparse block H matrix - for (unsigned int i = 0; i < f.obs.size(); i++) { - unsigned int aug = i + f.missing_frames; - if (aug >= of_history_size_) - break; - // of_.cvs_observations[aug * of_max_features_ * 2 + index] = f.obs[i].x; - // of_.cvs_observations[aug * of_max_features_ * 2 + index + of_max_features_] = f.obs[i].y; - // of_.cvs_valid_flag[aug * of_max_features_ + index] = 1; - if (optical_flow_augs_feature_counts_.size() > aug) - optical_flow_augs_feature_counts_[aug]--; - } - iter = optical_flow_features_.erase(iter); - } else { - // only use oldest features, and choose three oldest frames and newest - if (f.obs.size() >= of_history_size_) { - of_.cvs_observations[0 * of_max_features_ * 2 + index] = f.obs[0].x; - of_.cvs_observations[0 * of_max_features_ * 2 + index + of_max_features_] = f.obs[0].y; - of_.cvs_valid_flag[0 * of_max_features_ + index] = 1; - for (unsigned int aug = of_history_size_ - 3; aug < of_history_size_; aug++) { - of_.cvs_observations[aug * of_max_features_ * 2 + index] = f.obs[aug].x; - of_.cvs_observations[aug * of_max_features_ * 2 + index + of_max_features_] = f.obs[aug].y; - of_.cvs_valid_flag[aug * of_max_features_ + index] = 1; - } - } - iter++; - index++; - } - if (index >= of_max_features_) - break; - } - // just delete any missing features we didn't have space for - while (iter != optical_flow_features_.end()) { - OFFeature & f = (*iter).second; - if (f.missing_frames > 0) { - for (unsigned int i = 0; i < f.obs.size(); i++) { - unsigned int aug = i + f.missing_frames; - if (aug >= of_history_size_) - break; - // of_.cvs_observations[aug * of_max_features_ * 2 + index] = f.obs[i].x; - // of_.cvs_observations[aug * of_max_features_ * 2 + index + of_max_features_] = f.obs[i].y; - // of_.cvs_valid_flag[aug * of_max_features_ + index] = 1; - if (optical_flow_augs_feature_counts_.size() > aug) - optical_flow_augs_feature_counts_[aug]--; - } - iter = optical_flow_features_.erase(iter); - } - iter++; - } -} - -void Ekf::SparseMapUpdate(const ff_msgs::VisualLandmarks & vl) { - VisualLandmarksUpdate(vl); - if (!output_file_ && reset_ekf_ && vl.landmarks.size() >= 5) - ResetPose(nav_cam_to_body_, vl.pose); - cmc_mode_ = ff_msgs::SetEkfInputRequest::MODE_MAP_LANDMARKS; -} - -void Ekf::ResetAR(void) { - reset_dock_pose_ = true; -} - -void Ekf::ResetHR(void) { - reset_handrail_pose_ = true; -} - -bool Ekf::ARTagUpdate(const ff_msgs::VisualLandmarks & vl) { - bool updated = false; - if (reset_dock_pose_) { - if (vl.landmarks.size() < 4) - return false; - geometry_msgs::Pose p; - p.position = msg_conversions::array_to_ros_point(gnc_.kfl_.P_B_ISS_ISS); - p.orientation = msg_conversions::array_to_ros_quat(gnc_.kfl_.quat_ISS2B); - Eigen::Affine3d wTb = msg_conversions::ros_pose_to_eigen_transform(p); - Eigen::Affine3d dTc = msg_conversions::ros_pose_to_eigen_transform(vl.pose); - Eigen::Affine3d bTc = dock_cam_to_body_; - dock_to_world_ = wTb * bTc * dTc.inverse(); - reset_dock_pose_ = false; - updated = true; - } - - ff_msgs::VisualLandmarks vl_mod = vl; - for (unsigned int i = 0; i < vl.landmarks.size(); i++) { - Eigen::Vector3d l(vl.landmarks[i].x, vl.landmarks[i].y, vl.landmarks[i].z); - l = dock_to_world_ * l; - vl_mod.landmarks[i].x = l.x(); - vl_mod.landmarks[i].y = l.y(); - vl_mod.landmarks[i].z = l.z(); - } - VisualLandmarksUpdate(vl_mod); - vl_mod.pose.position = msg_conversions::eigen_to_ros_point( - dock_to_world_ * msg_conversions::ros_point_to_eigen_vector(vl.pose.position)); - Eigen::Quaterniond rot(dock_to_world_.linear()); - vl_mod.pose.orientation = msg_conversions::eigen_to_ros_quat( - rot * msg_conversions::ros_to_eigen_quat(vl.pose.orientation)); - - if (!output_file_ && reset_ekf_ && vl_mod.landmarks.size() >= 4) - ResetPose(dock_cam_to_body_, vl_mod.pose); - cmc_mode_ = ff_msgs::SetEkfInputRequest::MODE_AR_TAGS; - - return updated; -} - -void Ekf::VisualLandmarksUpdate(const ff_msgs::VisualLandmarks & vl) { - // check that the camera frame matches the one we just registered - if (vl_camera_id_ != vl.camera_id) { - ROS_DEBUG_THROTTLE(1, "Registered visual landmark camera %d not found.", vl.camera_id); - return; - } - if (vl.landmarks.size() < 5) - return; - - last_estimate_pose_= vl.pose; - has_pose_estimate_ = true; - // find better way to choose limited landmarks to send? - for (int i = 0; i < std::min(ml_max_features_, static_cast(vl.landmarks.size())); i++) { - vis_.cvs_landmarks[i] = vl.landmarks[i].x; - vis_.cvs_landmarks[ml_max_features_ + i] = vl.landmarks[i].y; - vis_.cvs_landmarks[2 * ml_max_features_ + i] = vl.landmarks[i].z; - vis_.cvs_observations[i] = vl.landmarks[i].u; - vis_.cvs_observations[ml_max_features_ + i] = vl.landmarks[i].v; - vis_.cvs_valid_flag[i] = true; - } - for (int i = vl.landmarks.size(); i < ml_max_features_; i++) - vis_.cvs_valid_flag[i] = false; - - vis_.cvs_timestamp_sec = vl.header.stamp.sec; - vis_.cvs_timestamp_nsec = vl.header.stamp.nsec; -} - -bool Ekf::HRTagUpdate(const ff_msgs::DepthLandmarks & dl) { - bool updated = false; - if (reset_handrail_pose_) { - if (dl.landmarks.size() < 1) - return false; - geometry_msgs::Pose p; - p.position = msg_conversions::array_to_ros_point(gnc_.kfl_.P_B_ISS_ISS); - p.orientation = msg_conversions::array_to_ros_quat(gnc_.kfl_.quat_ISS2B); - - Eigen::Affine3d wTb = msg_conversions::ros_pose_to_eigen_transform(p); - Eigen::Affine3d cTh = msg_conversions::ros_pose_to_eigen_transform(dl.sensor_T_handrail); - Eigen::Affine3d bTc = perch_cam_to_body_; - handrail_to_world_ = wTb * bTc * cTh; - ROS_WARN(" [EKF] Recalculated handrail_to_world transform."); - reset_handrail_pose_ = false; - updated = true; - } - ff_msgs::DepthLandmarks dl_mod = dl; - for (unsigned int i = 0; i < dl.landmarks.size(); i++) { - Eigen::Vector3d l(dl.landmarks[i].u, dl.landmarks[i].v, dl.landmarks[i].w); - l = handrail_to_world_ * l; - dl_mod.landmarks[i].u = l.x(); - dl_mod.landmarks[i].v = l.y(); - dl_mod.landmarks[i].w = l.z(); - } - HandrailUpdate(dl); - dl_mod.sensor_T_handrail.position = msg_conversions::eigen_to_ros_point( - handrail_to_world_ * msg_conversions::ros_point_to_eigen_vector(dl.sensor_T_handrail.position)); - Eigen::Quaterniond rot(handrail_to_world_.linear()); - dl_mod.sensor_T_handrail.orientation = msg_conversions::eigen_to_ros_quat( - rot * msg_conversions::ros_to_eigen_quat(dl.sensor_T_handrail.orientation)); - - if (!output_file_ && reset_ekf_ && dl_mod.landmarks.size() >= 1) - ResetPose(perch_cam_to_body_, dl_mod.sensor_T_handrail); - cmc_mode_ = ff_msgs::SetEkfInputRequest::MODE_HANDRAIL; - - return updated; -} - -void Ekf::HandrailUpdate(const ff_msgs::DepthLandmarks & dl) { - // check that the camera frame matches the one we just registered - if (dl_camera_id_ != dl.camera_id) { - ROS_DEBUG_THROTTLE(1, "Registered depth landmark camera not found."); - return; - } - - for (int i = 0; i < std::min(dl_max_features_, static_cast(dl.landmarks.size())); i++) { - hand_.cvs_observations[i] = dl.landmarks[i].u; - hand_.cvs_observations[dl_max_features_ + i] = dl.landmarks[i].v; - hand_.cvs_observations[2 * dl_max_features_ + i] = dl.landmarks[i].w; - hand_.cvs_valid_flag[i] = true; - } - - for (int i = dl.landmarks.size(); i < dl_max_features_; i++) - hand_.cvs_valid_flag[i] = false; - - hand_.cvs_3d_knowledge_flag = dl.end_seen; - - if (reset_ekf_) { - if (hand_.cvs_handrail_update_global_pose_flag == 0) - hand_.cvs_handrail_update_global_pose_flag = 1; - else - reset_ekf_ = false; - } else { - hand_.cvs_handrail_update_global_pose_flag = 0; - } - - hand_.cvs_handrail_local_pos[0] = dl.sensor_T_handrail.position.x; - hand_.cvs_handrail_local_pos[1] = dl.sensor_T_handrail.position.y; - hand_.cvs_handrail_local_pos[2] = dl.sensor_T_handrail.position.z; - hand_.cvs_handrail_local_quat[0] = dl.sensor_T_handrail.orientation.x; - hand_.cvs_handrail_local_quat[1] = dl.sensor_T_handrail.orientation.y; - hand_.cvs_handrail_local_quat[2] = dl.sensor_T_handrail.orientation.z; - hand_.cvs_handrail_local_quat[3] = dl.sensor_T_handrail.orientation.w; - - hand_.cvs_timestamp_sec = dl.header.stamp.sec; - hand_.cvs_timestamp_nsec = dl.header.stamp.nsec; -} - -void Ekf::OpticalFlowRegister(const ff_msgs::CameraRegistration & cr) { - processing_of_reg_ = true; - unsigned int erased_aug = 0; - if (of_camera_id_ != 0) { - // ROS_WARN("Failed to get observations on time. Tossing last frame."); - erased_aug = 0; - } else { - if (deleting_augs_.size() > 0) { - erased_aug = deleting_augs_[0]; - deleting_augs_.erase(deleting_augs_.begin()); - } else { - // choose the augmentation to delete - if (optical_flow_augs_feature_counts_.size() < of_history_size_) { - erased_aug = of_history_size_ - 1; - } else if (optical_flow_augs_feature_counts_[of_history_size_ - 1] < 10) { - erased_aug = of_history_size_ - 1; - } else { - erased_aug = 0; - } - } - // delete the features from the deleted augmented state - std::map::iterator of_iter = optical_flow_features_.begin(); - while (of_iter != optical_flow_features_.end()) { - OFFeature & f = (*of_iter).second; - if (f.obs.size() > erased_aug) { - f.obs.erase(f.obs.begin() + erased_aug); - } - if (f.obs.size() == 0) - of_iter = optical_flow_features_.erase(of_iter); - else - of_iter++; - } - - // update arrays of times and counts - if (optical_flow_augs_feature_counts_.size() > of_history_size_) { - optical_flow_augs_feature_counts_.erase(optical_flow_augs_feature_counts_.begin() + erased_aug); - optical_flow_augs_times_.erase(optical_flow_augs_times_.begin() + erased_aug); - } - optical_flow_augs_feature_counts_.insert(optical_flow_augs_feature_counts_.begin(), 0); - optical_flow_augs_times_.insert(optical_flow_augs_times_.begin(), cr.header.stamp); - } - - // output to GNC - reg_.cvs_optical_flow_pulse = of_history_size_ - erased_aug; - of_camera_id_ = cr.camera_id; -} - -void Ekf::SparseMapRegister(const ff_msgs::CameraRegistration & reg) { - VisualLandmarksRegister(reg); -} - -void Ekf::ARTagRegister(const ff_msgs::CameraRegistration & reg) { - VisualLandmarksRegister(reg); -} - -void Ekf::VisualLandmarksRegister(const ff_msgs::CameraRegistration & reg) { - vl_camera_id_ = reg.camera_id; - reg_.cvs_landmark_pulse = true; -} - -void Ekf::HandrailRegister(const ff_msgs::CameraRegistration & reg) { - dl_camera_id_ = reg.camera_id; - reg_.cvs_handrail_pulse = true; -} - -void Ekf::SetSpeedGain(const uint8_t gain) { - gnc_.cmc_.speed_gain_cmd = gain; -} - -void Ekf::SetResetCallback(std::function callback) { - reset_callback_ = callback; -} - -// this saves all the inputs to a file if an output file is specified, -// can be debugged later in matlab -void Ekf::WriteToFile(void) { - // first output timestamp - fprintf(output_file_, "%d, %d, ", gnc_.imu_.imu_timestamp_sec, gnc_.imu_.imu_timestamp_nsec); // 1, 2 - // next all the imu data - fprintf(output_file_, "%g, %g, %g, %g, %g, %g, ", // 3, 4, 5, 6, 7, 8 - gnc_.imu_.imu_omega_B_ECI_sensor[0], gnc_.imu_.imu_omega_B_ECI_sensor[1], gnc_.imu_.imu_omega_B_ECI_sensor[2], - gnc_.imu_.imu_A_B_ECI_sensor[0], gnc_.imu_.imu_A_B_ECI_sensor[1], gnc_.imu_.imu_A_B_ECI_sensor[2]); - - // now the registration pulses - fprintf(output_file_, "%d, %d, ", gnc_.reg_.cvs_landmark_pulse, gnc_.reg_.cvs_optical_flow_pulse); // 9, 10 - // next visual landmarks - fprintf(output_file_, "%d, %d, ", gnc_.vis_.cvs_timestamp_sec, gnc_.vis_.cvs_timestamp_nsec); // 11, 12 - // now the estimated pose - fprintf(output_file_, "%g, %g, %g, ", last_estimate_pose_.position.x, // 13, 14, 15 - last_estimate_pose_.position.y, last_estimate_pose_.position.z); - fprintf(output_file_, "%g, %g, %g, %g, ", last_estimate_pose_.orientation.x, // 16, 17, 18, 19 - last_estimate_pose_.orientation.y, last_estimate_pose_.orientation.z, - last_estimate_pose_.orientation.w); - // now the contents of cvs_landmarks. Stored as x0 x1 x2 ... y0 y1 y2... z0 z1 z2... - for (int i = 0; i < ml_max_features_ * 3; i++) // 20 ~ 169 (50 * 3) - fprintf(output_file_, "%g, ", gnc_.vis_.cvs_landmarks[i]); - // then the observations, again stored as u0 u1 u2... v0 v1 v2... - for (int i = 0; i < ml_max_features_ * 2; i++) // 170 ~ 269 (50 * 2) - fprintf(output_file_, "%g, ", gnc_.vis_.cvs_observations[i]); - // finally the valid flags - for (int i = 0; i < ml_max_features_; i++) // 270 ~ 319 (50) - fprintf(output_file_, "%d, ", gnc_.vis_.cvs_valid_flag[i]); - - // next is the optical flow - fprintf(output_file_, "%d, %d, ", gnc_.of_.cvs_timestamp_sec, gnc_.of_.cvs_timestamp_nsec); // 320, 321 - // now the optical flow observations, stored as (xij is jth observation of feature i) - // x00 x10 x20 x30 ... xn0 - // y00 y10 y20 y30 ... yn0 - // x01 x11 x21 x31 ... xn1 - // ... - for (unsigned int i = 0; i < of_max_features_ * of_history_size_ * 2; i++) // 322 ~ 1921 (50 * 2 * 16) - fprintf(output_file_, "%g, ", gnc_.of_.cvs_observations[i]); - // then valid flags, stored as - // v00 v10 v20 v30 ... vn0 - // v01 v11 v21 v31 ... vn1 - // ... - for (unsigned int i = 0; i < of_max_features_ * of_history_size_; i++) // 1922 ~ 2721 (50 * 16) - fprintf(output_file_, "%d, ", gnc_.of_.cvs_valid_flag[i]); - - // handrail registration pulses - fprintf(output_file_, "%d, ", gnc_.reg_.cvs_handrail_pulse); // 2722 - fprintf(output_file_, "%d, %d, ", gnc_.hand_.cvs_timestamp_sec, gnc_.hand_.cvs_timestamp_nsec); // 2723, 2724 - // now the contents of handrail_landmarks. Stored as u0 u1 u2... v0 v1 v2... w0 w1 w2... - for (int i = 0; i < dl_max_features_ * 3; i++) // 2725 ~ 2874 (50 * 3) - fprintf(output_file_, "%g, ", gnc_.hand_.cvs_observations[i]); - - // finally the valid flags - for (int i = 0; i < dl_max_features_; i++) // 2875 ~ 2924 (50) - fprintf(output_file_, "%d, ", gnc_.hand_.cvs_valid_flag[i]); - - // now the handrail local pose - fprintf(output_file_, "%g, %g, %g, ", gnc_.hand_.cvs_handrail_local_pos[0], // 2925 ~ 2927 (3) - gnc_.hand_.cvs_handrail_local_pos[1], gnc_.hand_.cvs_handrail_local_pos[2]); - fprintf(output_file_, "%g, %g, %g, %g, ", gnc_.hand_.cvs_handrail_local_quat[0], // 2928 ~ 2931 (4) - gnc_.hand_.cvs_handrail_local_quat[1], gnc_.hand_.cvs_handrail_local_quat[2], - gnc_.hand_.cvs_handrail_local_quat[3]); - - // handrail end point detection flag - fprintf(output_file_, "%d, ", gnc_.hand_.cvs_3d_knowledge_flag); // 2932 - // handrail global pose update flag - fprintf(output_file_, "%d, ", gnc_.hand_.cvs_handrail_update_global_pose_flag); // 2933 - - // localization mode cmd - fprintf(output_file_, "%d", gnc_.cmc_.localization_mode_cmd); // 2934 - fprintf(output_file_, "\n"); -} - -void Ekf::PrepareStep(const sensor_msgs::Imu & imu, const geometry_msgs::Quaternion & quat) { - // set IMU values - // set timestamp - imu_.imu_timestamp_sec = imu.header.stamp.sec; - imu_.imu_timestamp_nsec = imu.header.stamp.nsec; - - // set angular vel, ros message is double cast into float - imu_.imu_omega_B_ECI_sensor[0] = static_cast(imu.angular_velocity.x); - imu_.imu_omega_B_ECI_sensor[1] = static_cast(imu.angular_velocity.y); - imu_.imu_omega_B_ECI_sensor[2] = static_cast(imu.angular_velocity.z); - - // set linear accel - imu_.imu_A_B_ECI_sensor[0] = static_cast(imu.linear_acceleration.x); - imu_.imu_A_B_ECI_sensor[1] = static_cast(imu.linear_acceleration.y); - imu_.imu_A_B_ECI_sensor[2] = static_cast(imu.linear_acceleration.z); - - // set validity - imu_.imu_validity_flag = 1; - - // set saturation - imu_.imu_sat_flag = 0; - - // set the ISS2BODY quaternion in preparation for step - this is effectively ignored - // if tun_ase_gravity_removal = true in gnc.config - gnc_.quat_[0] = quat.x; - gnc_.quat_[1] = quat.y; - gnc_.quat_[2] = quat.z; - gnc_.quat_[3] = quat.w; - - // then copy all other values in preparation for step - memcpy(&gnc_.vis_, &vis_, sizeof(cvs_landmark_msg)); - memcpy(&gnc_.reg_, ®_, sizeof(cvs_registration_pulse)); - memcpy(&gnc_.of_, &of_, sizeof(cvs_optical_flow_msg)); - memcpy(&gnc_.hand_, &hand_, sizeof(cvs_handrail_msg)); - memcpy(&gnc_.imu_, &imu_, sizeof(imu_msg)); - gnc_.cmc_.localization_mode_cmd = cmc_mode_; - - // prevent double registrations - reg_.cvs_landmark_pulse = false; - reg_.cvs_optical_flow_pulse = false; - // registration complete, now update next time - if (of_inputs_delayed_) { - of_.cvs_timestamp_sec = imu.header.stamp.sec; - of_.cvs_timestamp_nsec = imu.header.stamp.nsec; - of_inputs_delayed_ = false; - } - processing_of_reg_ = false; - - ApplyReset(); -} - -int Ekf::Step(ff_msgs::EkfState* state) { - if (output_file_) - WriteToFile(); - else - gnc_.Step(); - if (gnc_.kfl_.confidence == 2) - reset_ekf_ = true; - UpdateState(state); - return 1; -} - -void Ekf::UpdateState(ff_msgs::EkfState* state) { - // now copy everything to the output message - state->header.stamp.sec = imu_.imu_timestamp_sec; - state->header.stamp.nsec = imu_.imu_timestamp_nsec; - state->header.frame_id = "world"; - state->child_frame_id = "body"; - state->pose.position = msg_conversions::array_to_ros_point(gnc_.kfl_.P_B_ISS_ISS); - state->velocity = msg_conversions::array_to_ros_vector(gnc_.kfl_.V_B_ISS_ISS); - state->accel = msg_conversions::array_to_ros_vector(gnc_.kfl_.A_B_ISS_ISS); - state->pose.orientation = msg_conversions::array_to_ros_quat(gnc_.kfl_.quat_ISS2B); - state->omega = msg_conversions::array_to_ros_vector(gnc_.kfl_.omega_B_ISS_B); - state->accel_bias = msg_conversions::array_to_ros_vector(gnc_.kfl_.accel_bias); - state->gyro_bias = msg_conversions::array_to_ros_vector(gnc_.kfl_.gyro_bias); - state->confidence = gnc_.kfl_.confidence; - state->aug_state_enum = gnc_.kfl_.aug_state_enum; - state->status = gnc_.kfl_.kfl_status; - state->of_count = gnc_.kfl_.update_OF_tracks_cnt; - state->ml_count = gnc_.kfl_.update_ML_features_cnt; - std::copy(gnc_.kfl_.cov_diag, gnc_.kfl_.cov_diag + 15, state->cov_diag.c_array()); - state->hr_global_pose.position = msg_conversions::array_to_ros_point(gnc_.kfl_.hr_P_hr_ISS_ISS); - state->hr_global_pose.orientation = msg_conversions::array_to_ros_quat(gnc_.kfl_.hr_quat_ISS2hr); - if (state->ml_count > 0) - std::copy(gnc_.kfl_.ml_mahal_distance, gnc_.kfl_.ml_mahal_distance + ml_max_features_, - state->ml_mahal_dists.c_array()); - else - memset(state->ml_mahal_dists.c_array(), 0, ml_max_features_ * sizeof(float)); -} - -void Ekf::Reset(void) { - reset_ekf_ = true; -} - -void Ekf::ResetPose(const Eigen::Affine3d & camera_to_body, geometry_msgs::Pose const& pose) { - reset_camera_to_body_ = camera_to_body; - reset_pose_ = pose; - reset_ready_ = true; -} - -// reset ekf, during step function to prevent race conditions -void Ekf::ApplyReset(void) { - if (!reset_ready_) - return; - - // set the robot's position based on the pose - Eigen::Quaterniond world_q_body(reset_pose_.orientation.w, reset_pose_.orientation.x, - reset_pose_.orientation.y, reset_pose_.orientation.z); - Eigen::Quaterniond camera_to_body_rotation(reset_camera_to_body_.linear()); - Eigen::Vector3d world_r_body(reset_pose_.position.x, reset_pose_.position.y, reset_pose_.position.z); - world_q_body = world_q_body * camera_to_body_rotation.conjugate(); - Eigen::Quaterniond q1(0, reset_camera_to_body_.translation().x(), reset_camera_to_body_.translation().y(), - reset_camera_to_body_.translation().z()); - Eigen::Quaterniond temp = world_q_body * q1 * world_q_body.conjugate(); - world_r_body = world_r_body - Eigen::Vector3d(temp.x(), temp.y(), temp.z()); - Eigen::Vector3d world_r_imu = world_r_body + world_q_body * imu_to_body_.translation(); - auto& quat_ISS2B = gnc_.est_->defaultParam->tun_ase_state_ic_quat_ISS2B; - auto& P_B_ISS_ISS = gnc_.est_->defaultParam->tun_ase_state_ic_P_B_ISS_ISS; - auto& P_EST_ISS_ISS = gnc_.est_->defaultParam->tun_ase_state_ic_P_EST_ISS_ISS; - auto& V_B_ISS_ISS = gnc_.est_->defaultParam->tun_ase_state_ic_V_B_ISS_ISS; - quat_ISS2B[0] = world_q_body.x(); - quat_ISS2B[1] = world_q_body.y(); - quat_ISS2B[2] = world_q_body.z(); - quat_ISS2B[3] = world_q_body.w(); - P_B_ISS_ISS[0] = world_r_body[0]; - P_B_ISS_ISS[1] = world_r_body[1]; - P_B_ISS_ISS[2] = world_r_body[2]; - P_EST_ISS_ISS[0] = world_r_imu[0]; - P_EST_ISS_ISS[1] = world_r_imu[1]; - P_EST_ISS_ISS[2] = world_r_imu[2]; - V_B_ISS_ISS[0] = 0.0; - V_B_ISS_ISS[1] = 0.0; - V_B_ISS_ISS[2] = 0.0; - - ROS_INFO("Reset EKF."); - - // reset the EKF (especially for the covariance) - gnc_.Initialize(); - - // reset optical flow too - optical_flow_features_.clear(); - optical_flow_augs_feature_counts_.clear(); - optical_flow_augs_times_.clear(); - of_camera_id_ = 0; - processing_of_reg_ = false; - of_inputs_delayed_ = false; - - reset_ready_ = false; - reset_ekf_ = false; - - // If we have set the reset callback, call it now. - if (reset_callback_) - reset_callback_(); -} - -} // end namespace ekf diff --git a/gnc/ekf/src/ekf_nodelet.cc b/gnc/ekf/src/ekf_nodelet.cc deleted file mode 100644 index 61a103640d..0000000000 --- a/gnc/ekf/src/ekf_nodelet.cc +++ /dev/null @@ -1,62 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include - -namespace ekf { - -class EkfNodelet : public ff_util::FreeFlyerNodelet { - public: - EkfNodelet() : ff_util::FreeFlyerNodelet(NODE_EKF, true), killed_(false) {} - ~EkfNodelet() { - killed_ = true; - thread_->join(); - } - // This is called when the nodelet is loaded into the nodelet manager - void Initialize(ros::NodeHandle *nh) { - // Bootstrap our environment - ff_common::InitFreeFlyerApplication(getMyArgv()); - gnc_autocode::InitializeAutocode(this); - ekf_.reset( - new ekf::EkfWrapper(this->GetPlatformHandle(true), GetPlatform())); - thread_.reset(new std::thread( - &ekf::EkfWrapper::Run, ekf_.get(), std::ref(killed_))); - } - - private: - std::shared_ptr ekf_; - std::shared_ptr thread_; - std::atomic killed_; -}; - -} // end namespace ekf - -// Declare the plugin -PLUGINLIB_EXPORT_CLASS(ekf::EkfNodelet, nodelet::Nodelet); diff --git a/gnc/ekf/src/ekf_wrapper.cc b/gnc/ekf/src/ekf_wrapper.cc deleted file mode 100644 index dcec848bcd..0000000000 --- a/gnc/ekf/src/ekf_wrapper.cc +++ /dev/null @@ -1,513 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include -#include - -#include -#include - -#include -#include - -#include - -namespace ekf { - -EkfWrapper::EkfWrapper(ros::NodeHandle* nh, std::string const& platform_name) : - ekf_initialized_(false), imus_dropped_(0), have_imu_(false), - input_mode_(ff_msgs::SetEkfInputRequest::MODE_NONE), nh_(nh), - estimating_bias_(false), disp_features_(false), killed_(false) { - platform_name_ = (platform_name.empty() ? "" : platform_name + "/"); - - config_.AddFile("gnc.config"); - config_.AddFile("cameras.config"); - config_.AddFile("geometry.config"); - ReadParams(); - config_timer_ = nh->createTimer(ros::Duration(1), [this](ros::TimerEvent e) { - config_.CheckFilesUpdated(std::bind(&EkfWrapper::ReadParams, this));}, false, true); - pt_ekf_.Initialize("ekf"); - - // Register to receive a callback when the EKF resets - ekf_.SetResetCallback(std::bind(&EkfWrapper::ResetCallback, this)); - // subscribe to IMU first, then rest once IMU is ready - // this is so localization manager doesn't timeout - imu_sub_ = nh_->subscribe(TOPIC_HARDWARE_IMU, 5, &EkfWrapper::ImuCallBack, this, - ros::TransportHints().tcpNoDelay()); -} - -EkfWrapper::~EkfWrapper() { - killed_ = true; -} - -// wait to start up until the IMU is ready -void EkfWrapper::InitializeEkf(void) { - state_pub_ = nh_->advertise(TOPIC_GNC_EKF, 1); - pose_pub_ = nh_->advertise(TOPIC_LOCALIZATION_POSE, 1); - twist_pub_ = nh_->advertise(TOPIC_LOCALIZATION_TWIST, 1); - feature_pub_ = nh_->advertise(TOPIC_GNC_EKF_FEATURES, 1, - boost::bind(&EkfWrapper::SubscriberCallback, this), - boost::bind(&EkfWrapper::SubscriberCallback, this)); - reset_pub_ = nh_->advertise(TOPIC_GNC_EKF_RESET, 1); - - vl_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_ML_FEATURES, 1, &EkfWrapper::VLVisualLandmarksCallBack, this, - ros::TransportHints().tcpNoDelay()); - vl_reg_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_ML_REGISTRATION, 1, &EkfWrapper::VLRegisterCamera, this, - ros::TransportHints().tcpNoDelay()); - ar_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_AR_FEATURES, 1, &EkfWrapper::ARVisualLandmarksCallBack, this, - ros::TransportHints().tcpNoDelay()); - ar_reg_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_AR_REGISTRATION, 1, &EkfWrapper::ARRegisterCamera, this, - ros::TransportHints().tcpNoDelay()); - of_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_OF_FEATURES, 1, &EkfWrapper::OpticalFlowCallBack, this, - ros::TransportHints().tcpNoDelay()); - of_reg_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_OF_REGISTRATION, 1, &EkfWrapper::RegisterOpticalFlowCamera, this, - ros::TransportHints().tcpNoDelay()); - dl_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_HR_FEATURES, 1, &EkfWrapper::DepthLandmarksCallBack, this, - ros::TransportHints().tcpNoDelay()); - dl_reg_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_HR_REGISTRATION, 1, &EkfWrapper::RegisterDepthCamera, this, - ros::TransportHints().tcpNoDelay()); - truth_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_TRUTH, 1, &EkfWrapper::GroundTruthCallback, this, - ros::TransportHints().tcpNoDelay()); - truth_twist_sub_ = nh_->subscribe(TOPIC_LOCALIZATION_TRUTH_TWIST, 1, &EkfWrapper::GroundTruthTwistCallback, this, - ros::TransportHints().tcpNoDelay()); - flight_mode_sub_ = nh_->subscribe(TOPIC_MOBILITY_FLIGHT_MODE, 1, &EkfWrapper::FlightModeCallback, this); - - reset_srv_ = nh_->advertiseService(SERVICE_GNC_EKF_RESET, &EkfWrapper::ResetService, this); - bias_srv_ = nh_->advertiseService(SERVICE_GNC_EKF_INIT_BIAS, &EkfWrapper::InitializeBiasService, this); - input_mode_srv_ = nh_->advertiseService(SERVICE_GNC_EKF_SET_INPUT, &EkfWrapper::SetInputService, this); - - reset_hr_srv_ = nh_->advertiseService(SERVICE_GNC_EKF_RESET_HR, &EkfWrapper::ResetHandrailService, this); - - ekf_initialized_ = true; -} - -void EkfWrapper::ReadParams(void) { - if (!config_.ReadFiles()) { - ROS_ERROR("Failed to read config files."); - return; - } - - if (!config_.GetInt("bias_required_observations", &bias_required_observations_)) - ROS_FATAL("Unspecified bias_required_observations."); - - if (!config_.GetStr("imu_bias_file", &bias_file_)) { - ROS_FATAL("IMU bias file not specified."); - } - - ekf_.ReadParams(&config_); -} - -void EkfWrapper::SubscriberCallback() { - disp_features_ = (feature_pub_.getNumSubscribers() > 0); -} - -void EkfWrapper::PublishFeatures(ff_msgs::VisualLandmarks::ConstPtr const& l) { - if (!disp_features_) return; - features_.header = l->header; - features_.height = 1; - features_.width = l->landmarks.size(); - features_.fields.resize(3); - features_.fields[0].name = "x"; - features_.fields[0].offset = 0; - features_.fields[0].datatype = 7; - features_.fields[0].count = 1; - features_.fields[1].name = "y"; - features_.fields[1].offset = 4; - features_.fields[1].datatype = 7; - features_.fields[1].count = 1; - features_.fields[2].name = "z"; - features_.fields[2].offset = 8; - features_.fields[2].datatype = 7; - features_.fields[2].count = 1; - features_.is_bigendian = false; - features_.point_step = 12; - features_.row_step = features_.point_step * features_.width; - features_.is_dense = true; - features_.data.resize(features_.row_step); - for (unsigned int i = 0; i < l->landmarks.size(); i++) { - memcpy(&features_.data[features_.point_step * i + 0], &l->landmarks[i].x, 4); - memcpy(&features_.data[features_.point_step * i + 4], &l->landmarks[i].y, 4); - memcpy(&features_.data[features_.point_step * i + 8], &l->landmarks[i].z, 4); - } - feature_pub_.publish(features_); -} - -void EkfWrapper::PublishFeatures(ff_msgs::DepthLandmarks::ConstPtr const& l) { - if (!disp_features_) return; - features_.header = std_msgs::Header(); - features_.header.stamp = ros::Time::now(); - features_.header.frame_id = platform_name_ + "perch_cam"; - features_.height = 1; - features_.width = l->landmarks.size(); - features_.fields.resize(3); - features_.fields[0].name = "x"; - features_.fields[0].offset = 0; - features_.fields[0].datatype = 7; - features_.fields[0].count = 1; - features_.fields[1].name = "y"; - features_.fields[1].offset = 4; - features_.fields[1].datatype = 7; - features_.fields[1].count = 1; - features_.fields[2].name = "z"; - features_.fields[2].offset = 8; - features_.fields[2].datatype = 7; - features_.fields[2].count = 1; - features_.is_bigendian = false; - features_.point_step = 12; - features_.row_step = features_.point_step * features_.width; - features_.is_dense = true; - features_.data.resize(features_.row_step); - // The features are expressed as image plane coordinated (u,v) and a depth. We - // must use the camera model to convert this to an (x,y,z) coordinate. - static camera::CameraParameters cam_params(&config_, "perch_cam"); - static camera::CameraModel camera_model(Eigen::Vector3d(0, 0, 0), - Eigen::Matrix3d::Identity(), cam_params); - float x, y, z; - for (unsigned int i = 0; i < l->landmarks.size(); i++) { - Eigen::Vector3d p_c = l->landmarks[i].w * camera_model.Ray( - static_cast(l->landmarks[i].u), static_cast(l->landmarks[i].v)); - x = static_cast(p_c[0]); - y = static_cast(p_c[1]); - z = static_cast(p_c[2]); - memcpy(&features_.data[features_.point_step * i + 0], &x, 4); - memcpy(&features_.data[features_.point_step * i + 4], &y, 4); - memcpy(&features_.data[features_.point_step * i + 8], &z, 4); - } - feature_pub_.publish(features_); -} - -void EkfWrapper::ResetCallback() { - static std_msgs::Empty msg; - reset_pub_.publish(msg); -} - -void EkfWrapper::ImuCallBack(sensor_msgs::Imu::ConstPtr const& imu) { - // concurrency protection - std::unique_lock lock(mutex_imu_msg_); - while (have_imu_ && !killed_) - cv_imu_.wait_for(lock, std::chrono::milliseconds(8)); - - // copy IMU data - imu_ = *imu; - - have_imu_ = true; - // now notify the condition variable waiting for IMU that we have it - lock.unlock(); - cv_imu_.notify_all(); - - if (estimating_bias_) - EstimateBias(imu); -} - -void EkfWrapper::EstimateBias(sensor_msgs::Imu::ConstPtr const& imu) { - bias_reset_count_++; - bias_reset_sums_[0] += imu->angular_velocity.x; - bias_reset_sums_[1] += imu->angular_velocity.y; - bias_reset_sums_[2] += imu->angular_velocity.z; - bias_reset_sums_[3] += imu->linear_acceleration.x; - bias_reset_sums_[4] += imu->linear_acceleration.y; - bias_reset_sums_[5] += imu->linear_acceleration.z; - if (bias_reset_count_ >= bias_required_observations_) { - for (int i = 0; i < 6; i++) - bias_reset_sums_[i] = bias_reset_sums_[i] / bias_reset_count_; - - ROS_INFO("Esimated biases: gyro: %g %g %g accel: %g %g %g", - bias_reset_sums_[0], bias_reset_sums_[1], bias_reset_sums_[2], - bias_reset_sums_[3], bias_reset_sums_[4], bias_reset_sums_[5]); - FILE* f = fopen(bias_file_.c_str(), "w"); - if (f) { - fprintf(f, "%g %g %g\n", bias_reset_sums_[0], bias_reset_sums_[1], bias_reset_sums_[2]); - fprintf(f, "%g %g %g\n", bias_reset_sums_[3], bias_reset_sums_[4], bias_reset_sums_[5]); - fclose(f); - } else { - ROS_ERROR("Bias file %s could not be opened.", bias_file_.c_str()); - } - ekf_.SetBias(Eigen::Vector3f(bias_reset_sums_[0], bias_reset_sums_[1], bias_reset_sums_[2]), - Eigen::Vector3f(bias_reset_sums_[3], bias_reset_sums_[4], bias_reset_sums_[5])); - - estimating_bias_ = false; - ekf_.Reset(); - } -} - -void EkfWrapper::OpticalFlowCallBack(ff_msgs::Feature2dArray::ConstPtr const& of) { - std::lock_guard lock(mutex_of_msg_); - ekf_.OpticalFlowUpdate(*of.get()); -} - -void EkfWrapper::VLVisualLandmarksCallBack(ff_msgs::VisualLandmarks::ConstPtr const& vl) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_MAP_LANDMARKS) { - std::lock_guard lock(mutex_vl_msg_); - ekf_.SparseMapUpdate(*vl.get()); - PublishFeatures(vl); - } -} - -void EkfWrapper::ARVisualLandmarksCallBack(ff_msgs::VisualLandmarks::ConstPtr const& vl) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_AR_TAGS) { - std::lock_guard lock(mutex_vl_msg_); - bool updated = ekf_.ARTagUpdate(*vl.get()); - if (updated) { - Eigen::Affine3d t = ekf_.GetDockToWorldTransform(); - geometry_msgs::TransformStamped transform; - transform.header = std_msgs::Header(); - transform.header.stamp = ros::Time::now(); - transform.header.frame_id = "world"; - transform.child_frame_id = "dock/body"; - transform.transform.translation = msg_conversions::eigen_to_ros_vector(t.translation()); - transform.transform.rotation = msg_conversions::eigen_to_ros_quat(Eigen::Quaterniond(t.linear())); - transform_pub_.sendTransform(transform); - } - PublishFeatures(vl); - } -} - -void EkfWrapper::DepthLandmarksCallBack(ff_msgs::DepthLandmarks::ConstPtr const& dl) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_HANDRAIL) { - std::lock_guard lock(mutex_dl_msg_); - bool updated = ekf_.HRTagUpdate(*dl.get()); - if (updated) { - Eigen::Affine3d t = ekf_.GetHandrailToWorldTransform(); - geometry_msgs::TransformStamped transform; - transform.header = std_msgs::Header(); - transform.header.stamp = ros::Time::now(); - transform.header.frame_id = "world"; - transform.child_frame_id = "handrail/body"; - transform.transform.translation = msg_conversions::eigen_to_ros_vector(t.translation()); - transform.transform.rotation = msg_conversions::eigen_to_ros_quat(Eigen::Quaterniond(t.linear())); - transform_pub_.sendTransform(transform); - } - PublishFeatures(dl); - } -} - -void EkfWrapper::RegisterOpticalFlowCamera(ff_msgs::CameraRegistration::ConstPtr const& cr) { - std::lock_guard lock(mutex_of_msg_); - ekf_.OpticalFlowRegister(*cr.get()); -} - -void EkfWrapper::VLRegisterCamera(ff_msgs::CameraRegistration::ConstPtr const& reg) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_MAP_LANDMARKS) { - std::lock_guard lock(mutex_vl_msg_); - ekf_.SparseMapRegister(*reg.get()); - } -} - -void EkfWrapper::ARRegisterCamera(ff_msgs::CameraRegistration::ConstPtr const& reg) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_AR_TAGS) { - std::lock_guard lock(mutex_vl_msg_); - ekf_.ARTagRegister(*reg.get()); - } -} - -void EkfWrapper::RegisterDepthCamera(ff_msgs::CameraRegistration::ConstPtr const& reg) { - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_HANDRAIL) { - std::lock_guard lock(mutex_vl_msg_); - ekf_.HandrailRegister(*reg.get()); - } -} - -void EkfWrapper::GroundTruthCallback(geometry_msgs::PoseStamped::ConstPtr const& pose) { - // For certain contexts (like MGTF) we want to extract the correct orientation, and pass it to - // GNC, so that Earth's gravity can be extracted out of the linear acceleration. - std::lock_guard lk(mutex_truth_msg_); - assert(pose->header.frame_id == "world"); - quat_ = pose->pose.orientation; - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_TRUTH) { - truth_pose_= *pose; - pose_pub_.publish(pose); - } -} - -void EkfWrapper::GroundTruthTwistCallback(geometry_msgs::TwistStamped::ConstPtr const& twist) { - std::lock_guard lk(mutex_truth_msg_); - assert(twist->header.frame_id == "world"); - if (input_mode_ == ff_msgs::SetEkfInputRequest::MODE_TRUTH) { - truth_twist_= *twist; - twist_pub_.publish(twist); - } -} - -void EkfWrapper::FlightModeCallback(ff_msgs::FlightMode::ConstPtr const& mode) { - ekf_.SetSpeedGain(mode->speed); -} - -void EkfWrapper::Run(std::atomic const& killed) { - // Kill the step thread - while (!killed) { - ros::spinOnce(); - Step(); - } - // Make sure the IMU thread also gets killed - killed_ = true; -} - -int EkfWrapper::Step() { - // don't modify anything while we're copying the data - { - // wait until we get an imu reading with the condition variable - std::unique_lock lk(mutex_imu_msg_); - if (!have_imu_) - cv_imu_.wait_for(lk, std::chrono::milliseconds(8)); - if (!have_imu_) { - imus_dropped_++; - // publish a failure if we stop getting imu messages - if (imus_dropped_ > 10 && ekf_initialized_) { - state_.header.stamp = ros::Time::now(); - state_.confidence = 2; // lost - state_pub_.publish(state_); - ekf_.Reset(); - } - return 0; // Changed by Andrew due to 250Hz ctl messages when sim blocks (!) - } - imus_dropped_ = 0; - if (!ekf_initialized_) - InitializeEkf(); - - std::lock(mutex_act_msg_, mutex_vl_msg_, mutex_dl_msg_, mutex_of_msg_, mutex_truth_msg_); - std::lock_guard lock_actMsg(mutex_act_msg_, std::adopt_lock); - std::lock_guard lock_vlMsg(mutex_vl_msg_, std::adopt_lock); - std::lock_guard lock_dlMsg(mutex_dl_msg_, std::adopt_lock); - std::lock_guard lock_ofMsg(mutex_of_msg_, std::adopt_lock); - std::lock_guard lock_truthMsg(mutex_truth_msg_, std::adopt_lock); - // copy everything in EKF, so data structures can be modified for next - // step while current step processes. We pass the ground truth quaternion - // representing the latest ISS2BODY rotation, which is used in certain - // testing contexts to remove the effect of Earth's gravity. - ekf_.PrepareStep(imu_, quat_); - // don't reuse imu reading - have_imu_ = false; - } - cv_imu_.notify_all(); - - int ret = 1; - switch (input_mode_) { - // Don't do anything when localization in turned off - case ff_msgs::SetEkfInputRequest::MODE_NONE: - state_.ml_count = 0; // these could be set from before - state_.of_count = 0; - break; - // In truth mode we don't step the filter forward, but we do copy the pose - // and twist into the EKF message. - case ff_msgs::SetEkfInputRequest::MODE_TRUTH: { - std::lock_guard lk(mutex_truth_msg_); - ros::Time t = ros::Time::now(); - if (fabs((truth_pose_.header.stamp - t).toSec()) < 1 && - fabs((truth_twist_.header.stamp - t).toSec()) < 1) { - state_.header.stamp = t; - state_.pose = truth_pose_.pose; - state_.velocity = truth_twist_.twist.linear; - state_.omega = truth_twist_.twist.angular; - break; - } - ret = 0; - break; - } - // All other pipelines get stepped forward normally - default: - pt_ekf_.Tick(); - ret = ekf_.Step(&state_); - pt_ekf_.Tock(); - break; - } - // Let other elements in the system know that the bias is being estimated - state_.estimating_bias = estimating_bias_; - // Only send the state if the Step() function was successful - if (ret) - PublishState(state_); - return ret; -} - -void EkfWrapper::PublishState(const ff_msgs::EkfState & state) { - // Publish the full EKF state - state_pub_.publish(state); - // Only publish a transform if the confidence is good enough and we have - // actually populated the state (examine the header to check) - if (state.confidence == 0 && !state.header.frame_id.empty()) { - geometry_msgs::TransformStamped transform; - transform.header = state.header; - transform.child_frame_id = platform_name_ + "body"; - transform.transform.translation.x = state.pose.position.x; - transform.transform.translation.y = state.pose.position.y; - transform.transform.translation.z = state.pose.position.z; - transform.transform.rotation = state.pose.orientation; - transform_pub_.sendTransform(transform); - } - // Publish a truthful - if (input_mode_ != ff_msgs::SetEkfInputRequest::MODE_TRUTH) { - geometry_msgs::PoseStamped pose; - pose.header.stamp = state.header.stamp; - pose.pose = state.pose; - pose_pub_.publish(pose); - geometry_msgs::TwistStamped twist; - twist.header.stamp = state.header.stamp; - twist.twist.linear = state.velocity; - twist.twist.angular = state.omega; - twist_pub_.publish(twist); - } -} - -bool EkfWrapper::ResetService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { // NOLINT - ekf_.Reset(); - return true; -} - -// Service to reset the handrail position which is only calculated once. Unused, but -// should be used in the future. -bool EkfWrapper::ResetHandrailService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { - ekf_.ResetHR(); - return true; -} - -bool EkfWrapper::InitializeBiasService(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { // NOLINT - bias_reset_count_ = 0; - for (int i = 0; i < 6; i++) - bias_reset_sums_[i] = 0.0; - estimating_bias_ = true; - ROS_INFO("Beginning bias estimation."); - return true; -} - -bool EkfWrapper::SetInputService(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res) { //NOLINT - input_mode_ = req.mode; - switch (input_mode_) { - case ff_msgs::SetEkfInputRequest::MODE_AR_TAGS: - ekf_.ResetAR(); - ROS_INFO("EKF input switched to AR tags."); - break; - case ff_msgs::SetEkfInputRequest::MODE_HANDRAIL: - ekf_.ResetHR(); - ROS_INFO("EKF input switched to handrail."); - break; - case ff_msgs::SetEkfInputRequest::MODE_MAP_LANDMARKS: - ROS_INFO("EKF input switched to mapped landmarks."); - break; - case ff_msgs::SetEkfInputRequest::MODE_NONE: - ROS_INFO("EKF input switched to none."); - break; - case ff_msgs::SetEkfInputRequest::MODE_TRUTH: - ROS_INFO("EKF input switched to ground truth."); - break; - default: - break; - } - return true; -} - -} // end namespace ekf diff --git a/gnc/gnc_autocode/CMakeLists.txt b/gnc/gnc_autocode/CMakeLists.txt index b06e34b52e..e88687259f 100644 --- a/gnc/gnc_autocode/CMakeLists.txt +++ b/gnc/gnc_autocode/CMakeLists.txt @@ -41,7 +41,6 @@ if (GNC_ROOT_DIR) # blindly setting paths, TODO: confirm these paths exist set (GNC_ADDITIONAL_DIR ${GNC_ROOT_DIR}/AdditionalIncludes) set (GNC_CTL_DIR ${GNC_ROOT_DIR}/ctl_controller0_ert_rtw) - set (GNC_EST_DIR ${GNC_ROOT_DIR}/est_estimator_ert_rtw) set (GNC_FAM_DIR ${GNC_ROOT_DIR}/fam_force_allocation_module_ert_rtw) set (GNC_SIM_DIR ${GNC_ROOT_DIR}/sim_model_lib0_ert_rtw) set (GNC_BL1_DIR ${GNC_ROOT_DIR}/bpm_blower_1_propulsion_module_ert_rtw) @@ -60,9 +59,6 @@ set(GNC_SOURCES ${GNC_CTL_DIR}/ctl_tunable_funcs.cpp ${GNC_CTL_DIR}/ctl_controller0.cpp ${GNC_CTL_DIR}/ctl_controller0_data.cpp - ${GNC_EST_DIR}/est_estimator.cpp - ${GNC_EST_DIR}/est_estimator_data.cpp - ${GNC_EST_DIR}/est_tunable_funcs.cpp ${GNC_FAM_DIR}/fam_force_allocation_module.cpp ${GNC_FAM_DIR}/fam_force_allocation_module_data.cpp ${GNC_FAM_DIR}/fam_tunable_funcs.cpp @@ -87,7 +83,6 @@ set(GNC_SOURCES set(GNC_INCLUDES ${GNC_CTL_DIR} - ${GNC_EST_DIR} ${GNC_FAM_DIR} ${GNC_SIM_DIR} ${GNC_BL1_DIR} @@ -129,12 +124,10 @@ include_directories( add_library(gnc_autocode src/autocode.cc src/constants.cc - src/ekf.cc src/fam.cc src/sim_csv.cc src/blowers.cc src/ctl.cc - src/ekf_csv.cc src/sim.cc ${GNC_SOURCES} ) diff --git a/gnc/gnc_autocode/include/gnc_autocode/ekf.h b/gnc/gnc_autocode/include/gnc_autocode/ekf.h deleted file mode 100644 index db70858649..0000000000 --- a/gnc/gnc_autocode/include/gnc_autocode/ekf.h +++ /dev/null @@ -1,55 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_EKF_H_ -#define GNC_AUTOCODE_EKF_H_ - -extern "C" { -#include -} - -namespace config_reader { - class ConfigReader; -} - -namespace gnc_autocode { - -class GncEkfAutocode { - public: - GncEkfAutocode(void); - ~GncEkfAutocode(void); - - virtual void Initialize(); - virtual void Step(); - virtual void ReadParams(config_reader::ConfigReader* config); - - RT_MODEL_est_estimator_T* est_; - cvs_landmark_msg vis_; - cvs_registration_pulse reg_; - cvs_optical_flow_msg of_; - cvs_handrail_msg hand_; - imu_msg imu_; - real32_T quat_[4]; - // TODO(bcoltin): this needs to be removed by GNC - cmc_msg cmc_; - kfl_msg kfl_; - float* P_; -}; -} // end namespace gnc_autocode - -#endif // GNC_AUTOCODE_EKF_H_ diff --git a/gnc/gnc_autocode/include/gnc_autocode/ekf_csv.h b/gnc/gnc_autocode/include/gnc_autocode/ekf_csv.h deleted file mode 100644 index 84a3beb0bb..0000000000 --- a/gnc/gnc_autocode/include/gnc_autocode/ekf_csv.h +++ /dev/null @@ -1,44 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GNC_AUTOCODE_EKF_CSV_H_ -#define GNC_AUTOCODE_EKF_CSV_H_ - -#include - -#include -#include - -namespace gnc_autocode { - -class GncEkfCSV : public GncEkfAutocode { - public: - GncEkfCSV(void); - ~GncEkfCSV(void); - - virtual void Initialize(std::string directory); - virtual void Step(); - - FILE* kfl_file_; - protected: - int ReadStepState(void); - void SkipFirstLine(FILE* f); -}; -} // end namespace gnc_autocode - -#endif // GNC_AUTOCODE_EKF_CSV_H_ diff --git a/gnc/gnc_autocode/src/ekf.cc b/gnc/gnc_autocode/src/ekf.cc deleted file mode 100644 index d82b5475b7..0000000000 --- a/gnc/gnc_autocode/src/ekf.cc +++ /dev/null @@ -1,56 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include - -#include - -namespace gnc_autocode { - -GncEkfAutocode::GncEkfAutocode(void) { - int s = 15 + 6 + 6 * ASE_OF_NUM_AUG; - P_ = static_cast(malloc(sizeof(float) * s * s)); - assert(P_); - est_ = est_estimator(&vis_, ®_, &of_, &hand_, &imu_, &cmc_, quat_, &kfl_, P_); - assert(est_); - assert(rtmGetErrorStatus(est_) == NULL); - Initialize(); -} - -GncEkfAutocode::~GncEkfAutocode() { - est_estimator_terminate(est_); - free(P_); -} - -void GncEkfAutocode::Step() { - est_estimator_step(est_, &vis_, ®_, &of_, &hand_, &imu_, &cmc_, quat_, &kfl_, P_); -} - -void GncEkfAutocode::Initialize() { - // initialize model - est_estimator_initialize(est_, &vis_, ®_, &of_, &hand_, &imu_, &cmc_, quat_, &kfl_, P_); -} - -void GncEkfAutocode::ReadParams(config_reader::ConfigReader* config) { - est_ReadParams(config, est_); -} - -} // end namespace gnc_autocode diff --git a/gnc/gnc_autocode/src/ekf_csv.cc b/gnc/gnc_autocode/src/ekf_csv.cc deleted file mode 100644 index da68f1a4bf..0000000000 --- a/gnc/gnc_autocode/src/ekf_csv.cc +++ /dev/null @@ -1,111 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -namespace gnc_autocode { - -GncEkfCSV::GncEkfCSV(void) : GncEkfAutocode() {} - -void GncEkfCSV::Initialize(std::string directory) { - kfl_file_ = fopen((directory + "/out_kfl_msg.csv").c_str(), "r"); - if (kfl_file_ == NULL) { - fprintf(stderr, "Failed to open out_kfl_msg.csv\n"); - exit(1); - } - SkipFirstLine(kfl_file_); -} - -void GncEkfCSV::SkipFirstLine(FILE* f) { - char c; - do { - c = fgetc(f); - } while (c != '\n'); -} - -GncEkfCSV::~GncEkfCSV() { - fclose(kfl_file_); -} - -int GncEkfCSV::ReadStepState(void) { - int ret = fscanf(kfl_file_, "%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,", - &kfl_.quat_ISS2B[0], &kfl_.quat_ISS2B[1], &kfl_.quat_ISS2B[2], &kfl_.quat_ISS2B[3], - &kfl_.omega_B_ISS_B[0], &kfl_.omega_B_ISS_B[1], &kfl_.omega_B_ISS_B[2], - &kfl_.gyro_bias[0], &kfl_.gyro_bias[1], &kfl_.gyro_bias[2]); - if (ret != 10) { - fprintf(stderr, "Failed to read line part 1 in KFL file.\n"); - return 1; - } - ret = fscanf(kfl_file_, "%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%hhu,%hhu,", - &kfl_.V_B_ISS_ISS[0], &kfl_.V_B_ISS_ISS[1], &kfl_.V_B_ISS_ISS[2], - &kfl_.A_B_ISS_ISS[0], &kfl_.A_B_ISS_ISS[1], &kfl_.A_B_ISS_ISS[2], - &kfl_.accel_bias[0], &kfl_.accel_bias[1], &kfl_.accel_bias[2], - &kfl_.P_B_ISS_ISS[0], &kfl_.P_B_ISS_ISS[1], &kfl_.P_B_ISS_ISS[2], - &kfl_.confidence, &kfl_.aug_state_enum); - if (ret != 14) { - fprintf(stderr, "Failed to read line part 2 in KFL file.\n"); - return 1; - } - ret = fscanf(kfl_file_, "%g,%g,%g,%g,%g,%g,%g,", - &kfl_.ml_quat_ISS2cam[0], &kfl_.ml_quat_ISS2cam[1], - &kfl_.ml_quat_ISS2cam[2], &kfl_.ml_quat_ISS2cam[3], - &kfl_.ml_P_cam_ISS_ISS[0], &kfl_.ml_P_cam_ISS_ISS[1], &kfl_.ml_P_cam_ISS_ISS[2]); - if (ret != 7) { - fprintf(stderr, "Failed to read line part 3 in KFL file.\n"); - return 1; - } - for (int i = 0; i < 5; i++) { - ret = fscanf(kfl_file_, "%g,%g,%g,%g,", - &kfl_.of_quat_ISS2cam[i], &kfl_.of_quat_ISS2cam[5 + i], - &kfl_.of_quat_ISS2cam[10 + i], &kfl_.of_quat_ISS2cam[15 + i]); - if (ret != 4) { - fprintf(stderr, "Failed to read line part 4 in KFL file.\n"); - return 1; - } - } - for (int i = 0; i < 5; i++) { - ret = fscanf(kfl_file_, "%g,%g,%g,", &kfl_.of_P_cam_ISS_ISS[i], - &kfl_.of_P_cam_ISS_ISS[5 + i], &kfl_.of_P_cam_ISS_ISS[10 + i]); - if (ret != 3) { - fprintf(stderr, "Failed to read line part 5 in KFL file.\n"); - return 1; - } - } - for (int i = 0; i < 51; i++) { - ret = fscanf(kfl_file_, "%lf,", &kfl_.cov_diag[i]); - if (ret != 1) { - fprintf(stderr, "Failed to read line part 6 in KFL file.\n"); - return 1; - } - } - ret = fscanf(kfl_file_, "%hu\n", &kfl_.kfl_status); - if (ret != 1) { - fprintf(stderr, "Failed to read line part 7 in KFL file.\n"); - return 1; - } - return 0; -} - - -void GncEkfCSV::Step(void) { - if (ReadStepState()) - exit(0); -} - -} // namespace gnc_autocode - diff --git a/gnc/matlab/Prototypes/Proto_4/build_Proto4.m b/gnc/matlab/Prototypes/Proto_4/build_Proto4.m index 080164ac5c..752fd29d77 100644 --- a/gnc/matlab/Prototypes/Proto_4/build_Proto4.m +++ b/gnc/matlab/Prototypes/Proto_4/build_Proto4.m @@ -20,8 +20,7 @@ %% run([fileparts(mfilename('fullpath')) '/../../' 'astrobee_set_path']) % List of subsystems to build -models = {'astrobee/fsw_lib/est_estimator',... - 'astrobee/fsw_lib/ctl_controller', ... +models = {'astrobee/fsw_lib/ctl_controller', ... 'astrobee/fsw_lib/fam_force_allocation_module',... 'astrobee/sim_model_lib',... 'astrobee/sim_model_lib/veh_vehicle_model/bpm_blower_1_propulsion_module', ... @@ -59,7 +58,6 @@ %% Generate tunable functions dirs = ls; ctl_dir = regexp(dirs, 'ctl_\w+_rtw', 'match'); -est_dir = regexp(dirs, 'est_\w+_rtw', 'match'); fam_dir = regexp(dirs, 'fam_\w+_rtw', 'match'); sim_dir = regexp(dirs, 'sim_\w+_rtw', 'match'); @@ -69,12 +67,6 @@ cd .. end -if ~isempty(est_dir) - cd(est_dir{1}); - generate_est_param_reads; - cd .. -end - if ~isempty(fam_dir) cd(fam_dir{1}); generate_fam_param_reads; diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/buildInfo.mat b/gnc/matlab/code_generation/est_estimator_ert_rtw/buildInfo.mat deleted file mode 100644 index 621d550a2a..0000000000 Binary files a/gnc/matlab/code_generation/est_estimator_ert_rtw/buildInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/codeInfo.mat b/gnc/matlab/code_generation/est_estimator_ert_rtw/codeInfo.mat deleted file mode 100644 index 359f7838b5..0000000000 Binary files a/gnc/matlab/code_generation/est_estimator_ert_rtw/codeInfo.mat and /dev/null differ diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/defines.txt b/gnc/matlab/code_generation/est_estimator_ert_rtw/defines.txt deleted file mode 100644 index 3d2edf20c6..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/defines.txt +++ /dev/null @@ -1,15 +0,0 @@ -RT_MALLOC -MODEL=est_estimator -NUMST=1 -NCSTATES=0 -HAVESTDIO -UNIX -ONESTEPFCN=1 -TERMFCN=1 -MAT_FILE=0 -MULTI_INSTANCE_CODE=1 -INTEGER_CODE=0 -MT=0 -CLASSIC_INTERFACE=0 -ALLOCATIONFCN=1 -TID01EQ=0 diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/ert_main.cpp b/gnc/matlab/code_generation/est_estimator_ert_rtw/ert_main.cpp deleted file mode 100644 index 10e2381df9..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/ert_main.cpp +++ /dev/null @@ -1,165 +0,0 @@ -// -// File: ert_main.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include -#include // This ert_main.c example uses printf/fflush -#include "est_estimator.h" // Model's header file -#include "rtwtypes.h" - -// '/landmark_msg' -static cvs_landmark_msg est_estimator_U_landmark_msg; - -// '/Vision Registration' -static cvs_registration_pulse est_estimator_U_VisionRegistration; - -// '/optical_flow_msg' -static cvs_optical_flow_msg est_estimator_U_cvs_optical_flow_msg_n; - -// '/handrail_msg' -static cvs_handrail_msg est_estimator_U_handrail_msg; - -// '/imu_msg' -static imu_msg est_estimator_U_imu_msg_c; - -// '/cmc_msg' -static cmc_msg est_estimator_U_cmc_msg_o; - -// '/true_q_ISS2B' -static real32_T est_estimator_U_Q_ISS2B[4]; - -// '/kfl_msg' -static kfl_msg est_estimator_Y_kfl_msg_h; - -// '/P_out' -static ase_cov_datatype est_estimator_Y_P_out[13689]; -const char *RT_MEMORY_ALLOCATION_ERROR = "memory allocation error"; - -// -// Associating rt_OneStep with a real-time clock or interrupt service routine -// is what makes the generated code "real-time". The function rt_OneStep is -// always associated with the base rate of the model. Subrates are managed -// by the base rate from inside the generated code. Enabling/disabling -// interrupts and floating point context switches are target specific. This -// example code indicates where these should take place relative to executing -// the generated code step function. Overrun behavior should be tailored to -// your application needs. This example simply sets an error status in the -// real-time model and returns from rt_OneStep. -// -void rt_OneStep(RT_MODEL_est_estimator_T *const est_estimator_M); -void rt_OneStep(RT_MODEL_est_estimator_T *const est_estimator_M) -{ - static boolean_T OverrunFlag = false; - - // Disable interrupts here - - // Check for overrun - if (OverrunFlag) { - rtmSetErrorStatus(est_estimator_M, "Overrun"); - return; - } - - OverrunFlag = true; - - // Save FPU context here (if necessary) - // Re-enable timer or interrupt here - // Set model inputs here - - // Step the model - est_estimator_step(est_estimator_M, &est_estimator_U_landmark_msg, - &est_estimator_U_VisionRegistration, - &est_estimator_U_cvs_optical_flow_msg_n, - &est_estimator_U_handrail_msg, &est_estimator_U_imu_msg_c, - &est_estimator_U_cmc_msg_o, est_estimator_U_Q_ISS2B, - &est_estimator_Y_kfl_msg_h, est_estimator_Y_P_out); - - // Get model outputs here - - // Indicate task complete - OverrunFlag = false; - - // Disable interrupts here - // Restore FPU context here (if necessary) - // Enable interrupts here -} - -// -// The example "main" function illustrates what is required by your -// application code to initialize, execute, and terminate the generated code. -// Attaching rt_OneStep to a real-time clock is target specific. This example -// illustrates how you do this relative to initializing the model. -// -int_T main(int_T argc, const char *argv[]) -{ - RT_MODEL_est_estimator_T *est_estimator_M; - - // Unused arguments - (void)(argc); - (void)(argv); - - // Allocate model data - est_estimator_M = est_estimator(&est_estimator_U_landmark_msg, - &est_estimator_U_VisionRegistration, &est_estimator_U_cvs_optical_flow_msg_n, - &est_estimator_U_handrail_msg, &est_estimator_U_imu_msg_c, - &est_estimator_U_cmc_msg_o, est_estimator_U_Q_ISS2B, - &est_estimator_Y_kfl_msg_h, est_estimator_Y_P_out); - if (est_estimator_M == NULL) { - (void)fprintf(stderr,"Memory allocation error during model " - "registration"); - return(1); - } - - if (rtmGetErrorStatus(est_estimator_M) != NULL) { - (void)fprintf(stderr,"Error during model registration: %s\n", - rtmGetErrorStatus(est_estimator_M)); - - // Disable rt_OneStep() here - - // Terminate model - est_estimator_terminate(est_estimator_M); - return(1); - } - - // Initialize model - est_estimator_initialize(est_estimator_M, &est_estimator_U_landmark_msg, - &est_estimator_U_VisionRegistration, &est_estimator_U_cvs_optical_flow_msg_n, - &est_estimator_U_handrail_msg, &est_estimator_U_imu_msg_c, - &est_estimator_U_cmc_msg_o, est_estimator_U_Q_ISS2B, - &est_estimator_Y_kfl_msg_h, est_estimator_Y_P_out); - - // Attach rt_OneStep to a timer or interrupt service routine with - // period 0.016 seconds (the model's base sample time) here. The - // call syntax for rt_OneStep is - // - // rt_OneStep(est_estimator_M); - - printf("Warning: The simulation will run forever. " - "Generated ERT main won't simulate model step behavior. " - "To change this behavior select the 'MAT-file logging' option.\n"); - fflush((NULL)); - while (rtmGetErrorStatus(est_estimator_M) == (NULL)) { - // Perform other application tasks here - } - - // Disable rt_OneStep() here - - // Terminate model - est_estimator_terminate(est_estimator_M); - return 0; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.cpp b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.cpp deleted file mode 100644 index 3a6d27ad71..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.cpp +++ /dev/null @@ -1,9448 +0,0 @@ -// -// File: est_estimator.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "est_estimator.h" -#include "est_estimator_private.h" - -const kfl_msg est_estimator_rtZkfl_msg = { - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // quat_ISS2B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // gyro_bias - - { - 0.0F, 0.0F, 0.0F } - , // V_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // accel_bias - - { - 0.0F, 0.0F, 0.0F } - , // P_B_ISS_ISS - 0U, // confidence - 0U, // aug_state_enum - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // ml_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F } - , // ml_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F - } - , // cov_diag - 0U, // kfl_status - 0U, // update_OF_tracks_cnt - 0U, // update_ML_features_cnt - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // of_mahal_distance - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // ml_mahal_distance - - { - 0.0F, 0.0F, 0.0F } - , // hr_P_hr_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // hr_quat_ISS2hr - - { - 0.0F, 0.0F, 0.0F } - // P_EST_ISS_ISS -} ; // kfl_msg ground - -const cmc_msg est_estimator_rtZcmc_msg = { { 0U,// timestamp_sec - 0U, // timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // P_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // V_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // A_B_ISS_ISS - { 0.0F, 0.0F, 0.0F, 0.0F }, // quat_ISS2B - { 0.0F, 0.0F, 0.0F }, // omega_B_ISS_B - { 0.0F, 0.0F, 0.0F } // alpha_B_ISS_B - }, // cmc_state_cmd_a - { 0U, // timestamp_sec - 0U, // timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // P_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // V_B_ISS_ISS - { 0.0F, 0.0F, 0.0F }, // A_B_ISS_ISS - { 0.0F, 0.0F, 0.0F, 0.0F }, // quat_ISS2B - { 0.0F, 0.0F, 0.0F }, // omega_B_ISS_B - { 0.0F, 0.0F, 0.0F } // alpha_B_ISS_B - }, // cmc_state_cmd_b - 0U, // cmc_mode_cmd - 0U, // speed_gain_cmd - 0U, // localization_mode_cmd - { 0.0F, 0.0F, 0.0F }, // att_kp - { 0.0F, 0.0F, 0.0F }, // att_ki - { 0.0F, 0.0F, 0.0F }, // omega_kd - { 0.0F, 0.0F, 0.0F }, // pos_kp - { 0.0F, 0.0F, 0.0F }, // pos_ki - { 0.0F, 0.0F, 0.0F }, // vel_kd - { 0.0F, 0.0F, 0.0F }, // center_of_mass - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// inertia_matrix - 0.0F // mass -}; - -const cvs_handrail_msg est_estimator_rtZcvs_handrail_m = { 0U,// cvs_timestamp_sec - 0U, // cvs_timestamp_nsec - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// cvs_landmarks - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// cvs_observations - { 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U },// cvs_valid_flag - 0U, // cvs_3d_knowledge_flag - { 0.0F, 0.0F, 0.0F }, // cvs_handrail_local_pos - { 0.0F, 0.0F, 0.0F, 0.0F }, // cvs_handrail_local_quat - 0U // cvs_handrail_update_global_pose_flag -}; - -const cvs_landmark_msg est_estimator_rtZcvs_landmark_m = { 0U,// cvs_timestamp_sec - 0U, // cvs_timestamp_nsec - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// cvs_landmarks - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F },// cvs_observations - { 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U }// cvs_valid_flag -}; - -const cvs_optical_flow_msg est_estimator_rtZcvs_optical_fl = { 0U,// cvs_timestamp_sec - 0U, // cvs_timestamp_nsec - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F }, // cvs_observations - { 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, - 0U, 0U }, // cvs_valid_flag - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }// cvs_id_tag -}; - -const cvs_registration_pulse est_estimator_rtZcvs_registrati = { 0U,// cvs_ar_tag_pulse - 0U, // cvs_landmark_pulse - 0U, // cvs_optical_flow_pulse - 0U // cvs_handrail_pulse -}; - -const imu_msg est_estimator_rtZimu_msg = { 0U,// imu_timestamp_sec - 0U, // imu_timestamp_nsec - { 0.0F, 0.0F, 0.0F }, // imu_A_B_ECI_sensor - { 0.0F, 0.0F, 0.0F }, // imu_accel_bias - { 0.0F, 0.0F, 0.0F }, // imu_omega_B_ECI_sensor - { 0.0F, 0.0F, 0.0F }, // imu_gyro_bias - 0U, // imu_validity_flag - 0U // imu_sat_flag -}; - -// -// System initialize for atomic system: -// '/MATLAB Function1' -// '/MATLAB Function1' -// -void est_estima_MATLABFunction1_Init(DW_MATLABFunction1_est_estima_T *localDW) -{ - localDW->prev_impeller_speed = -1.0F; - - // No number will be equal to NaN, this will trigger the coefficients to update on the first tick - localDW->y = 0.0F; - localDW->den[0] = 0.0F; - localDW->num[0] = localDW->den[0]; - localDW->den[1] = 0.0F; - localDW->num[1] = localDW->den[1]; - localDW->den[2] = 1.0F; - localDW->num[2] = localDW->den[2]; -} - -// -// Start for atomic system: -// '/MATLAB Function1' -// '/MATLAB Function1' -// -void est_estim_MATLABFunction1_Start(B_MATLABFunction1_est_estimat_T *localB) -{ - localB->num_out[0] = 0.0F; - localB->den_out[0] = 0.0F; - localB->num_out[1] = 0.0F; - localB->den_out[1] = 0.0F; - localB->num_out[2] = 0.0F; - localB->den_out[2] = 0.0F; -} - -// -// Output and update for atomic system: -// '/MATLAB Function1' -// '/MATLAB Function1' -// -void est_estimator_MATLABFunction1(real_T rtu_Ts, real32_T rtu_impeller_speed, - B_MATLABFunction1_est_estimat_T *localB, DW_MATLABFunction1_est_estima_T - *localDW) -{ - real_T Ws; - real_T Nyquist_W; - real32_T Aliased_W[4]; - real32_T Omega_n; - boolean_T b_x[4]; - int32_T idx; - int32_T b_ii; - int32_T ii_data_idx_0; - int32_T ii_sizes_idx_1; - boolean_T exitg1; - - // MATLAB Function 'est_estimator/filter_prep/imu_prep/filter/MATLAB Function1': ':1' - // Calculate notch filter coefficients, note only executes when the impeller speeds change - // Setup to only run when the impeller_speed changes - // update_flag = (prev_impeller_speed ~= impeller_speed); - if (localDW->prev_impeller_speed != rtu_impeller_speed) { - // ':1:22' - // Filter Bandwidth - // ':1:25' - // Sample frequency, Hz - // ':1:26' - Ws = 1.0 / rtu_Ts * 6.2831853071795862; - - // Sample frequency, Rad/sec - // ':1:27' - Nyquist_W = Ws / 2.0; - - // Nyquist rate, rad/sec - // ':1:29' - // Look for the possible aliased frequncies - // Find the actual aliased frequency - // ':1:32' - Omega_n = (real32_T)fabs((real_T)((real32_T)(Ws * 0.0) - rtu_impeller_speed)); - b_x[0] = ((Omega_n > 0.0F) && ((real_T)Omega_n <= Nyquist_W)); - Aliased_W[0] = Omega_n; - Omega_n = (real32_T)fabs((real_T)((real32_T)Ws - rtu_impeller_speed)); - b_x[1] = ((Omega_n > 0.0F) && ((real_T)Omega_n <= Nyquist_W)); - Aliased_W[1] = Omega_n; - Omega_n = (real32_T)fabs((real_T)((real32_T)(Ws * 2.0) - rtu_impeller_speed)); - b_x[2] = ((Omega_n > 0.0F) && ((real_T)Omega_n <= Nyquist_W)); - Aliased_W[2] = Omega_n; - Omega_n = (real32_T)fabs((real_T)((real32_T)(Ws * 3.0) - rtu_impeller_speed)); - b_x[3] = ((Omega_n > 0.0F) && ((real_T)Omega_n <= Nyquist_W)); - Aliased_W[3] = Omega_n; - idx = 0; - ii_sizes_idx_1 = 1; - b_ii = 1; - exitg1 = false; - while ((!exitg1) && (b_ii < 5)) { - if (b_x[(int32_T)(b_ii - 1)]) { - idx = 1; - ii_data_idx_0 = b_ii; - exitg1 = true; - } else { - b_ii++; - } - } - - if (idx == 0) { - ii_sizes_idx_1 = 0; - } - - if (!(ii_sizes_idx_1 == 0)) { - // ':1:34' - // ':1:35' - localDW->y = Aliased_W[(int32_T)(ii_data_idx_0 - 1)]; - - // Impeller frequency - // ':1:36' - Omega_n = 3.14159274F * localDW->y / (real32_T)Nyquist_W; - - // Normalized frequency - // ':1:37' - Ws = 39.478417604357432 / Nyquist_W; - - // Normalized bandwidth - // Calc coefficients of filter, from: - // http://onlinelibrary.wiley.com/store/10.1002/mop.23439/asset/23439_ftp.pdf?v=1&t=iyyqfya1&s=b26e8f569108cf78a04dbd5e7edb44f70b9bc3d1 - // ':1:41' - // ':1:42' - Ws = (1.0 - tan(Ws / 2.0)) / (tan(Ws / 2.0) + 1.0); - - // ':1:43' - localDW->num[0] = (real32_T)(1.0 + Ws) / 2.0F; - localDW->num[1] = 2.0F * -(real32_T)cos((real_T)Omega_n) * (real32_T)(1.0 - + Ws) / 2.0F; - localDW->num[2] = (real32_T)(Ws + 1.0) / 2.0F; - - // ':1:44' - localDW->den[0] = 1.0F; - localDW->den[1] = (real32_T)(1.0 + Ws) * -(real32_T)cos((real_T)Omega_n); - localDW->den[2] = (real32_T)Ws; - } else { - // ':1:47' - localDW->y = 0.0F; - - // ':1:48' - // ':1:49' - localDW->num[0] = 0.0F; - localDW->den[0] = 0.0F; - localDW->num[1] = 0.0F; - localDW->den[1] = 0.0F; - localDW->num[2] = 1.0F; - localDW->den[2] = 1.0F; - } - } - - // ':1:54' - localDW->prev_impeller_speed = rtu_impeller_speed; - - // ':1:55' - // ':1:56' - // ':1:57' - localB->num_out[0] = localDW->num[0]; - localB->den_out[0] = localDW->den[0]; - localB->num_out[1] = localDW->num[1]; - localB->den_out[1] = localDW->den[1]; - localB->num_out[2] = localDW->num[2]; - localB->den_out[2] = localDW->den[2]; -} - -// -// Termination for atomic system: -// '/MATLAB Function1' -// '/MATLAB Function1' -// -void est_estima_MATLABFunction1_Term(void) -{ -} - -// -// Output and update for atomic system: -// '/MATLAB Function' -// '/MATLAB Function' -// '/MATLAB Function' -// -void est_estimator_MATLABFunction(const real32_T rtu_u[16], - B_MATLABFunction_est_estimato_T *localB) -{ - real32_T normA; - real32_T b_s; - int32_T b_j; - int32_T eint; - static const real32_T theta[3] = { 0.425873F, 1.8801527F, 3.92572474F }; - - real32_T rtu_u_0[16]; - int32_T i; - boolean_T exitg1; - boolean_T exitg2; - - // MATLAB Function 'first_order_quaternion_propogation/MATLAB Function': ':1' - // ':1:4' - normA = 0.0F; - b_j = 0; - exitg2 = false; - while ((!exitg2) && (b_j < 4)) { - b_s = (((real32_T)fabs((real_T)rtu_u[(int32_T)((int32_T)(b_j << 2) + 1)]) + - (real32_T)fabs((real_T)rtu_u[(int32_T)(b_j << 2)])) + (real32_T)fabs - ((real_T)rtu_u[(int32_T)((int32_T)(b_j << 2) + 2)])) + (real32_T)fabs - ((real_T)rtu_u[(int32_T)((int32_T)(b_j << 2) + 3)]); - if (rtIsNaNF(b_s)) { - normA = (rtNaNF); - exitg2 = true; - } else { - if (b_s > normA) { - normA = b_s; - } - - b_j++; - } - } - - if (normA <= 3.92572474F) { - b_j = 0; - exitg1 = false; - while ((!exitg1) && (b_j < 3)) { - if (normA <= theta[b_j]) { - mglnkfkfmglfjekn_PadeApproximantOfDegree(rtu_u, (uint8_T)(int32_T) - ((int32_T)(b_j << 1) + 3), localB->y); - exitg1 = true; - } else { - b_j++; - } - } - } else { - b_s = normA / 3.92572474F; - if ((!rtIsInfF(b_s)) && (!rtIsNaNF(b_s))) { - b_s = (real32_T)frexp((real_T)b_s, &eint); - normA = (real32_T)eint; - } else { - normA = 0.0F; - } - - if (b_s == 0.5F) { - normA--; - } - - b_s = rt_powf_snf(2.0F, normA); - for (eint = 0; eint < 16; eint++) { - rtu_u_0[eint] = rtu_u[eint] / b_s; - } - - mglnkfkfmglfjekn_PadeApproximantOfDegree(rtu_u_0, 7U, localB->y); - for (b_j = 0; b_j <= (int32_T)((int32_T)normA - 1); b_j++) { - for (eint = 0; eint < 4; eint++) { - for (i = 0; i < 4; i++) { - rtu_u_0[(int32_T)(eint + (int32_T)(i << 2))] = 0.0F; - rtu_u_0[(int32_T)(eint + (int32_T)(i << 2))] += localB->y[(int32_T)(i << - 2)] * localB->y[eint]; - rtu_u_0[(int32_T)(eint + (int32_T)(i << 2))] += localB->y[(int32_T) - ((int32_T)(i << 2) + 1)] * localB->y[(int32_T)(eint + 4)]; - rtu_u_0[(int32_T)(eint + (int32_T)(i << 2))] += localB->y[(int32_T) - ((int32_T)(i << 2) + 2)] * localB->y[(int32_T)(eint + 8)]; - rtu_u_0[(int32_T)(eint + (int32_T)(i << 2))] += localB->y[(int32_T) - ((int32_T)(i << 2) + 3)] * localB->y[(int32_T)(eint + 12)]; - } - } - - for (eint = 0; eint < 4; eint++) { - localB->y[(int32_T)(eint << 2)] = rtu_u_0[(int32_T)(eint << 2)]; - localB->y[(int32_T)(1 + (int32_T)(eint << 2))] = rtu_u_0[(int32_T) - ((int32_T)(eint << 2) + 1)]; - localB->y[(int32_T)(2 + (int32_T)(eint << 2))] = rtu_u_0[(int32_T) - ((int32_T)(eint << 2) + 2)]; - localB->y[(int32_T)(3 + (int32_T)(eint << 2))] = rtu_u_0[(int32_T) - ((int32_T)(eint << 2) + 3)]; - } - } - } -} - -// -// Termination for atomic system: -// '/MATLAB Function' -// '/MATLAB Function' -// '/MATLAB Function' -// -void est_estimat_MATLABFunction_Term(void) -{ -} - -// -// Output and update for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void est_estimator_Normalize(const real32_T rtu_q_in[4], real32_T - rty_positive_scalar_q[4], P_Normalize_est_estimator_T *localP) -{ - // Product: '/Product' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - - rty_positive_scalar_q[0] = rtu_q_in[0] * (real32_T)localP->Constant1_Value; - rty_positive_scalar_q[1] = rtu_q_in[1] * (real32_T)localP->Constant1_Value; - rty_positive_scalar_q[2] = rtu_q_in[2] * (real32_T)localP->Constant1_Value; - rty_positive_scalar_q[3] = rtu_q_in[3] * (real32_T)localP->Constant1_Value; -} - -// -// Termination for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void est_estimator_Normalize_Term(void) -{ -} - -// -// Output and update for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void est_estimator_Normalize_p(const real32_T rtu_Vec[4], real32_T rtu_Magnitude, - real32_T rty_Normalized_Vec[4]) -{ - // Product: '/Divide' - rty_Normalized_Vec[0] = rtu_Vec[0] / rtu_Magnitude; - rty_Normalized_Vec[1] = rtu_Vec[1] / rtu_Magnitude; - rty_Normalized_Vec[2] = rtu_Vec[2] / rtu_Magnitude; - rty_Normalized_Vec[3] = rtu_Vec[3] / rtu_Magnitude; -} - -// -// Termination for action system: -// '/Normalize' -// '/Normalize' -// '/Normalize' -// -void est_estimator_Normalize_h_Term(void) -{ -} - -// -// Output and update for action system: -// '/If Action Subsystem1' -// '/If Action Subsystem1' -// -void est_estimato_IfActionSubsystem1(const kfl_msg *rtu_state_in, const - ase_cov_datatype rtu_P_in[13689], const real_T rtu_aug_velocity[3], const - real_T rtu_aug_velocity_p[3], const real_T rtu_aug_velocity_l[48], const - real_T rtu_aug_velocity_o[48], kfl_msg *rty_state_out, ase_cov_datatype - rty_P_out[13689], real_T rty_aug_velocity_out[3], real_T - rty_aug_velocity_out_p[3], real_T rty_aug_velocity_out_l[48], real_T - rty_aug_velocity_out_o[48]) -{ - int32_T i; - - // SignalConversion: '/Signal Conversion' - *rty_state_out = *rtu_state_in; - - // Inport: '/P_in' - for (i = 0; i < 13689; i++) { - rty_P_out[i] = rtu_P_in[i]; - } - - // End of Inport: '/P_in' - - // SignalConversion: '/Signal Conversion1' - rty_aug_velocity_out[0] = rtu_aug_velocity[0]; - rty_aug_velocity_out[1] = rtu_aug_velocity[1]; - rty_aug_velocity_out[2] = rtu_aug_velocity[2]; - - // SignalConversion: '/Signal Conversion1' - rty_aug_velocity_out_p[0] = rtu_aug_velocity_p[0]; - rty_aug_velocity_out_p[1] = rtu_aug_velocity_p[1]; - rty_aug_velocity_out_p[2] = rtu_aug_velocity_p[2]; - - // SignalConversion: '/Signal Conversion1' - for (i = 0; i < 48; i++) { - rty_aug_velocity_out_l[i] = rtu_aug_velocity_l[i]; - } - - // End of SignalConversion: '/Signal Conversion1' - - // SignalConversion: '/Signal Conversion1' - for (i = 0; i < 48; i++) { - rty_aug_velocity_out_o[i] = rtu_aug_velocity_o[i]; - } - - // End of SignalConversion: '/Signal Conversion1' -} - -// -// Termination for action system: -// '/If Action Subsystem1' -// '/If Action Subsystem1' -// -void est_est_IfActionSubsystem1_Term(void) -{ -} - -// Model step function -void est_estimator_step(RT_MODEL_est_estimator_T *const est_estimator_M, - cvs_landmark_msg *est_estimator_U_landmark_msg, cvs_registration_pulse - *est_estimator_U_VisionRegistration, cvs_optical_flow_msg - *est_estimator_U_cvs_optical_flow_msg_n, cvs_handrail_msg - *est_estimator_U_handrail_msg, imu_msg *est_estimator_U_imu_msg_c, cmc_msg - *est_estimator_U_cmc_msg_o, real32_T est_estimator_U_Q_ISS2B[4], kfl_msg - *est_estimator_Y_kfl_msg_h, ase_cov_datatype est_estimator_Y_P_out[13689]) -{ - P_est_estimator_T *est_estimator_P = ((P_est_estimator_T *) - est_estimator_M->defaultParam); - B_est_estimator_T *est_estimator_B = ((B_est_estimator_T *) - est_estimator_M->blockIO); - DW_est_estimator_T *est_estimator_DW = ((DW_est_estimator_T *) - est_estimator_M->dwork); - - // local block i/o variables - real32_T rtb_ex_matrix_multiply1[180]; - real32_T rtb_ex_matrix_multiply3[225]; - real32_T rtb_ex_of_residual_and_h_o2[96]; - real32_T rtb_ex_of_residual_and_h_o6[50]; - real32_T rtb_ex_compute_delta_state_and_[117]; - real32_T rtb_ex_apply_delta_state_o1[4]; - real32_T rtb_ex_apply_delta_state_o2[3]; - real32_T rtb_ex_apply_delta_state_o3[3]; - real32_T rtb_ex_apply_delta_state_o4[3]; - real32_T rtb_ex_apply_delta_state_o5[3]; - real32_T rtb_ex_apply_delta_state_o6[4]; - real32_T rtb_ex_apply_delta_state_o7[3]; - real32_T rtb_ex_apply_delta_state_o9[64]; - real32_T rtb_ex_apply_delta_state_o10[48]; - real32_T rtb_ex_compute_delta_state_an_p[117]; - real32_T rtb_ex_apply_delta_state_o1_d[4]; - real32_T rtb_ex_apply_delta_state_o2_i[3]; - real32_T rtb_ex_apply_delta_state_o3_e[3]; - real32_T rtb_ex_apply_delta_state_o4_g[3]; - real32_T rtb_ex_apply_delta_state_o5_i[3]; - real32_T rtb_ex_apply_delta_state_o6_d[4]; - real32_T rtb_ex_apply_delta_state_o7_p[3]; - real32_T rtb_ex_apply_delta_state_o9_c[64]; - real32_T rtb_ex_apply_delta_state_o10_i[48]; - real32_T rtb_ex_matrix_multiply5[225]; - real32_T rtb_MathFunction_eu[225]; - int32_T rtb_ex_of_residual_and_h_o1; - int32_T rtb_ex_compute_delta_state_an_g; - int32_T rtb_ex_compute_delta_state_a_pg; - uint32_T rtb_ex_of_residual_and_h_o4; - uint16_T rtb_ex_apply_delta_state_o8; - uint16_T rtb_ex_apply_delta_state_o8_j; - uint8_T rtb_ex_of_residual_and_h_o5; - - // local scratch DWork variables - int32_T ForEach_itr; - int32_T ForEach_itr_g; - real32_T accel[3]; - real32_T S[9]; - real32_T C[9]; - static const int8_T b[9] = { -1, 0, 0, 0, -1, 0, 0, 0, -1 }; - - static const int8_T c[45] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0 }; - - static const real32_T theta[3] = { 0.425873F, 1.8801527F, 3.92572474F }; - - static const int8_T d[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - boolean_T d_0[12]; - real32_T x[12]; - boolean_T b_0[17]; - int32_T ar; - static const int8_T Aug_Indx[102] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, - 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, - 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, - 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, - 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, - 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102 }; - - real_T kept_augmentations[15]; - real_T of_in_prange[90]; - real32_T M[126]; - static const int8_T g[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - real_T camera_tf_camera1[256]; - real32_T global_tf_camera1[16]; - real32_T A[96]; - real32_T b_1[32]; - real_T num_augs; - real_T aug_ind; - real_T ind0; - int32_T b_m; - static const int8_T b_2[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - int32_T handrail_knowledge_dims; - static const int8_T t[12] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0 }; - - static const int8_T v[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - static const int8_T c_a[9] = { -1, 0, 0, 0, -1, 0, 0, 0, -1 }; - - real_T camera_ml_tf_global[12]; - real_T next_ml_tf_global[12]; - real32_T temp[6]; - int32_T num_original; - boolean_T empty_non_axis_sizes; - int32_T nx; - static const int8_T g_0[12] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0 }; - - uint32_T rtb_BitwiseOperator; - real32_T rtb_P_B_ISS_ISS[3]; - real_T rtb_UnitDelay18[50]; - real_T rtb_UnitDelay19[50]; - real32_T rtb_UnitDelay25[4]; - real32_T rtb_Sum1_k3[3]; - boolean_T rtb_Compare; - boolean_T rtb_FixPtRelationalOperator_n; - real32_T rtb_VectorConcatenate[16]; - real32_T rtb_VectorConcatenate_hq[16]; - real32_T rtb_Assignment_b[9]; - real32_T rtb_Sum_k1; - uint8_T rtb_Saturation_n; - real32_T rtb_ImpAsg_InsertedFor_Out1_a_d[3]; - real32_T rtb_Product1[4]; - real32_T rtb_Assignment_n[9]; - real32_T rtb_ImpAsg_InsertedFor_Out1_at_[3]; - boolean_T rtb_LogicalOperator; - boolean_T rtb_of_update_flag; - real32_T rtb_Q[144]; - real32_T rtb_state_trans[225]; - real_T rtb_ml_vel_aug[3]; - real_T rtb_ml_omega_aug[3]; - real32_T rtb_Merge_o[4]; - real32_T rtb_Sum2[3]; - int32_T s12_iter; - real32_T rtb_VectorConcatenate_c[16]; - real32_T rtb_VectorConcatenate_p[16]; - real32_T rtb_Add_e[3]; - real32_T rtb_VectorConcatenate_m[16]; - real32_T rtb_VectorConcatenate_i[16]; - uint8_T rtb_Switch_m; - real32_T rtb_global_points[200]; - real32_T rtb_camera_tf_global[256]; - int32_T rtb_valid_out[800]; - real32_T rtb_Gain1_f; - real32_T rtb_Assignment_ef[9]; - real32_T rtb_Sum_l; - uint8_T rtb_numFeatures_f; - real32_T rtb_Switch1; - uint8_T rtb_BusCreator2_update_ML_featu; - uint32_T rtb_BusAssignment_aug_state_enu; - real32_T rtb_Product2[16]; - boolean_T rtb_Compare_ic[50]; - real32_T rtb_Sum_b[225]; - real_T rtb_of_vel_aug[48]; - real_T rtb_of_omega_aug[48]; - kfl_msg rtb_Merge2; - ase_cov_datatype rtb_Assignment6[90]; - ase_cov_datatype rtb_Product2_b[90]; - ase_cov_datatype rtb_Product3[576]; - kfl_msg rtb_BusAssignment_a; - real32_T rtb_Product_j[6]; - real32_T rtb_H_out[702]; - real32_T rtb_R_mat[36]; - real32_T rtb_hr_global_landmarks[150]; - real32_T rtb_Assignment1[180]; - real32_T rtb_MathFunction2[180]; - real32_T rtb_ex_matrix_multiply4[225]; - real_T ml_vel_aug[3]; - real_T ml_omega_aug[3]; - uint8_T numFeatures; - real32_T hr_P_hr_ISS_ISS[3]; - real_T of_vel_aug[48]; - real_T of_omega_aug[48]; - real32_T UnitDelay_DSTATE_n_V_B_ISS_ISS[3]; - real32_T UnitDelay_DSTATE_n_accel_bias[3]; - real32_T UnitDelay_DSTATE_n_ml_quat_ISS2[4]; - real32_T UnitDelay_DSTATE_n_ml_P_cam_ISS[3]; - real32_T UnitDelay_DSTATE_n_of_quat_ISS2[64]; - real32_T UnitDelay_DSTATE_n_of_P_cam_ISS[48]; - ase_cov_datatype UnitDelay_DSTATE_n_cov_diag[117]; - uint16_T UnitDelay_DSTATE_n_kfl_status; - real_T UnitDelay_DSTATE_n_of_mahal_dis[50]; - real_T UnitDelay_DSTATE_n_ml_mahal_dis[50]; - real32_T UnitDelay_DSTATE_n_P_EST_ISS_IS[3]; - uint32_T UnitDelay_DSTATE_aug_state_enum; - int32_T i; - real_T rtb_ml_vel_aug_0[3]; - real32_T rtb_Compare_2[4]; - real_T tmp[3]; - real32_T rtb_Switch1_0[2]; - real32_T tmp_0[2]; - real32_T rtb_Switch1_1[2]; - real32_T H[4]; - int32_T rtb_valid_out_0[16]; - uint32_T tmp_1[17]; - real32_T tmp_2[117]; - real32_T rtb_VectorConcatenate_l[16]; - real32_T rtb_Product1_0[9]; - real32_T rtb_Assignment_o[9]; - real32_T tmp_3[9]; - real32_T rtb_Assignment_l[9]; - real32_T rtb_Product1_1[12]; - real32_T rtb_Product1_2[12]; - int32_T br; - real32_T tmp_4[9]; - real32_T tmp_5[9]; - int32_T i_0; - real32_T tmp_6[9]; - real32_T h[6]; - real32_T tmp_7[9]; - real32_T temp_0[6]; - real32_T temp_1[6]; - real32_T temp_2[6]; - real32_T tmp_8[12]; - real32_T v_0[18]; - real32_T S_0[18]; - real_T tmp_9; - real32_T tmp_a[9]; - real32_T tmp_b[9]; - ase_cov_datatype Switch1[90]; - real32_T tmp_c[9]; - real32_T tmp_d[9]; - real32_T rtb_Merge2_0[60]; - real_T rtb_VectorConcatenate_o[4]; - real32_T tmp_e[9]; - real32_T rtb_BusAssignment_h[48]; - real32_T M_0[126]; - real32_T M_1[126]; - real32_T M_2[540]; - real32_T tmp_f[9]; - real32_T rtb_Merge2_1[9]; - int8_T tmp_g[6]; - int8_T b_data[12]; - int8_T c_data[12]; - int8_T valid_indx_mat_data[102]; - int8_T c_data_0[17]; - int8_T f_data[90]; - int8_T rot_indices_data[63]; - int8_T c_data_1[63]; - real32_T C_data[63]; - int16_T h_data[1600]; - int32_T x_sizes[3]; - real32_T a_data[96]; - real32_T c_a_data[189]; - real32_T newerr_data[200]; - real32_T r_data[150]; - real32_T landmark_error_rail_frame_data[150]; - real_T r_vec_data[150]; - real32_T H_data[900]; - real32_T T_H_data[900]; - int16_T p_data[150]; - real32_T d_result_data[200]; - real32_T y_data[150]; - real32_T b_a_data[900]; - real32_T camera_landmarks_data[150]; - real32_T z_est_data[100]; - real32_T r_data_0[100]; - real32_T next_landmarks_data[150]; - real32_T omega_error_data[100]; - real_T r_vec_data_0[100]; - real32_T H_data_0[600]; - int32_T H_sizes[2]; - boolean_T invalid_vec_data[50]; - boolean_T invalid_data[100]; - int32_T q1_sizes[2]; - real32_T T_H_data_0[600]; - int32_T T_H_sizes[2]; - int8_T e_data[50]; - real32_T varargin_1_data[150]; - real32_T y_data_0[100]; - boolean_T g_result_data[100]; - real32_T a_data_0[600]; - int32_T of_measured_in_sizes[3]; - real32_T A_data[96]; - int8_T valid_indx_mat_data_0[102]; - real32_T b_data_0[32]; - boolean_T d_1; - int32_T C_sizes_idx_1; - int32_T O_sizes_idx_0; - int32_T z_est_sizes_idx_1; - int8_T c_sz_idx_0; - int16_T tmp_h; - real32_T UnitDelay_DSTATE_n_P_B_ISS_ISS_; - real32_T UnitDelay_DSTATE_n_A_B_ISS_ISS_; - real32_T UnitDelay_DSTATE_n_P_B_ISS_IS_0; - real32_T UnitDelay_DSTATE_n_hr_P_hr_ISS_; - real32_T UnitDelay_DSTATE_n_hr_P_hr_IS_0; - real32_T UnitDelay_DSTATE_n_hr_P_hr_IS_1; - real32_T UnitDelay_DSTATE_n_hr_quat_ISS2; - real32_T UnitDelay_DSTATE_n_hr_quat_IS_0; - real32_T UnitDelay_DSTATE_n_hr_quat_IS_1; - real32_T UnitDelay_DSTATE_n_hr_quat_IS_2; - real32_T hr_quat_ISS2hr_idx_0; - real32_T hr_quat_ISS2hr_idx_1; - real32_T hr_quat_ISS2hr_idx_2; - real32_T hr_quat_ISS2hr_idx_3; - boolean_T exitg1; - boolean_T exitg2; - - // Outputs for Atomic SubSystem: '/est_estimator' - // UnitDelay: '/Unit Delay20' - rtb_ml_vel_aug[0] = est_estimator_DW->UnitDelay20_DSTATE[0]; - - // UnitDelay: '/Unit Delay21' - rtb_ml_omega_aug[0] = est_estimator_DW->UnitDelay21_DSTATE[0]; - - // UnitDelay: '/Unit Delay20' - rtb_ml_vel_aug[1] = est_estimator_DW->UnitDelay20_DSTATE[1]; - - // UnitDelay: '/Unit Delay21' - rtb_ml_omega_aug[1] = est_estimator_DW->UnitDelay21_DSTATE[1]; - - // UnitDelay: '/Unit Delay20' - rtb_ml_vel_aug[2] = est_estimator_DW->UnitDelay20_DSTATE[2]; - - // UnitDelay: '/Unit Delay21' - rtb_ml_omega_aug[2] = est_estimator_DW->UnitDelay21_DSTATE[2]; - - // UnitDelay: '/Unit Delay22' - memcpy(&rtb_of_vel_aug[0], &est_estimator_DW->UnitDelay22_DSTATE[0], (uint32_T) - (48U * sizeof(real_T))); - - // UnitDelay: '/Unit Delay23' - memcpy(&rtb_of_omega_aug[0], &est_estimator_DW->UnitDelay23_DSTATE[0], - (uint32_T)(48U * sizeof(real_T))); - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - - memcpy(&est_estimator_B->Assignment[0], &est_estimator_P->Constant2_Value_ad[0], - (uint32_T)(13689U * sizeof(real32_T))); - - // SignalConversion: '/TmpSignal ConversionAtAssignmentInport2' incorporates: - // Constant: '/Constant4' - // Constant: '/Constant5' - // Gain: '/Gain' - - for (i = 0; i < 15; i++) { - tmp_2[i] = est_estimator_P->tun_ase_state_ic_cov_diag[i]; - } - - for (i = 0; i < 102; i++) { - tmp_2[(int32_T)(i + 15)] = est_estimator_P->tun_aug_ic_cov * - est_estimator_P->Constant5_Value_a[i]; - } - - // Assignment: '/Assignment' incorporates: - // SignalConversion: '/TmpSignal ConversionAtAssignmentInport2' - - for (i = 0; i < 117; i++) { - est_estimator_B->Assignment[(int32_T)(118 * i)] = tmp_2[i]; - } - - // Delay: '/Delay' - if ((int32_T)est_estimator_DW->icLoad != 0) { - memcpy(&est_estimator_DW->Delay_DSTATE[0], &est_estimator_B->Assignment[0], - (uint32_T)(13689U * sizeof(ase_cov_datatype))); - } - - // UnitDelay: '/Unit Delay18' - memcpy(&rtb_UnitDelay18[0], &est_estimator_DW->UnitDelay18_DSTATE[0], - (uint32_T)(50U * sizeof(real_T))); - - // UnitDelay: '/Unit Delay19' - memcpy(&rtb_UnitDelay19[0], &est_estimator_DW->UnitDelay19_DSTATE[0], - (uint32_T)(50U * sizeof(real_T))); - - // UnitDelay: '/Unit Delay25' - rtb_UnitDelay25[0] = est_estimator_DW->UnitDelay25_DSTATE[0]; - rtb_UnitDelay25[1] = est_estimator_DW->UnitDelay25_DSTATE[1]; - rtb_UnitDelay25[2] = est_estimator_DW->UnitDelay25_DSTATE[2]; - rtb_UnitDelay25[3] = est_estimator_DW->UnitDelay25_DSTATE[3]; - - // BusCreator: '/Bus Creator2' incorporates: - // UnitDelay: '/Unit Delay16' - // UnitDelay: '/Unit Delay17' - - numFeatures = est_estimator_DW->UnitDelay16_DSTATE; - rtb_BusCreator2_update_ML_featu = est_estimator_DW->UnitDelay17_DSTATE; - - // Sum: '/Sum of Elements2' incorporates: - // Inport: '/handrail_msg' - - UnitDelay_DSTATE_aug_state_enum = (uint32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[0]; - for (ar = 0; ar < 49; ar++) { - UnitDelay_DSTATE_aug_state_enum += (uint32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[(int32_T)(ar + 1)]; - } - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - // Sum: '/Sum of Elements2' - - rtb_Compare = ((uint8_T)UnitDelay_DSTATE_aug_state_enum >= - est_estimator_P->Constant_Value_oq); - - // Logic: '/Logical Operator8' incorporates: - // Inport: '/handrail_msg' - - empty_non_axis_sizes = ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag - [0] != 0); - for (handrail_knowledge_dims = 0; handrail_knowledge_dims < 49; - handrail_knowledge_dims++) { - empty_non_axis_sizes = (empty_non_axis_sizes || ((int32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[(int32_T) - (handrail_knowledge_dims + 1)] != 0)); - } - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // Inport: '/cmc_msg' - // SignalConversion: '/Signal Conversion' - // UnitDelay: '/Delay Input1' - - rtb_FixPtRelationalOperator_n = - (est_estimator_U_cmc_msg_o->localization_mode_cmd != - est_estimator_DW->DelayInput1_DSTATE_k); - - // Switch: '/Switch' incorporates: - // S-Function (sfix_bitop): '/FixPt Bitwise Operator3' - // S-Function (sfix_bitop): '/FixPt Bitwise Operator4' - // S-Function (sfix_bitop): '/FixPt Bitwise Operator5' - // UnitDelay: '/Unit Delay9' - - if (rtb_FixPtRelationalOperator_n) { - rtb_BitwiseOperator = (uint32_T)~(uint32_T)((uint32_T) - ~est_estimator_DW->UnitDelay9_DSTATE | - est_estimator_P->FixPtBitwiseOperator3_BitMask); - } else { - rtb_BitwiseOperator = est_estimator_DW->UnitDelay9_DSTATE; - } - - // End of Switch: '/Switch' - - // BusAssignment: '/Bus Assignment' - rtb_BusAssignment_aug_state_enu = rtb_BitwiseOperator; - - // Constant: '/Constant3' - rtb_VectorConcatenate[0] = est_estimator_P->Constant3_Value_c4; - - // BusAssignment: '/Bus Assignment' incorporates: - // UnitDelay: '/Unit Delay24' - - rtb_Add_e[0] = est_estimator_DW->UnitDelay24_DSTATE[0]; - rtb_Add_e[1] = est_estimator_DW->UnitDelay24_DSTATE[1]; - rtb_Add_e[2] = est_estimator_DW->UnitDelay24_DSTATE[2]; - - // Gain: '/Gain' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[1] = est_estimator_P->Gain_Gain_n1 * (real32_T) - est_estimator_P->Constant_Value_l[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[2] = (real32_T)est_estimator_P->Constant_Value_l[1]; - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[3] = est_estimator_P->Gain1_Gain_au * (real32_T) - est_estimator_P->Constant_Value_l[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[4] = (real32_T)est_estimator_P->Constant_Value_l[2]; - - // Constant: '/Constant2' - rtb_VectorConcatenate[5] = est_estimator_P->Constant2_Value_am; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[6] = est_estimator_P->Gain2_Gain_a5 * (real32_T) - est_estimator_P->Constant_Value_l[0]; - - // Gain: '/Gain3' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[7] = est_estimator_P->Gain3_Gain_m * (real32_T) - est_estimator_P->Constant_Value_l[1]; - - // Gain: '/Gain4' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[8] = est_estimator_P->Gain4_Gain_h * (real32_T) - est_estimator_P->Constant_Value_l[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[9] = (real32_T)est_estimator_P->Constant_Value_l[0]; - - // Constant: '/Constant1' - rtb_VectorConcatenate[10] = est_estimator_P->Constant1_Value_p; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[11] = est_estimator_P->Gain5_Gain_a * (real32_T) - est_estimator_P->Constant_Value_l[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[12] = (real32_T)est_estimator_P->Constant_Value_l[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[13] = (real32_T)est_estimator_P->Constant_Value_l[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate[14] = (real32_T)est_estimator_P->Constant_Value_l[2]; - - // Constant: '/Constant' - rtb_VectorConcatenate[15] = est_estimator_P->Constant_Value_d; - - // Constant: '/Constant3' - rtb_VectorConcatenate_hq[0] = est_estimator_P->Constant3_Value_dg; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum_k1 = est_estimator_P->tun_abp_quat_body2imu[3] * - est_estimator_P->tun_abp_quat_body2imu[3] * est_estimator_P->Gain_Gain_g0 - - (real32_T)est_estimator_P->Constant1_Value_mb; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_a[i]; - } - - rtb_Assignment_b[0] = rtb_Sum_k1; - rtb_Assignment_b[4] = rtb_Sum_k1; - rtb_Assignment_b[8] = rtb_Sum_k1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant' - - rtb_Sum_k1 = est_estimator_P->Gain1_Gain_jx * - est_estimator_P->tun_abp_quat_body2imu[3]; - - // Product: '/Product' incorporates: - // Constant: '/Constant' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Product1_0[0] = (real32_T)est_estimator_P->Constant3_Value_k; - rtb_Product1_0[1] = est_estimator_P->tun_abp_quat_body2imu[2]; - rtb_Product1_0[2] = est_estimator_P->Gain_Gain_e * - est_estimator_P->tun_abp_quat_body2imu[1]; - rtb_Product1_0[3] = est_estimator_P->Gain1_Gain_nf * - est_estimator_P->tun_abp_quat_body2imu[2]; - rtb_Product1_0[4] = (real32_T)est_estimator_P->Constant3_Value_k; - rtb_Product1_0[5] = est_estimator_P->tun_abp_quat_body2imu[0]; - rtb_Product1_0[6] = est_estimator_P->tun_abp_quat_body2imu[1]; - rtb_Product1_0[7] = est_estimator_P->Gain2_Gain_h5 * - est_estimator_P->tun_abp_quat_body2imu[0]; - rtb_Product1_0[8] = (real32_T)est_estimator_P->Constant3_Value_k; - - // Math: '/Math Function' incorporates: - // Constant: '/Constant' - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Assignment_o[i] = est_estimator_P->tun_abp_quat_body2imu[0] * - est_estimator_P->tun_abp_quat_body2imu[i]; - rtb_Assignment_o[(int32_T)(i + 3)] = est_estimator_P->tun_abp_quat_body2imu - [1] * est_estimator_P->tun_abp_quat_body2imu[i]; - rtb_Assignment_o[(int32_T)(i + 6)] = est_estimator_P->tun_abp_quat_body2imu - [2] * est_estimator_P->tun_abp_quat_body2imu[i]; - } - - // End of Math: '/Math Function' - for (i = 0; i < 3; i++) { - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - rtb_Assignment_n[(int32_T)(3 * i)] = (rtb_Assignment_b[i] - rtb_Sum_k1 * - rtb_Product1_0[i]) + rtb_Assignment_o[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_gg; - rtb_Assignment_n[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 3)] - rtb_Product1_0[(int32_T)(i + 3)] * rtb_Sum_k1) + - rtb_Assignment_o[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_gg; - rtb_Assignment_n[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 6)] - rtb_Product1_0[(int32_T)(i + 6)] * rtb_Sum_k1) + - rtb_Assignment_o[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_gg; - - // Sum: '/Sum1' incorporates: - // Constant: '/Constant1' - // Inport: '/imu_msg' - // Product: '/Product' - - rtb_P_B_ISS_ISS[i] = est_estimator_U_imu_msg_c->imu_omega_B_ECI_sensor[i] - - est_estimator_P->ase_gyro_fixed_bias[i]; - } - - // Product: '/Product' - for (i = 0; i < 3; i++) { - rtb_Sum1_k3[i] = rtb_Assignment_n[(int32_T)(i + 6)] * rtb_P_B_ISS_ISS[2] + - (rtb_Assignment_n[(int32_T)(i + 3)] * rtb_P_B_ISS_ISS[1] + - rtb_Assignment_n[i] * rtb_P_B_ISS_ISS[0]); - } - - // Saturate: '/Saturation' incorporates: - // Inport: '/cmc_msg' - // SignalConversion: '/Signal Conversion' - - if (est_estimator_U_cmc_msg_o->speed_gain_cmd > - est_estimator_P->fam_impeller_speeds_cnt) { - rtb_Saturation_n = est_estimator_P->fam_impeller_speeds_cnt; - } else if (est_estimator_U_cmc_msg_o->speed_gain_cmd < - est_estimator_P->Saturation_LowerSat_n) { - rtb_Saturation_n = est_estimator_P->Saturation_LowerSat_n; - } else { - rtb_Saturation_n = est_estimator_U_cmc_msg_o->speed_gain_cmd; - } - - // End of Saturate: '/Saturation' - - // Outputs for Iterator SubSystem: '/filter' incorporates: - // ForEach: '/For Each' - - for (ForEach_itr_g = 0; ForEach_itr_g < 3; ForEach_itr_g++) { - // MATLAB Function: '/MATLAB Function1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant2' - // Selector: '/Selector' - - est_estimator_MATLABFunction1(est_estimator_P->astrobee_fsw_step_size, - est_estimator_P->fam_impeller_speeds[(int32_T)((int32_T)rtb_Saturation_n - - 1)], &est_estimator_B->CoreSubsys[ForEach_itr_g].sf_MATLABFunction1, - &est_estimator_DW->CoreSubsys[ForEach_itr_g].sf_MATLABFunction1); - - // DiscreteTransferFcn: '/Discrete Transfer Fcn' incorporates: - // DiscreteTransferFcn: '/3 Hz Low Pass' - - rtb_Sum_k1 = (est_estimator_P->CoreSubsys.uHzLowPass_NumCoef * - est_estimator_DW->CoreSubsys[ForEach_itr_g].uHzLowPass_states - - est_estimator_B->CoreSubsys[ForEach_itr_g]. - sf_MATLABFunction1.den_out[1] * est_estimator_DW-> - CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0]) - - est_estimator_B->CoreSubsys[ForEach_itr_g].sf_MATLABFunction1.den_out[2] * - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[1]; - rtb_Sum_l = (est_estimator_B->CoreSubsys[ForEach_itr_g]. - sf_MATLABFunction1.num_out[0] * rtb_Sum_k1 + - est_estimator_B->CoreSubsys[ForEach_itr_g]. - sf_MATLABFunction1.num_out[1] * est_estimator_DW-> - CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0]) + - est_estimator_B->CoreSubsys[ForEach_itr_g].sf_MATLABFunction1.num_out[2] * - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[1]; - - // Update for DiscreteTransferFcn: '/3 Hz Low Pass' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_In1_at_outport_0' - - est_estimator_DW->CoreSubsys[ForEach_itr_g].uHzLowPass_states = - (rtb_Sum1_k3[ForEach_itr_g] - - est_estimator_P->CoreSubsys.uHzLowPass_DenCoef[1] * - est_estimator_DW->CoreSubsys[ForEach_itr_g].uHzLowPass_states) / - est_estimator_P->CoreSubsys.uHzLowPass_DenCoef[0]; - - // Update for DiscreteTransferFcn: '/Discrete Transfer Fcn' - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[1] = - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0]; - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0] = - rtb_Sum_k1; - - // ForEachSliceAssignment: '/ImpAsg_InsertedFor_Out1_at_inport_0' - rtb_ImpAsg_InsertedFor_Out1_a_d[ForEach_itr_g] = rtb_Sum_l; - } - - // End of Outputs for SubSystem: '/filter' - - // Sum: '/Sum' incorporates: - // UnitDelay: '/Unit Delay3' - - rtb_ImpAsg_InsertedFor_Out1_a_d[0] -= est_estimator_DW->UnitDelay3_DSTATE[0]; - rtb_ImpAsg_InsertedFor_Out1_a_d[1] -= est_estimator_DW->UnitDelay3_DSTATE[1]; - hr_quat_ISS2hr_idx_0 = rtb_ImpAsg_InsertedFor_Out1_a_d[2] - - est_estimator_DW->UnitDelay3_DSTATE[2]; - - // Gain: '/Gain' - rtb_VectorConcatenate_hq[1] = est_estimator_P->Gain_Gain_co * - hr_quat_ISS2hr_idx_0; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' - rtb_VectorConcatenate_hq[2] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - - // Gain: '/Gain1' - rtb_VectorConcatenate_hq[3] = est_estimator_P->Gain1_Gain_cz * - rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' - rtb_VectorConcatenate_hq[4] = hr_quat_ISS2hr_idx_0; - - // Constant: '/Constant2' - rtb_VectorConcatenate_hq[5] = est_estimator_P->Constant2_Value_c; - - // Gain: '/Gain2' - rtb_VectorConcatenate_hq[6] = est_estimator_P->Gain2_Gain_g4 * - rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - - // Gain: '/Gain3' - rtb_VectorConcatenate_hq[7] = est_estimator_P->Gain3_Gain_fu * - rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - - // Gain: '/Gain4' - rtb_VectorConcatenate_hq[8] = est_estimator_P->Gain4_Gain_g * - rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' - rtb_VectorConcatenate_hq[9] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - - // Constant: '/Constant1' - rtb_VectorConcatenate_hq[10] = est_estimator_P->Constant1_Value_o; - - // Gain: '/Gain5' - rtb_VectorConcatenate_hq[11] = est_estimator_P->Gain5_Gain_jr * - hr_quat_ISS2hr_idx_0; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' - rtb_VectorConcatenate_hq[12] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' - rtb_VectorConcatenate_hq[13] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' - rtb_VectorConcatenate_hq[14] = hr_quat_ISS2hr_idx_0; - - // Constant: '/Constant' - rtb_VectorConcatenate_hq[15] = est_estimator_P->Constant_Value_lw; - - // Product: '/Product2' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Constant: '/Constant' - // Product: '/Product' - // Sum: '/Add' - - for (i = 0; i < 16; i++) { - rtb_Product2[i] = (est_estimator_P->Constant3_Value_dc * - rtb_VectorConcatenate[i] * (real32_T) - est_estimator_P->ase_ts + rtb_VectorConcatenate_hq[i]) * - est_estimator_P->Constant1_Value_i3 * (real32_T)est_estimator_P->ase_ts; - } - - // End of Product: '/Product2' - - // MATLAB Function: '/MATLAB Function' - est_estimator_MATLABFunction(rtb_Product2, &est_estimator_B->sf_MATLABFunction); - - // Product: '/Product5' incorporates: - // Constant: '/Constant' - - hr_quat_ISS2hr_idx_1 = (real32_T)est_estimator_P->ase_ts * (real32_T) - est_estimator_P->ase_ts * (real32_T)est_estimator_P->ase_ts; - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m < 4; b_m++) { - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_Product2[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_l[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_Product2[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_hq[(int32_T)(i << 2)] * rtb_VectorConcatenate[b_m]; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_l[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)(i << 2)] * rtb_VectorConcatenate_hq[b_m]; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_Product2[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate[(int32_T)(b_m + 4)]; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_l[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate_hq[(int32_T)(b_m + 4)]; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_Product2[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate[(int32_T)(b_m + 8)]; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_l[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate_hq[(int32_T)(b_m + 8)]; - - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_Product2[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate[(int32_T)(b_m + 12)]; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_l[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate_hq[(int32_T)(b_m + 12)]; - } - } - - // Sum: '/Add1' incorporates: - // Constant: '/Constant2' - // Product: '/Product1' - // Product: '/Product5' - // Sum: '/Add2' - - for (i = 0; i < 4; i++) { - rtb_VectorConcatenate[(int32_T)(i << 2)] = (rtb_Product2[(int32_T)(i << 2)] - - rtb_VectorConcatenate_l[(int32_T)(i << 2)]) * hr_quat_ISS2hr_idx_1 / - est_estimator_P->Constant2_Value_j0 + est_estimator_B-> - sf_MATLABFunction.y[(int32_T)(i << 2)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(i << 2))] = (rtb_Product2 - [(int32_T)((int32_T)(i << 2) + 1)] - rtb_VectorConcatenate_l[(int32_T) - ((int32_T)(i << 2) + 1)]) * hr_quat_ISS2hr_idx_1 / - est_estimator_P->Constant2_Value_j0 + est_estimator_B-> - sf_MATLABFunction.y[(int32_T)((int32_T)(i << 2) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(i << 2))] = (rtb_Product2 - [(int32_T)((int32_T)(i << 2) + 2)] - rtb_VectorConcatenate_l[(int32_T) - ((int32_T)(i << 2) + 2)]) * hr_quat_ISS2hr_idx_1 / - est_estimator_P->Constant2_Value_j0 + est_estimator_B-> - sf_MATLABFunction.y[(int32_T)((int32_T)(i << 2) + 2)]; - rtb_VectorConcatenate[(int32_T)(3 + (int32_T)(i << 2))] = (rtb_Product2 - [(int32_T)((int32_T)(i << 2) + 3)] - rtb_VectorConcatenate_l[(int32_T) - ((int32_T)(i << 2) + 3)]) * hr_quat_ISS2hr_idx_1 / - est_estimator_P->Constant2_Value_j0 + est_estimator_B-> - sf_MATLABFunction.y[(int32_T)((int32_T)(i << 2) + 3)]; - } - - // End of Sum: '/Add1' - - // Product: '/Product1' incorporates: - // UnitDelay: '/Unit Delay1' - - for (i = 0; i < 4; i++) { - rtb_Sum_k1 = rtb_VectorConcatenate[(int32_T)(i + 12)] * - est_estimator_DW->UnitDelay1_DSTATE[3] + (rtb_VectorConcatenate[(int32_T) - (i + 8)] * est_estimator_DW->UnitDelay1_DSTATE[2] + - (rtb_VectorConcatenate[(int32_T)(i + 4)] * - est_estimator_DW->UnitDelay1_DSTATE[1] + rtb_VectorConcatenate[i] * - est_estimator_DW->UnitDelay1_DSTATE[0])); - rtb_Product1[i] = rtb_Sum_k1; - } - - // If: '/If' incorporates: - // Inport: '/In1' - - if (rtb_Product1[3] < 0.0F) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize(rtb_Product1, rtb_Merge_o, - (P_Normalize_est_estimator_T *)&est_estimator_P->Normalize); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Merge_o[0] = rtb_Product1[0]; - rtb_Merge_o[1] = rtb_Product1[1]; - rtb_Merge_o[2] = rtb_Product1[2]; - rtb_Merge_o[3] = rtb_Product1[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Sum_k1 = (real32_T)sqrt((real_T)(((rtb_Merge_o[0] * rtb_Merge_o[0] + - rtb_Merge_o[1] * rtb_Merge_o[1]) + rtb_Merge_o[2] * rtb_Merge_o[2]) + - rtb_Merge_o[3] * rtb_Merge_o[3])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_Sum_k1 > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize_p(rtb_Merge_o, rtb_Sum_k1, rtb_Product1); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Product1[0] = rtb_Merge_o[0]; - rtb_Product1[1] = rtb_Merge_o[1]; - rtb_Product1[2] = rtb_Merge_o[2]; - rtb_Product1[3] = rtb_Merge_o[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum_k1 = rtb_Product1[3] * rtb_Product1[3] * est_estimator_P->Gain_Gain_c0 - - (real32_T)est_estimator_P->Constant1_Value_c; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_n[i] = (real32_T)est_estimator_P->Constant2_Value_ja[i]; - } - - rtb_Assignment_n[0] = rtb_Sum_k1; - rtb_Assignment_n[4] = rtb_Sum_k1; - rtb_Assignment_n[8] = rtb_Sum_k1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' - rtb_Sum_k1 = est_estimator_P->Gain1_Gain_il * rtb_Product1[3]; - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Assignment_ef[0] = (real32_T)est_estimator_P->Constant3_Value_c; - rtb_Assignment_ef[1] = rtb_Product1[2]; - rtb_Assignment_ef[2] = est_estimator_P->Gain_Gain_lj * rtb_Product1[1]; - rtb_Assignment_ef[3] = est_estimator_P->Gain1_Gain_lb * rtb_Product1[2]; - rtb_Assignment_ef[4] = (real32_T)est_estimator_P->Constant3_Value_c; - rtb_Assignment_ef[5] = rtb_Product1[0]; - rtb_Assignment_ef[6] = rtb_Product1[1]; - rtb_Assignment_ef[7] = est_estimator_P->Gain2_Gain_m4 * rtb_Product1[0]; - rtb_Assignment_ef[8] = (real32_T)est_estimator_P->Constant3_Value_c; - - // Math: '/Math Function' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product' - // Product: '/Product1' - // Sum: '/Sum1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = rtb_Product1[0] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 3)] = rtb_Product1[1] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 6)] = rtb_Product1[2] * rtb_Product1[i]; - } - - for (i = 0; i < 3; i++) { - rtb_Assignment_b[(int32_T)(3 * i)] = (rtb_Assignment_n[i] - rtb_Sum_k1 * - rtb_Assignment_ef[i]) + rtb_Product1_0[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_j; - rtb_Assignment_b[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_n - [(int32_T)(i + 3)] - rtb_Assignment_ef[(int32_T)(i + 3)] * rtb_Sum_k1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_j; - rtb_Assignment_b[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_n - [(int32_T)(i + 6)] - rtb_Assignment_ef[(int32_T)(i + 6)] * rtb_Sum_k1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_j; - } - - // End of Math: '/Math Function' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Sum_k1 = est_estimator_P->tun_abp_quat_body2imu[3] * - est_estimator_P->tun_abp_quat_body2imu[3] * est_estimator_P->Gain_Gain_p - - (real32_T)est_estimator_P->Constant1_Value_l; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_n[i] = (real32_T)est_estimator_P->Constant2_Value_bl[i]; - } - - rtb_Assignment_n[0] = rtb_Sum_k1; - rtb_Assignment_n[4] = rtb_Sum_k1; - rtb_Assignment_n[8] = rtb_Sum_k1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant2' - - rtb_Sum_k1 = est_estimator_P->Gain1_Gain_fe * - est_estimator_P->tun_abp_quat_body2imu[3]; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant3' - // Constant: '/Constant4' - // Constant: '/Constant6' - // Product: '/Product' - - if (est_estimator_P->tun_ase_gravity_removal > - est_estimator_P->Switch_Threshold) { - // Gain: '/Gain1' incorporates: - // Inport: '/true_q_ISS2B' - - rtb_Gain1_f = est_estimator_P->Gain1_Gain_d * est_estimator_U_Q_ISS2B[3]; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Inport: '/true_q_ISS2B' - // Math: '/Math Function' - - rtb_Sum_l = est_estimator_U_Q_ISS2B[3] * est_estimator_U_Q_ISS2B[3] * - est_estimator_P->Gain_Gain_l - (real32_T) - est_estimator_P->Constant1_Value_k; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_ef[i] = (real32_T)est_estimator_P->Constant2_Value_p[i]; - } - - rtb_Assignment_ef[0] = rtb_Sum_l; - rtb_Assignment_ef[4] = rtb_Sum_l; - rtb_Assignment_ef[8] = rtb_Sum_l; - - // End of Assignment: '/Assignment' - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - // Inport: '/true_q_ISS2B' - - rtb_Assignment_l[0] = (real32_T)est_estimator_P->Constant3_Value_i; - rtb_Assignment_l[1] = est_estimator_U_Q_ISS2B[2]; - rtb_Assignment_l[2] = est_estimator_P->Gain_Gain * est_estimator_U_Q_ISS2B[1]; - rtb_Assignment_l[3] = est_estimator_P->Gain1_Gain * est_estimator_U_Q_ISS2B - [2]; - rtb_Assignment_l[4] = (real32_T)est_estimator_P->Constant3_Value_i; - rtb_Assignment_l[5] = est_estimator_U_Q_ISS2B[0]; - rtb_Assignment_l[6] = est_estimator_U_Q_ISS2B[1]; - rtb_Assignment_l[7] = est_estimator_P->Gain2_Gain_e * - est_estimator_U_Q_ISS2B[0]; - rtb_Assignment_l[8] = (real32_T)est_estimator_P->Constant3_Value_i; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Inport: '/true_q_ISS2B' - // Math: '/Math Function1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = est_estimator_U_Q_ISS2B[i] * est_estimator_U_Q_ISS2B[0]; - rtb_Product1_0[(int32_T)(i + 3)] = est_estimator_U_Q_ISS2B[i] * - est_estimator_U_Q_ISS2B[1]; - rtb_Product1_0[(int32_T)(i + 6)] = est_estimator_U_Q_ISS2B[i] * - est_estimator_U_Q_ISS2B[2]; - } - - // End of Product: '/Product1' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Assignment_o[(int32_T)(3 * i)] = (rtb_Assignment_ef[(int32_T)(3 * i)] - - rtb_Assignment_l[(int32_T)(3 * i)] * rtb_Gain1_f) + rtb_Product1_0 - [(int32_T)(3 * i)] * est_estimator_P->Gain2_Gain; - rtb_Assignment_o[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_ef - [(int32_T)((int32_T)(3 * i) + 1)] - rtb_Assignment_l[(int32_T)((int32_T) - (3 * i) + 1)] * rtb_Gain1_f) + rtb_Product1_0[(int32_T)((int32_T)(3 * i) - + 1)] * est_estimator_P->Gain2_Gain; - rtb_Assignment_o[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_ef - [(int32_T)((int32_T)(3 * i) + 2)] - rtb_Assignment_l[(int32_T)((int32_T) - (3 * i) + 2)] * rtb_Gain1_f) + rtb_Product1_0[(int32_T)((int32_T)(3 * i) - + 2)] * est_estimator_P->Gain2_Gain; - } - - // End of Sum: '/Sum1' - for (i = 0; i < 3; i++) { - rtb_Sum1_k3[i] = rtb_Assignment_o[(int32_T)(i + 6)] * - est_estimator_P->tun_ase_gravity_accel[2] + (rtb_Assignment_o[(int32_T) - (i + 3)] * est_estimator_P->tun_ase_gravity_accel[1] + - rtb_Assignment_o[i] * est_estimator_P->tun_ase_gravity_accel[0]); - } - } else { - rtb_Sum1_k3[0] = est_estimator_P->Constant6_Value[0]; - rtb_Sum1_k3[1] = est_estimator_P->Constant6_Value[1]; - rtb_Sum1_k3[2] = est_estimator_P->Constant6_Value[2]; - } - - // End of Switch: '/Switch' - - // Product: '/Product' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_3[0] = (real32_T)est_estimator_P->Constant3_Value_o; - tmp_3[1] = est_estimator_P->tun_abp_quat_body2imu[2]; - tmp_3[2] = est_estimator_P->Gain_Gain_az * - est_estimator_P->tun_abp_quat_body2imu[1]; - tmp_3[3] = est_estimator_P->Gain1_Gain_ln * - est_estimator_P->tun_abp_quat_body2imu[2]; - tmp_3[4] = (real32_T)est_estimator_P->Constant3_Value_o; - tmp_3[5] = est_estimator_P->tun_abp_quat_body2imu[0]; - tmp_3[6] = est_estimator_P->tun_abp_quat_body2imu[1]; - tmp_3[7] = est_estimator_P->Gain2_Gain_c * - est_estimator_P->tun_abp_quat_body2imu[0]; - tmp_3[8] = (real32_T)est_estimator_P->Constant3_Value_o; - - // Math: '/Math Function' incorporates: - // Constant: '/Constant2' - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = est_estimator_P->tun_abp_quat_body2imu[0] * - est_estimator_P->tun_abp_quat_body2imu[i]; - rtb_Product1_0[(int32_T)(i + 3)] = est_estimator_P->tun_abp_quat_body2imu[1] - * est_estimator_P->tun_abp_quat_body2imu[i]; - rtb_Product1_0[(int32_T)(i + 6)] = est_estimator_P->tun_abp_quat_body2imu[2] - * est_estimator_P->tun_abp_quat_body2imu[i]; - } - - // End of Math: '/Math Function' - for (i = 0; i < 3; i++) { - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - rtb_Assignment_l[(int32_T)(3 * i)] = (rtb_Assignment_n[i] - rtb_Sum_k1 * - tmp_3[i]) + rtb_Product1_0[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_i5; - rtb_Assignment_l[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_n - [(int32_T)(i + 3)] - tmp_3[(int32_T)(i + 3)] * rtb_Sum_k1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_i5; - rtb_Assignment_l[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_n - [(int32_T)(i + 6)] - tmp_3[(int32_T)(i + 6)] * rtb_Sum_k1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_i5; - - // Sum: '/Sum' incorporates: - // Constant: '/Constant5' - // Inport: '/imu_msg' - // Product: '/Product' - - rtb_P_B_ISS_ISS[i] = est_estimator_U_imu_msg_c->imu_A_B_ECI_sensor[i] - - est_estimator_P->ase_accel_fixed_bias[i]; - } - - // Sum: '/Sum2' incorporates: - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Sum2[i] = ((rtb_Assignment_l[(int32_T)(i + 3)] * rtb_P_B_ISS_ISS[1] + - rtb_Assignment_l[i] * rtb_P_B_ISS_ISS[0]) + - rtb_Assignment_l[(int32_T)(i + 6)] * rtb_P_B_ISS_ISS[2]) - - rtb_Sum1_k3[i]; - } - - // End of Sum: '/Sum2' - - // Outputs for Iterator SubSystem: '/filter_with_HP_filter' incorporates: - // ForEach: '/For Each' - - for (ForEach_itr = 0; ForEach_itr < 3; ForEach_itr++) { - // MATLAB Function: '/MATLAB Function1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant2' - // Selector: '/Selector' - - est_estimator_MATLABFunction1(est_estimator_P->astrobee_fsw_step_size, - est_estimator_P->fam_impeller_speeds[(int32_T)((int32_T)rtb_Saturation_n - - 1)], &est_estimator_B->CoreSubsys_l[ForEach_itr].sf_MATLABFunction1, - &est_estimator_DW->CoreSubsys_l[ForEach_itr].sf_MATLABFunction1); - - // DiscreteTransferFcn: '/Discrete Transfer Fcn' incorporates: - // DiscreteTransferFcn: '/3 Hz Low Pass' - - rtb_Sum_k1 = (est_estimator_P->CoreSubsys_l.uHzLowPass_NumCoef * - est_estimator_DW->CoreSubsys_l[ForEach_itr].uHzLowPass_states - - est_estimator_B->CoreSubsys_l[ForEach_itr]. - sf_MATLABFunction1.den_out[1] * est_estimator_DW-> - CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[0]) - - est_estimator_B->CoreSubsys_l[ForEach_itr].sf_MATLABFunction1.den_out[2] * - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[1]; - rtb_Switch1 = (est_estimator_B->CoreSubsys_l[ForEach_itr]. - sf_MATLABFunction1.num_out[0] * rtb_Sum_k1 + - est_estimator_B->CoreSubsys_l[ForEach_itr]. - sf_MATLABFunction1.num_out[1] * - est_estimator_DW->CoreSubsys_l[ForEach_itr]. - DiscreteTransferFcn_states[0]) + - est_estimator_B->CoreSubsys_l[ForEach_itr].sf_MATLABFunction1.num_out[2] * - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[1]; - - // DiscreteTransferFcn: '/High Pass Filter' incorporates: - // ForEachSliceSelector: '/ImpSel_InsertedFor_In1_at_outport_0' - - rtb_Sum_l = (rtb_Sum2[ForEach_itr] - est_estimator_P->tun_grav_hp_den[1] * - est_estimator_DW->CoreSubsys_l[ForEach_itr]. - HighPassFilter_states) / est_estimator_P->tun_grav_hp_den[0]; - - // Switch: '/Switch' incorporates: - // Constant: '/Constant1' - // DiscreteTransferFcn: '/High Pass Filter' - // ForEachSliceSelector: '/ImpSel_InsertedFor_In1_at_outport_0' - - if ((int32_T)est_estimator_P->tun_grav_hp_enable_f != 0) { - hr_quat_ISS2hr_idx_1 = est_estimator_P->tun_grav_hp_num[0] * rtb_Sum_l + - est_estimator_P->tun_grav_hp_num[1] * est_estimator_DW-> - CoreSubsys_l[ForEach_itr].HighPassFilter_states; - } else { - hr_quat_ISS2hr_idx_1 = rtb_Sum2[ForEach_itr]; - } - - // End of Switch: '/Switch' - - // Update for DiscreteTransferFcn: '/3 Hz Low Pass' - est_estimator_DW->CoreSubsys_l[ForEach_itr].uHzLowPass_states = - (hr_quat_ISS2hr_idx_1 - est_estimator_P->CoreSubsys_l.uHzLowPass_DenCoef[1] - * est_estimator_DW->CoreSubsys_l[ForEach_itr].uHzLowPass_states) / - est_estimator_P->CoreSubsys_l.uHzLowPass_DenCoef[0]; - - // Update for DiscreteTransferFcn: '/Discrete Transfer Fcn' - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[1] = - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[0]; - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[0] = - rtb_Sum_k1; - - // Update for DiscreteTransferFcn: '/High Pass Filter' - est_estimator_DW->CoreSubsys_l[ForEach_itr].HighPassFilter_states = - rtb_Sum_l; - - // ForEachSliceAssignment: '/ImpAsg_InsertedFor_Out1_at_inport_0' - rtb_ImpAsg_InsertedFor_Out1_at_[ForEach_itr] = rtb_Switch1; - } - - // End of Outputs for SubSystem: '/filter_with_HP_filter' - - // Sum: '/Sum1' incorporates: - // UnitDelay: '/Unit Delay6' - - rtb_ImpAsg_InsertedFor_Out1_at_[0] -= est_estimator_DW->UnitDelay6_DSTATE[0]; - rtb_ImpAsg_InsertedFor_Out1_at_[1] -= est_estimator_DW->UnitDelay6_DSTATE[1]; - rtb_Sum_l = rtb_ImpAsg_InsertedFor_Out1_at_[2] - - est_estimator_DW->UnitDelay6_DSTATE[2]; - rtb_ImpAsg_InsertedFor_Out1_at_[2] = rtb_Sum_l; - for (i = 0; i < 3; i++) { - // Product: '/Product' - rtb_Sum_k1 = rtb_Assignment_b[(int32_T)(i + 6)] * rtb_Sum_l + - (rtb_Assignment_b[(int32_T)(i + 3)] * rtb_ImpAsg_InsertedFor_Out1_at_[1] + - rtb_Assignment_b[i] * rtb_ImpAsg_InsertedFor_Out1_at_[0]); - - // Sum: '/Sum5' incorporates: - // Gain: '/Gain1' - // UnitDelay: '/Unit Delay4' - - rtb_Gain1_f = (real32_T)est_estimator_P->ase_ts * rtb_Sum_k1 + - est_estimator_DW->UnitDelay4_DSTATE[i]; - - // Sum: '/Sum2' incorporates: - // Gain: '/Gain' - // UnitDelay: '/Unit Delay26' - - rtb_P_B_ISS_ISS[i] = (real32_T)est_estimator_P->ase_ts * rtb_Gain1_f + - est_estimator_DW->UnitDelay26_DSTATE[i]; - - // Product: '/Product' - rtb_Sum1_k3[i] = rtb_Sum_k1; - - // Sum: '/Sum5' - rtb_Sum2[i] = rtb_Gain1_f; - } - - // S-Function (sfix_bitop): '/Bitwise Operator' incorporates: - // BusAssignment: '/Bus Assignment' - - rtb_BitwiseOperator &= est_estimator_P->BitwiseOperator_BitMask; - - // Sum: '/Sum of Elements1' incorporates: - // Inport: '/landmark_msg' - - UnitDelay_DSTATE_aug_state_enum = (uint32_T) - est_estimator_U_landmark_msg->cvs_valid_flag[0]; - for (ar = 0; ar < 49; ar++) { - UnitDelay_DSTATE_aug_state_enum += (uint32_T) - est_estimator_U_landmark_msg->cvs_valid_flag[(int32_T)(ar + 1)]; - } - - rtb_Switch_m = (uint8_T)UnitDelay_DSTATE_aug_state_enum; - - // End of Sum: '/Sum of Elements1' - - // Logic: '/Logical Operator1' incorporates: - // Inport: '/landmark_msg' - - rtb_LogicalOperator = ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[0] - != 0); - for (handrail_knowledge_dims = 0; handrail_knowledge_dims < 49; - handrail_knowledge_dims++) { - rtb_LogicalOperator = (rtb_LogicalOperator || ((int32_T) - est_estimator_U_landmark_msg->cvs_valid_flag[(int32_T) - (handrail_knowledge_dims + 1)] != 0)); - } - - // Logic: '/Logical Operator' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Inport: '/cmc_msg' - // Inport: '/landmark_msg' - // Logic: '/Logical Operator4' - // Logic: '/Logical Operator5' - // Logic: '/Logical Operator6' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator6' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/FixPt Relational Operator' - // RelationalOperator: '/FixPt Relational Operator' - // SignalConversion: '/Signal Conversion' - // UnitDelay: '/Delay Input1' - // UnitDelay: '/Delay Input1' - - rtb_LogicalOperator = ((((est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_map) && (rtb_Switch_m >= - est_estimator_P->Constant_Value_lf)) || ((rtb_Switch_m >= - est_estimator_P->Constant_Value_js) && - (est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_docking))) && - (((est_estimator_U_landmark_msg->cvs_timestamp_sec != - est_estimator_DW->DelayInput1_DSTATE_o) || - (est_estimator_U_landmark_msg->cvs_timestamp_nsec != - est_estimator_DW->DelayInput1_DSTATE_n)) && rtb_LogicalOperator) && - (rtb_BitwiseOperator != 0U)); - - // Logic: '/Logical Operator4' incorporates: - // Inport: '/optical_flow_msg' - // RelationalOperator: '/FixPt Relational Operator' - // RelationalOperator: '/FixPt Relational Operator' - // UnitDelay: '/Delay Input1' - // UnitDelay: '/Delay Input1' - - rtb_of_update_flag = - ((est_estimator_U_cvs_optical_flow_msg_n->cvs_timestamp_sec != - est_estimator_DW->DelayInput1_DSTATE_b) || - (est_estimator_U_cvs_optical_flow_msg_n->cvs_timestamp_nsec != - est_estimator_DW->DelayInput1_DSTATE_d)); - - // Outputs for Enabled SubSystem: '/Enabled Row-Wise SUM' incorporates: - // EnablePort: '/Enable' - - for (nx = 0; nx < 50; nx++) { - if (rtb_of_update_flag) { - // Sum: '/Sum of Elements3' incorporates: - // Inport: '/optical_flow_msg' - - UnitDelay_DSTATE_aug_state_enum = (uint32_T) - est_estimator_U_cvs_optical_flow_msg_n->cvs_valid_flag[nx]; - for (i = 0; i < 15; i++) { - UnitDelay_DSTATE_aug_state_enum += (uint32_T) - est_estimator_U_cvs_optical_flow_msg_n->cvs_valid_flag[(int32_T) - ((int32_T)((int32_T)(i + 1) * 50) + nx)]; - } - - est_estimator_B->SumofElements3[nx] = (uint8_T) - UnitDelay_DSTATE_aug_state_enum; - - // End of Sum: '/Sum of Elements3' - } - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - - rtb_Compare_ic[nx] = (est_estimator_B->SumofElements3[nx] >= - est_estimator_P->CompareToConstant7_const); - } - - // End of Outputs for SubSystem: '/Enabled Row-Wise SUM' - - // Sum: '/Sum of Elements' - UnitDelay_DSTATE_aug_state_enum = (uint32_T)rtb_Compare_ic[0]; - for (ar = 0; ar < 49; ar++) { - UnitDelay_DSTATE_aug_state_enum += (uint32_T)rtb_Compare_ic[(int32_T)(ar + 1)]; - } - - // MATLAB Function: '/MATLAB Function' - // MATLAB Function 'predictor/Covariance Propogation/MATLAB Function': ':1' - // ':1:27' - // ':1:24' - // ':1:23' - // ':1:7' - // ':1:4' - S[0] = 0.0F; - S[3] = -rtb_Product1[2]; - S[6] = rtb_Product1[1]; - S[1] = rtb_Product1[2]; - S[4] = 0.0F; - S[7] = -rtb_Product1[0]; - S[2] = -rtb_Product1[1]; - S[5] = rtb_Product1[0]; - S[8] = 0.0F; - - // ':1:7' - // ':1:8' - // ':1:9' - rtb_Sum_k1 = rtb_Product1[3]; - rtb_Gain1_f = rtb_Product1[3]; - for (i = 0; i < 3; i++) { - rtb_Product1_1[(int32_T)(3 * i)] = rtb_Sum_k1 * (real32_T)d[i] + S[i]; - rtb_Product1_2[(int32_T)(i << 2)] = (real32_T)d[(int32_T)(3 * i)] * - rtb_Gain1_f - S[(int32_T)(3 * i)]; - rtb_Product1_1[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)d[(int32_T)(i + - 3)] * rtb_Sum_k1 + S[(int32_T)(i + 3)]; - rtb_Product1_2[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)d[(int32_T) - ((int32_T)(3 * i) + 1)] * rtb_Gain1_f - S[(int32_T)((int32_T)(3 * i) + 1)]; - rtb_Product1_1[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)d[(int32_T)(i + - 6)] * rtb_Sum_k1 + S[(int32_T)(i + 6)]; - rtb_Product1_2[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)d[(int32_T) - ((int32_T)(3 * i) + 2)] * rtb_Gain1_f - S[(int32_T)((int32_T)(3 * i) + 2)]; - rtb_Product1_1[(int32_T)(9 + i)] = -rtb_Product1[i]; - rtb_Product1_2[(int32_T)(3 + (int32_T)(i << 2))] = -rtb_Product1[i]; - } - - // ':1:11' - // ':1:15' - // ':1:16' - // ':1:23' - // state_trans_1 = single(eye(15) + F_E*0.01); % Consider using expm - // ':1:32' - tmp_4[0] = 0.0F; - tmp_4[3] = -hr_quat_ISS2hr_idx_0; - tmp_4[6] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - tmp_4[1] = hr_quat_ISS2hr_idx_0; - tmp_4[4] = 0.0F; - tmp_4[7] = -rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - tmp_4[2] = -rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - tmp_4[5] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - tmp_4[8] = 0.0F; - for (br = 0; br < 3; br++) { - accel[br] = (real32_T)fabs((real_T)rtb_ImpAsg_InsertedFor_Out1_at_[br]); - for (i = 0; i < 3; i++) { - C[(int32_T)(br + (int32_T)(3 * i))] = 0.0F; - C[(int32_T)(br + (int32_T)(3 * i))] += rtb_Product1_2[(int32_T)(i << 2)] * - rtb_Product1_1[br]; - C[(int32_T)(br + (int32_T)(3 * i))] += rtb_Product1_2[(int32_T)((int32_T) - (i << 2) + 1)] * rtb_Product1_1[(int32_T)(br + 3)]; - C[(int32_T)(br + (int32_T)(3 * i))] += rtb_Product1_2[(int32_T)((int32_T) - (i << 2) + 2)] * rtb_Product1_1[(int32_T)(br + 6)]; - C[(int32_T)(br + (int32_T)(3 * i))] += rtb_Product1_2[(int32_T)((int32_T) - (i << 2) + 3)] * rtb_Product1_1[(int32_T)(br + 9)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * br))] = -C[(int32_T)((int32_T) - (3 * i) + br)]; - } - } - - tmp_5[0] = 0.0F; - tmp_5[3] = -accel[2]; - tmp_5[6] = accel[1]; - tmp_5[1] = accel[2]; - tmp_5[4] = 0.0F; - tmp_5[7] = -accel[0]; - tmp_5[2] = -accel[1]; - tmp_5[5] = accel[0]; - tmp_5[8] = 0.0F; - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m < 3; b_m++) { - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_5[(int32_T)(3 * - b_m)] * rtb_Assignment_b[i]; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_5[(int32_T) - ((int32_T)(3 * b_m) + 1)] * rtb_Assignment_b[(int32_T)(i + 3)]; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_5[(int32_T) - ((int32_T)(3 * b_m) + 2)] * rtb_Assignment_b[(int32_T)(i + 6)]; - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * i))] = -tmp_4[(int32_T) - ((int32_T)(3 * i) + b_m)]; - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * (int32_T)(i + 3)))] = - (real32_T)b[(int32_T)((int32_T)(3 * i) + b_m)]; - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * (int32_T)(i + 6)))] = 0.0F; - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * (int32_T)(i + 9)))] = 0.0F; - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * (int32_T)(i + 12)))] = 0.0F; - } - } - - for (i = 0; i < 15; i++) { - rtb_state_trans[(int32_T)(3 + (int32_T)(15 * i))] = 0.0F; - rtb_state_trans[(int32_T)(4 + (int32_T)(15 * i))] = 0.0F; - rtb_state_trans[(int32_T)(5 + (int32_T)(15 * i))] = 0.0F; - } - - for (i = 0; i < 3; i++) { - rtb_state_trans[(int32_T)(6 + (int32_T)(15 * i))] = rtb_Product1_0[(int32_T) - (3 * i)]; - rtb_state_trans[(int32_T)(6 + (int32_T)(15 * (int32_T)(i + 3)))] = 0.0F; - rtb_state_trans[(int32_T)(6 + (int32_T)(15 * (int32_T)(i + 6)))] = 0.0F; - rtb_state_trans[(int32_T)(6 + (int32_T)(15 * (int32_T)(i + 9)))] = -C[i]; - rtb_state_trans[(int32_T)(6 + (int32_T)(15 * (int32_T)(i + 12)))] = 0.0F; - rtb_state_trans[(int32_T)(7 + (int32_T)(15 * i))] = rtb_Product1_0[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_state_trans[(int32_T)(7 + (int32_T)(15 * (int32_T)(i + 3)))] = 0.0F; - rtb_state_trans[(int32_T)(7 + (int32_T)(15 * (int32_T)(i + 6)))] = 0.0F; - rtb_state_trans[(int32_T)(7 + (int32_T)(15 * (int32_T)(i + 9)))] = -C - [(int32_T)(i + 3)]; - rtb_state_trans[(int32_T)(7 + (int32_T)(15 * (int32_T)(i + 12)))] = 0.0F; - rtb_state_trans[(int32_T)(8 + (int32_T)(15 * i))] = rtb_Product1_0[(int32_T) - ((int32_T)(3 * i) + 2)]; - rtb_state_trans[(int32_T)(8 + (int32_T)(15 * (int32_T)(i + 3)))] = 0.0F; - rtb_state_trans[(int32_T)(8 + (int32_T)(15 * (int32_T)(i + 6)))] = 0.0F; - rtb_state_trans[(int32_T)(8 + (int32_T)(15 * (int32_T)(i + 9)))] = -C - [(int32_T)(i + 6)]; - rtb_state_trans[(int32_T)(8 + (int32_T)(15 * (int32_T)(i + 12)))] = 0.0F; - } - - for (i = 0; i < 15; i++) { - rtb_state_trans[(int32_T)(9 + (int32_T)(15 * i))] = 0.0F; - rtb_state_trans[(int32_T)(12 + (int32_T)(15 * i))] = (real32_T)c[(int32_T)(3 - * i)]; - rtb_state_trans[(int32_T)(10 + (int32_T)(15 * i))] = 0.0F; - rtb_state_trans[(int32_T)(13 + (int32_T)(15 * i))] = (real32_T)c[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_state_trans[(int32_T)(11 + (int32_T)(15 * i))] = 0.0F; - rtb_state_trans[(int32_T)(14 + (int32_T)(15 * i))] = (real32_T)c[(int32_T) - ((int32_T)(3 * i) + 2)]; - for (b_m = 0; b_m < 15; b_m++) { - rtb_ex_matrix_multiply4[(int32_T)(b_m + (int32_T)(15 * i))] = - rtb_state_trans[(int32_T)((int32_T)(15 * i) + b_m)] * (real32_T) - est_estimator_P->ase_ts; - } - } - - rtb_Sum_k1 = 0.0F; - handrail_knowledge_dims = 0; - exitg2 = false; - while ((!exitg2) && (handrail_knowledge_dims < 15)) { - rtb_Gain1_f = 0.0F; - for (num_original = 0; num_original < 15; num_original++) { - rtb_Gain1_f += (real32_T)fabs((real_T)rtb_ex_matrix_multiply4[(int32_T) - ((int32_T)(15 * handrail_knowledge_dims) + num_original)]); - } - - if (rtIsNaNF(rtb_Gain1_f)) { - rtb_Sum_k1 = (rtNaNF); - exitg2 = true; - } else { - if (rtb_Gain1_f > rtb_Sum_k1) { - rtb_Sum_k1 = rtb_Gain1_f; - } - - handrail_knowledge_dims++; - } - } - - if (rtb_Sum_k1 <= 3.92572474F) { - num_original = 0; - exitg1 = false; - while ((!exitg1) && (num_original < 3)) { - if (rtb_Sum_k1 <= theta[num_original]) { - hdbaohdbkngdbimo_PadeApproximantOfDegree(rtb_ex_matrix_multiply4, - (uint8_T)(int32_T)((int32_T)(num_original << 1) + 3), rtb_state_trans); - exitg1 = true; - } else { - num_original++; - } - } - } else { - rtb_Gain1_f = rtb_Sum_k1 / 3.92572474F; - if ((!rtIsInfF(rtb_Gain1_f)) && (!rtIsNaNF(rtb_Gain1_f))) { - rtb_Gain1_f = (real32_T)frexp((real_T)rtb_Gain1_f, &s12_iter); - rtb_Sum_k1 = (real32_T)s12_iter; - } else { - rtb_Sum_k1 = 0.0F; - } - - if (rtb_Gain1_f == 0.5F) { - rtb_Sum_k1--; - } - - rtb_Gain1_f = rt_powf_snf(2.0F, rtb_Sum_k1); - for (i = 0; i < 225; i++) { - rtb_Sum_b[i] = rtb_ex_matrix_multiply4[i] / rtb_Gain1_f; - } - - hdbaohdbkngdbimo_PadeApproximantOfDegree(rtb_Sum_b, 7U, rtb_state_trans); - for (num_original = 0; num_original <= (int32_T)((int32_T)rtb_Sum_k1 - 1); - num_original++) { - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_ex_matrix_multiply4[(int32_T)(i + (int32_T)(15 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 15; i_0++) { - rtb_ex_matrix_multiply4[(int32_T)(i + (int32_T)(15 * b_m))] += - rtb_state_trans[(int32_T)((int32_T)(15 * i_0) + i)] * - rtb_state_trans[(int32_T)((int32_T)(15 * b_m) + i_0)]; - } - } - } - - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_state_trans[(int32_T)(b_m + (int32_T)(15 * i))] = - rtb_ex_matrix_multiply4[(int32_T)((int32_T)(15 * i) + b_m)]; - } - } - } - } - - // End of MATLAB Function: '/MATLAB Function' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - // Paramaterize Ts - rtb_Sum_k1 = rtb_Product1[3] * rtb_Product1[3] * est_estimator_P->Gain_Gain_fm - - (real32_T)est_estimator_P->Constant1_Value_e; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_bd[i]; - } - - rtb_Assignment_b[0] = rtb_Sum_k1; - rtb_Assignment_b[4] = rtb_Sum_k1; - rtb_Assignment_b[8] = rtb_Sum_k1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' - rtb_Sum_k1 = est_estimator_P->Gain1_Gain_l4 * rtb_Product1[3]; - - // Assignment: '/Assignment1' incorporates: - // Constant: '/Constant5' - - memcpy(&rtb_Assignment1[0], &est_estimator_P->Constant5_Value_p[0], (uint32_T) - (180U * sizeof(real32_T))); - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_6[0] = (real32_T)est_estimator_P->Constant3_Value_kc; - tmp_6[1] = rtb_Product1[2]; - tmp_6[2] = est_estimator_P->Gain_Gain_k * rtb_Product1[1]; - tmp_6[3] = est_estimator_P->Gain1_Gain_j5 * rtb_Product1[2]; - tmp_6[4] = (real32_T)est_estimator_P->Constant3_Value_kc; - tmp_6[5] = rtb_Product1[0]; - tmp_6[6] = rtb_Product1[1]; - tmp_6[7] = est_estimator_P->Gain2_Gain_i5w * rtb_Product1[0]; - tmp_6[8] = (real32_T)est_estimator_P->Constant3_Value_kc; - - // Math: '/Math Function1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = rtb_Product1[0] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 3)] = rtb_Product1[1] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 6)] = rtb_Product1[2] * rtb_Product1[i]; - } - - // End of Math: '/Math Function1' - - // Assignment: '/Assignment1' incorporates: - // Gain: '/Gain2' - // Gain: '/Gain2' - // Product: '/Product' - // Sum: '/Sum1' - - for (i = 0; i < 3; i++) { - rtb_Assignment1[(int32_T)(6 + (int32_T)(15 * (int32_T)(6 + i)))] = - ((rtb_Assignment_b[i] - rtb_Sum_k1 * tmp_6[i]) + rtb_Product1_0[(int32_T) - (3 * i)] * est_estimator_P->Gain2_Gain_kj) * - est_estimator_P->Gain2_Gain_cx; - rtb_Assignment1[(int32_T)(7 + (int32_T)(15 * (int32_T)(6 + i)))] = - ((rtb_Assignment_b[(int32_T)(i + 3)] - tmp_6[(int32_T)(i + 3)] * - rtb_Sum_k1) + rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_kj) * est_estimator_P->Gain2_Gain_cx; - rtb_Assignment1[(int32_T)(8 + (int32_T)(15 * (int32_T)(6 + i)))] = - ((rtb_Assignment_b[(int32_T)(i + 6)] - tmp_6[(int32_T)(i + 6)] * - rtb_Sum_k1) + rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_kj) * est_estimator_P->Gain2_Gain_cx; - } - - // MATLAB Function: '/diag' incorporates: - // Constant: '/Constant9' - - // MATLAB Function 'predictor/Covariance Propogation/diag': ':1' - // ':1:3' - x[0] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - x[1] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - x[2] = hr_quat_ISS2hr_idx_0; - x[3] = 0.0F; - x[4] = 0.0F; - x[5] = 0.0F; - x[6] = 0.0F; - x[7] = 0.0F; - x[8] = 0.0F; - x[9] = 0.0F; - x[10] = 0.0F; - x[11] = 0.0F; - br = 0; - for (num_original = 0; num_original < 12; num_original++) { - rtb_Product1_1[num_original] = est_estimator_P->tun_ase_Q_imu[num_original]; - d_1 = ((real32_T)fabs((real_T)x[num_original]) >= - est_estimator_P->tun_ase_max_gyro); - if (d_1) { - br++; - } - - d_0[num_original] = d_1; - } - - handrail_knowledge_dims = br; - br = 0; - for (num_original = 0; num_original < 12; num_original++) { - if (d_0[num_original]) { - b_data[br] = (int8_T)(int32_T)(num_original + 1); - br++; - } - } - - // ':1:3' - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - rtb_Product1_1[(int32_T)((int32_T)b_data[i] - 1)] = - est_estimator_P->tun_ase_q_saturated_gyro; - } - - // ':1:4' - x[0] = 0.0F; - x[1] = 0.0F; - x[2] = 0.0F; - x[3] = 0.0F; - x[4] = 0.0F; - x[5] = 0.0F; - x[6] = rtb_ImpAsg_InsertedFor_Out1_at_[0]; - x[7] = rtb_ImpAsg_InsertedFor_Out1_at_[1]; - x[8] = rtb_Sum_l; - x[9] = 0.0F; - x[10] = 0.0F; - x[11] = 0.0F; - num_original = 0; - for (br = 0; br < 12; br++) { - d_1 = ((real32_T)fabs((real_T)x[br]) >= est_estimator_P->tun_ase_max_accel); - if (d_1) { - num_original++; - } - - d_0[br] = d_1; - } - - handrail_knowledge_dims = num_original; - num_original = 0; - for (nx = 0; nx < 12; nx++) { - if (d_0[nx]) { - c_data[num_original] = (int8_T)(int32_T)(nx + 1); - num_original++; - } - } - - // ':1:4' - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - rtb_Product1_1[(int32_T)((int32_T)c_data[i] - 1)] = - est_estimator_P->tun_ase_q_saturated_accel; - } - - // ':1:5' - memset(&rtb_Q[0], 0, (uint32_T)(144U * sizeof(real32_T))); - for (num_original = 0; num_original < 12; num_original++) { - rtb_Q[(int32_T)(num_original + (int32_T)(12 * num_original))] = - rtb_Product1_1[num_original]; - } - - // End of MATLAB Function: '/diag' - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply1' - matrix_multiply((real32_T*)&rtb_Assignment1[0], 15, 12, (real32_T*)&rtb_Q[0], - 12, 12, &rtb_ex_matrix_multiply1[0]); - - // Math: '/Math Function2' - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 12; b_m++) { - rtb_MathFunction2[(int32_T)(b_m + (int32_T)(12 * i))] = rtb_Assignment1 - [(int32_T)((int32_T)(15 * b_m) + i)]; - } - } - - // End of Math: '/Math Function2' - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply3' - matrix_multiply((real32_T*)&rtb_ex_matrix_multiply1[0], 15, 12, (real32_T*) - &rtb_MathFunction2[0], 12, 15, &rtb_ex_matrix_multiply3[0]); - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply' - matrix_multiply((real32_T*)&rtb_state_trans[0], 15, 15, (real32_T*) - &rtb_ex_matrix_multiply3[0], 15, 15, &rtb_ex_matrix_multiply5 - [0]); - - // Math: '/Math Function5' - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_ex_matrix_multiply4[(int32_T)(b_m + (int32_T)(15 * i))] = - rtb_state_trans[(int32_T)((int32_T)(15 * b_m) + i)]; - } - } - - // End of Math: '/Math Function5' - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply2' - matrix_multiply((real32_T*)&rtb_ex_matrix_multiply5[0], 15, 15, (real32_T*) - &rtb_ex_matrix_multiply4[0], 15, 15, &rtb_MathFunction_eu[0]); - - // Sum: '/Sum' - for (i = 0; i < 225; i++) { - rtb_Sum_b[i] = rtb_MathFunction_eu[i] + rtb_ex_matrix_multiply3[i]; - } - - // End of Sum: '/Sum' - - // Gain: '/Gain3' - num_augs = 0.5 * est_estimator_P->ase_ts; - - // MATLAB Function: '/MATLAB Function' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant3' - // Delay: '/Delay' - - // MATLAB Function 'est_estimator/filter_prep/kfl_system_prep/MATLAB Function': ':1' - // ':1:4' - memcpy(&est_estimator_B->Assignment[0], &est_estimator_DW->Delay_DSTATE[0], - (uint32_T)(13689U * sizeof(real32_T))); - if (rtb_FixPtRelationalOperator_n) { - // ':1:6' - // Quat covariance is 1:3, Pos covariance is 13:15 - // ':1:8' - for (i = 0; i < 117; i++) { - est_estimator_B->Assignment[(int32_T)(117 * i)] = 0.0F; - est_estimator_B->Assignment[(int32_T)(1 + (int32_T)(117 * i))] = 0.0F; - est_estimator_B->Assignment[(int32_T)(2 + (int32_T)(117 * i))] = 0.0F; - } - - // ':1:9' - for (i = 0; i < 3; i++) { - memset(&est_estimator_B->Assignment[(int32_T)(i * 117)], 0, (uint32_T) - (117U * sizeof(real32_T))); - } - - // ':1:10' - for (i = 0; i < 9; i++) { - S[i] = 0.0F; - } - - S[0] = est_estimator_P->tun_ic_cov_quat; - S[4] = est_estimator_P->tun_ic_cov_quat; - S[8] = est_estimator_P->tun_ic_cov_quat; - for (i = 0; i < 3; i++) { - est_estimator_B->Assignment[(int32_T)(117 * i)] = S[(int32_T)(3 * i)]; - est_estimator_B->Assignment[(int32_T)(1 + (int32_T)(117 * i))] = S - [(int32_T)((int32_T)(3 * i) + 1)]; - est_estimator_B->Assignment[(int32_T)(2 + (int32_T)(117 * i))] = S - [(int32_T)((int32_T)(3 * i) + 2)]; - } - - // ':1:12' - for (i = 0; i < 117; i++) { - est_estimator_B->Assignment[(int32_T)(12 + (int32_T)(117 * i))] = 0.0F; - est_estimator_B->Assignment[(int32_T)(13 + (int32_T)(117 * i))] = 0.0F; - est_estimator_B->Assignment[(int32_T)(14 + (int32_T)(117 * i))] = 0.0F; - } - - // ':1:13' - for (i = 0; i < 3; i++) { - memset(&est_estimator_B->Assignment[(int32_T)((int32_T)(i * 117) + 1404)], - 0, (uint32_T)(117U * sizeof(real32_T))); - } - - // ':1:14' - for (i = 0; i < 9; i++) { - S[i] = 0.0F; - } - - S[0] = est_estimator_P->tun_ic_cov_pos; - S[4] = est_estimator_P->tun_ic_cov_pos; - S[8] = est_estimator_P->tun_ic_cov_pos; - for (i = 0; i < 3; i++) { - est_estimator_B->Assignment[(int32_T)(12 + (int32_T)(117 * (int32_T)(12 + - i)))] = S[(int32_T)(3 * i)]; - est_estimator_B->Assignment[(int32_T)(13 + (int32_T)(117 * (int32_T)(12 + - i)))] = S[(int32_T)((int32_T)(3 * i) + 1)]; - est_estimator_B->Assignment[(int32_T)(14 + (int32_T)(117 * (int32_T)(12 + - i)))] = S[(int32_T)((int32_T)(3 * i) + 2)]; - } - } - - // End of MATLAB Function: '/MATLAB Function' - - // Selector: '/Selector' - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_MathFunction_eu[(int32_T)(b_m + (int32_T)(15 * i))] = - est_estimator_B->Assignment[(int32_T)((int32_T)(117 * i) + b_m)]; - } - } - - // End of Selector: '/Selector' - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply4' - matrix_multiply((real32_T*)&rtb_state_trans[0], 15, 15, (real32_T*) - &rtb_MathFunction_eu[0], 15, 15, &rtb_ex_matrix_multiply4[0]); - - // Math: '/Math Function' - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_MathFunction_eu[(int32_T)(b_m + (int32_T)(15 * i))] = rtb_state_trans - [(int32_T)((int32_T)(15 * b_m) + i)]; - } - } - - // End of Math: '/Math Function' - - // S-Function (ex_matrix_multiply): '/ex_matrix_multiply5' - matrix_multiply((real32_T*)&rtb_ex_matrix_multiply4[0], 15, 15, (real32_T*) - &rtb_MathFunction_eu[0], 15, 15, &rtb_ex_matrix_multiply5[0]); - - // Sum: '/Sum6' incorporates: - // Gain: '/Gain3' - - for (i = 0; i < 225; i++) { - est_estimator_B->MatrixConcatenate[i] = (real32_T)num_augs * rtb_Sum_b[i] + - rtb_ex_matrix_multiply5[i]; - } - - // End of Sum: '/Sum6' - - // MATLAB Function: '/MATLAB Function2' - // MATLAB Function 'predictor/Covariance Propogation/MATLAB Function2': ':1' - // ':1:7' - // ':1:4' - for (i = 0; i < 102; i++) { - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->P_IC[(int32_T)(b_m + (int32_T)(15 * i))] = - est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T)(15 + i) * 117) - + b_m)]; - } - } - - // Extract the cross coupled part of the camera and IMU covariance - // Creates a matrix where each column is the indices for each augmentation - // ':1:7' - // Number of columns is number of OF augmentations + ML augmentations - // Collect the indices of the valid current augmentations - // Form a vector with the valid indices corresponding to the current - // number of augmentations - // ':1:12' - mglfbimobiechdbi_bitget(rtb_BusAssignment_aug_state_enu, tmp_1); - br = 0; - for (num_original = 0; num_original < 17; num_original++) { - rtb_FixPtRelationalOperator_n = (tmp_1[num_original] != 0U); - if (rtb_FixPtRelationalOperator_n) { - br++; - } - - b_0[num_original] = rtb_FixPtRelationalOperator_n; - } - - handrail_knowledge_dims = br; - br = 0; - for (num_original = 0; num_original < 17; num_original++) { - if (b_0[num_original]) { - c_data_0[br] = (int8_T)(int32_T)(num_original + 1); - br++; - } - } - - // ':1:12' - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - for (b_m = 0; b_m < 6; b_m++) { - valid_indx_mat_data[(int32_T)(b_m + (int32_T)(6 * i))] = Aug_Indx[(int32_T) - ((int32_T)((int32_T)((int32_T)c_data_0[i] - 1) * 6) + b_m)]; - } - } - - // ':1:13' - // Do the covariance propagation - C_sizes_idx_1 = (int32_T)(int8_T)(int32_T)(6 * handrail_knowledge_dims); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->Selector1[(int32_T)(b_m + (int32_T)(15 * i))] = 0.0F; - } - } - - if ((int32_T)(6 * handrail_knowledge_dims) != 0) { - num_original = (int32_T)((int32_T)((int32_T)(6 * handrail_knowledge_dims) - - 1) * 15); - for (br = 0; br <= num_original; br += 15) { - for (nx = (int32_T)(br + 1); nx <= (int32_T)(br + 15); nx++) { - est_estimator_B->Selector1[(int32_T)(nx - 1)] = 0.0F; - } - } - - br = 0; - for (nx = 0; nx <= num_original; nx += 15) { - ar = -1; - for (i = br; (int32_T)(i + 1) <= (int32_T)(br + 15); i++) { - for (b_m = 0; b_m <= (int32_T)(handrail_knowledge_dims - 1); b_m++) { - for (i_0 = 0; i_0 < 6; i_0++) { - valid_indx_mat_data_0[(int32_T)(i_0 + (int32_T)(6 * b_m))] = - valid_indx_mat_data[(int32_T)((int32_T)(6 * b_m) + i_0)]; - } - } - - if (est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T)((int32_T) - valid_indx_mat_data_0[div_nzp_s32_floor(i, 15)] + 14) * 117) + i % - 15)] != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = nx; (int32_T)(O_sizes_idx_0 + 1) <= (int32_T)(nx - + 15); O_sizes_idx_0++) { - s12_iter++; - for (b_m = 0; b_m <= (int32_T)(handrail_knowledge_dims - 1); b_m++) - { - for (i_0 = 0; i_0 < 6; i_0++) { - valid_indx_mat_data_0[(int32_T)(i_0 + (int32_T)(6 * b_m))] = - valid_indx_mat_data[(int32_T)((int32_T)(6 * b_m) + i_0)]; - } - } - - est_estimator_B->Selector1[O_sizes_idx_0] += - est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T)((int32_T) - valid_indx_mat_data_0[div_nzp_s32_floor(i, 15)] + 14) * 117) + i % - 15)] * rtb_state_trans[s12_iter]; - } - } - - ar += 15; - } - - br += 15; - } - } - - // ':1:16' - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - for (b_m = 0; b_m < 6; b_m++) { - valid_indx_mat_data_0[(int32_T)(b_m + (int32_T)(6 * i))] = - valid_indx_mat_data[(int32_T)((int32_T)(6 * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->P_IC[(int32_T)(b_m + (int32_T)(15 * (int32_T)((int32_T) - valid_indx_mat_data_0[i] - 1)))] = est_estimator_B->Selector1[(int32_T) - ((int32_T)(15 * i) + b_m)]; - } - } - - // End of MATLAB Function: '/MATLAB Function2' - - // Selector: '/Selector1' - // Aug_Indx(Aug_State_Flag) - // Do the multiplication with current valid indices - // P(:, D+1:size(P, 2)) = Phi * P(1:D, D+1:size(P, 2)); - for (i = 0; i < 102; i++) { - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->Selector1[(int32_T)(b_m + (int32_T)(15 * i))] = - est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T)(15 + i) * 117) - + b_m)]; - } - } - - // End of Selector: '/Selector1' - - // Switch: '/Switch' - if (rtb_BusAssignment_aug_state_enu != 0U) { - memcpy(&est_estimator_B->MatrixConcatenate[225], &est_estimator_B->P_IC[0], - (uint32_T)(1530U * sizeof(real32_T))); - } else { - memcpy(&est_estimator_B->MatrixConcatenate[225], &est_estimator_B-> - Selector1[0], (uint32_T)(1530U * sizeof(real32_T))); - } - - // End of Switch: '/Switch' - - // Switch: '/Switch1' incorporates: - // Math: '/Math Function3' - // Math: '/Math Function4' - - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 102; b_m++) { - if (rtb_BusAssignment_aug_state_enu != 0U) { - est_estimator_B->MatrixConcatenate2[(int32_T)(b_m + (int32_T)(i * 102))] - = est_estimator_B->P_IC[(int32_T)((int32_T)(15 * b_m) + i)]; - } else { - est_estimator_B->MatrixConcatenate2[(int32_T)(b_m + (int32_T)(i * 102))] - = est_estimator_B->Selector1[(int32_T)((int32_T)(15 * b_m) + i)]; - } - } - } - - // End of Switch: '/Switch1' - - // Selector: '/Selector2' - for (i = 0; i < 102; i++) { - memcpy(&est_estimator_B->MatrixConcatenate2[(int32_T)((int32_T)(i * 102) + - 1530)], &est_estimator_B->Assignment[(int32_T)((int32_T)(i * 117) + - 1770)], (uint32_T)(102U * sizeof(real32_T))); - } - - // End of Selector: '/Selector2' - - // Concatenate: '/Matrix Concatenate1' - for (i = 0; i < 117; i++) { - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->Assignment[(int32_T)(b_m + (int32_T)(117 * i))] = - est_estimator_B->MatrixConcatenate[(int32_T)((int32_T)(15 * i) + b_m)]; - } - - memcpy(&est_estimator_B->Assignment[(int32_T)((int32_T)(i * 117) + 15)], - &est_estimator_B->MatrixConcatenate2[(int32_T)(i * 102)], (uint32_T) - (102U * sizeof(real32_T))); - } - - // End of Concatenate: '/Matrix Concatenate1' - - // If: '/If' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Inport: '/cmc_msg' - // Inport: '/handrail_msg' - // Inport: '/P_in' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator3' - // Logic: '/Logical Operator5' - // Logic: '/Logical Operator7' - // Logic: '/Logical Operator8' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/FixPt Relational Operator' - // RelationalOperator: '/FixPt Relational Operator' - // S-Function (sfix_bitop): '/Bitwise Operator1' - // SignalConversion: '/Signal Conversion' - // Sum: '/Sum of Elements' - // Switch: '/Switch3' - // UnitDelay: '/Delay Input1' - // UnitDelay: '/Delay Input1' - - if ((rtb_Compare && (((est_estimator_U_handrail_msg->cvs_timestamp_sec != - est_estimator_DW->DelayInput1_DSTATE) || - (est_estimator_U_handrail_msg->cvs_timestamp_nsec != - est_estimator_DW->DelayInput1_DSTATE_h)) && - empty_non_axis_sizes) && (rtb_BitwiseOperator != 0U) && - (est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_perching)) || rtb_LogicalOperator) { - c_sz_idx_0 = 0; - } else if (((uint32_T)(rtb_BusAssignment_aug_state_enu & - est_estimator_P->BitwiseOperator1_BitMask_l) == - est_estimator_P->ase_aug_state_bitmask) && rtb_of_update_flag && - ((int32_T)est_estimator_P->tun_ase_enable_of != 0) && ((uint8_T) - UnitDelay_DSTATE_aug_state_enum >= - est_estimator_P->Constant_Value_e)) { - c_sz_idx_0 = 1; - } else { - c_sz_idx_0 = 2; - } - - switch (c_sz_idx_0) { - case 0: - // Outputs for IfAction SubSystem: '/Absolute_Update' incorporates: - // ActionPort: '/Action Port' - - // Outputs for Iterator SubSystem: '/ML Update' incorporates: - // WhileIterator: '/While Iterator' - - // InitializeConditions for UnitDelay: '/Unit Delay1' incorporates: - // UnitDelay: '/Unit Delay1' - - for (i = 0; i < 13689; i++) { - est_estimator_B->Switch1[i] = est_estimator_P->UnitDelay1_InitialCondition; - } - - // End of InitializeConditions for UnitDelay: '/Unit Delay1' - - // InitializeConditions for UnitDelay: '/Unit Delay' incorporates: - // UnitDelay: '/Unit Delay' - - rtb_Merge_o[0] = est_estimator_P->UnitDelay_InitialCondition.quat_ISS2B[0]; - rtb_Merge_o[1] = est_estimator_P->UnitDelay_InitialCondition.quat_ISS2B[1]; - rtb_Merge_o[2] = est_estimator_P->UnitDelay_InitialCondition.quat_ISS2B[2]; - rtb_Merge_o[3] = est_estimator_P->UnitDelay_InitialCondition.quat_ISS2B[3]; - rtb_ImpAsg_InsertedFor_Out1_at_[0] = - est_estimator_P->UnitDelay_InitialCondition.omega_B_ISS_B[0]; - accel[0] = est_estimator_P->UnitDelay_InitialCondition.gyro_bias[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = - est_estimator_P->UnitDelay_InitialCondition.V_B_ISS_ISS[0]; - rtb_Sum_k1 = est_estimator_P->UnitDelay_InitialCondition.A_B_ISS_ISS[0]; - UnitDelay_DSTATE_n_accel_bias[0] = - est_estimator_P->UnitDelay_InitialCondition.accel_bias[0]; - rtb_Sum_l = est_estimator_P->UnitDelay_InitialCondition.P_B_ISS_ISS[0]; - rtb_ImpAsg_InsertedFor_Out1_at_[1] = - est_estimator_P->UnitDelay_InitialCondition.omega_B_ISS_B[1]; - accel[1] = est_estimator_P->UnitDelay_InitialCondition.gyro_bias[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = - est_estimator_P->UnitDelay_InitialCondition.V_B_ISS_ISS[1]; - rtb_Gain1_f = est_estimator_P->UnitDelay_InitialCondition.A_B_ISS_ISS[1]; - UnitDelay_DSTATE_n_accel_bias[1] = - est_estimator_P->UnitDelay_InitialCondition.accel_bias[1]; - UnitDelay_DSTATE_n_P_B_ISS_ISS_ = - est_estimator_P->UnitDelay_InitialCondition.P_B_ISS_ISS[1]; - rtb_ImpAsg_InsertedFor_Out1_at_[2] = - est_estimator_P->UnitDelay_InitialCondition.omega_B_ISS_B[2]; - accel[2] = est_estimator_P->UnitDelay_InitialCondition.gyro_bias[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = - est_estimator_P->UnitDelay_InitialCondition.V_B_ISS_ISS[2]; - UnitDelay_DSTATE_n_A_B_ISS_ISS_ = - est_estimator_P->UnitDelay_InitialCondition.A_B_ISS_ISS[2]; - UnitDelay_DSTATE_n_accel_bias[2] = - est_estimator_P->UnitDelay_InitialCondition.accel_bias[2]; - UnitDelay_DSTATE_n_P_B_ISS_IS_0 = - est_estimator_P->UnitDelay_InitialCondition.P_B_ISS_ISS[2]; - rtb_Saturation_n = est_estimator_P->UnitDelay_InitialCondition.confidence; - rtb_BitwiseOperator = - est_estimator_P->UnitDelay_InitialCondition.aug_state_enum; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = - est_estimator_P->UnitDelay_InitialCondition.ml_quat_ISS2cam[0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = - est_estimator_P->UnitDelay_InitialCondition.ml_quat_ISS2cam[1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = - est_estimator_P->UnitDelay_InitialCondition.ml_quat_ISS2cam[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = - est_estimator_P->UnitDelay_InitialCondition.ml_quat_ISS2cam[3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = - est_estimator_P->UnitDelay_InitialCondition.ml_P_cam_ISS_ISS[0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = - est_estimator_P->UnitDelay_InitialCondition.ml_P_cam_ISS_ISS[1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = - est_estimator_P->UnitDelay_InitialCondition.ml_P_cam_ISS_ISS[2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], - &est_estimator_P->UnitDelay_InitialCondition.of_quat_ISS2cam[0], - (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &est_estimator_P->UnitDelay_InitialCondition.of_P_cam_ISS_ISS[0], - (uint32_T)(48U * sizeof(real32_T))); - memcpy(&UnitDelay_DSTATE_n_cov_diag[0], - &est_estimator_P->UnitDelay_InitialCondition.cov_diag[0], (uint32_T) - (117U * sizeof(ase_cov_datatype))); - UnitDelay_DSTATE_n_kfl_status = - est_estimator_P->UnitDelay_InitialCondition.kfl_status; - rtb_Switch_m = - est_estimator_P->UnitDelay_InitialCondition.update_OF_tracks_cnt; - rtb_numFeatures_f = - est_estimator_P->UnitDelay_InitialCondition.update_ML_features_cnt; - memcpy(&UnitDelay_DSTATE_n_of_mahal_dis[0], - &est_estimator_P->UnitDelay_InitialCondition.of_mahal_distance[0], - (uint32_T)(50U * sizeof(real_T))); - memcpy(&UnitDelay_DSTATE_n_ml_mahal_dis[0], - &est_estimator_P->UnitDelay_InitialCondition.ml_mahal_distance[0], - (uint32_T)(50U * sizeof(real_T))); - UnitDelay_DSTATE_n_hr_P_hr_ISS_ = - est_estimator_P->UnitDelay_InitialCondition.hr_P_hr_ISS_ISS[0]; - UnitDelay_DSTATE_n_hr_P_hr_IS_0 = - est_estimator_P->UnitDelay_InitialCondition.hr_P_hr_ISS_ISS[1]; - UnitDelay_DSTATE_n_hr_P_hr_IS_1 = - est_estimator_P->UnitDelay_InitialCondition.hr_P_hr_ISS_ISS[2]; - UnitDelay_DSTATE_n_hr_quat_ISS2 = - est_estimator_P->UnitDelay_InitialCondition.hr_quat_ISS2hr[0]; - UnitDelay_DSTATE_n_hr_quat_IS_0 = - est_estimator_P->UnitDelay_InitialCondition.hr_quat_ISS2hr[1]; - UnitDelay_DSTATE_n_hr_quat_IS_1 = - est_estimator_P->UnitDelay_InitialCondition.hr_quat_ISS2hr[2]; - UnitDelay_DSTATE_n_hr_quat_IS_2 = - est_estimator_P->UnitDelay_InitialCondition.hr_quat_ISS2hr[3]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = - est_estimator_P->UnitDelay_InitialCondition.P_EST_ISS_ISS[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = - est_estimator_P->UnitDelay_InitialCondition.P_EST_ISS_ISS[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = - est_estimator_P->UnitDelay_InitialCondition.P_EST_ISS_ISS[2]; - if (1 > est_estimator_P->Switch3_Threshold) { - memcpy(&est_estimator_B->Assignment[0], &est_estimator_B->Switch1[0], - (uint32_T)(13689U * sizeof(real32_T))); - } - - // Switch: '/Switch2' incorporates: - // BusAssignment: '/Bus Assignment' - // BusAssignment: '/Bus Assignment' - // BusCreator: '/Bus Creator2' - // Switch: '/Switch3' - // UnitDelay: '/Unit Delay10' - // UnitDelay: '/Unit Delay11' - // UnitDelay: '/Unit Delay12' - // UnitDelay: '/Unit Delay13' - // UnitDelay: '/Unit Delay14' - // UnitDelay: '/Unit Delay15' - // UnitDelay: '/Unit Delay3' - // UnitDelay: '/Unit Delay6' - // UnitDelay: '/Unit Delay7' - // UnitDelay: '/Unit Delay8' - - if (!(1 > est_estimator_P->Switch2_Threshold)) { - rtb_Merge_o[0] = rtb_Product1[0]; - rtb_Merge_o[1] = rtb_Product1[1]; - rtb_Merge_o[2] = rtb_Product1[2]; - rtb_Merge_o[3] = rtb_Product1[3]; - rtb_ImpAsg_InsertedFor_Out1_at_[0] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - accel[0] = est_estimator_DW->UnitDelay3_DSTATE[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = rtb_Sum2[0]; - rtb_Sum_k1 = rtb_Sum1_k3[0]; - UnitDelay_DSTATE_n_accel_bias[0] = est_estimator_DW->UnitDelay6_DSTATE[0]; - rtb_Sum_l = est_estimator_DW->UnitDelay7_DSTATE[0]; - rtb_ImpAsg_InsertedFor_Out1_at_[1] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - accel[1] = est_estimator_DW->UnitDelay3_DSTATE[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = rtb_Sum2[1]; - rtb_Gain1_f = rtb_Sum1_k3[1]; - UnitDelay_DSTATE_n_accel_bias[1] = est_estimator_DW->UnitDelay6_DSTATE[1]; - UnitDelay_DSTATE_n_P_B_ISS_ISS_ = est_estimator_DW->UnitDelay7_DSTATE[1]; - rtb_ImpAsg_InsertedFor_Out1_at_[2] = hr_quat_ISS2hr_idx_0; - accel[2] = est_estimator_DW->UnitDelay3_DSTATE[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = rtb_Sum2[2]; - UnitDelay_DSTATE_n_A_B_ISS_ISS_ = rtb_Sum1_k3[2]; - UnitDelay_DSTATE_n_accel_bias[2] = est_estimator_DW->UnitDelay6_DSTATE[2]; - UnitDelay_DSTATE_n_P_B_ISS_IS_0 = est_estimator_DW->UnitDelay7_DSTATE[2]; - rtb_Saturation_n = est_estimator_DW->UnitDelay8_DSTATE; - rtb_BitwiseOperator = rtb_BusAssignment_aug_state_enu; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = est_estimator_DW->UnitDelay10_DSTATE - [0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = est_estimator_DW->UnitDelay10_DSTATE - [1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = est_estimator_DW->UnitDelay10_DSTATE - [2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = est_estimator_DW->UnitDelay10_DSTATE - [3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = est_estimator_DW->UnitDelay11_DSTATE - [0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = est_estimator_DW->UnitDelay11_DSTATE - [1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = est_estimator_DW->UnitDelay11_DSTATE - [2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], - &est_estimator_DW->UnitDelay12_DSTATE[0], (uint32_T)(sizeof - (real32_T) << 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &est_estimator_DW->UnitDelay13_DSTATE[0], (uint32_T)(48U * sizeof - (real32_T))); - memcpy(&UnitDelay_DSTATE_n_cov_diag[0], - &est_estimator_DW->UnitDelay14_DSTATE[0], (uint32_T)(117U * sizeof - (ase_cov_datatype))); - UnitDelay_DSTATE_n_kfl_status = est_estimator_DW->UnitDelay15_DSTATE; - rtb_Switch_m = numFeatures; - rtb_numFeatures_f = rtb_BusCreator2_update_ML_featu; - memcpy(&UnitDelay_DSTATE_n_of_mahal_dis[0], &rtb_UnitDelay18[0], (uint32_T) - (50U * sizeof(real_T))); - memcpy(&UnitDelay_DSTATE_n_ml_mahal_dis[0], &rtb_UnitDelay19[0], (uint32_T) - (50U * sizeof(real_T))); - UnitDelay_DSTATE_n_hr_P_hr_ISS_ = rtb_Add_e[0]; - UnitDelay_DSTATE_n_hr_P_hr_IS_0 = rtb_Add_e[1]; - UnitDelay_DSTATE_n_hr_P_hr_IS_1 = rtb_Add_e[2]; - UnitDelay_DSTATE_n_hr_quat_ISS2 = rtb_UnitDelay25[0]; - UnitDelay_DSTATE_n_hr_quat_IS_0 = rtb_UnitDelay25[1]; - UnitDelay_DSTATE_n_hr_quat_IS_1 = rtb_UnitDelay25[2]; - UnitDelay_DSTATE_n_hr_quat_IS_2 = rtb_UnitDelay25[3]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = rtb_P_B_ISS_ISS[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = rtb_P_B_ISS_ISS[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = rtb_P_B_ISS_ISS[2]; - } - - // End of Switch: '/Switch2' - - // If: '/IF' - if ((int32_T)est_estimator_U_cmc_msg_o->localization_mode_cmd == 2) { - // Outputs for IfAction SubSystem: '/HR_Compute_Residual_and_H1' incorporates: - // ActionPort: '/Action Port' - - // MATLAB Function: '/Compute Global positions of Handrail Features' - // MATLAB Function 'camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/HR_Compute_Residual_and_H1/Compute Global positions of Handrail Features': ':1' - // ':1:4' - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - if (!est_estimator_DW->hr_P_hr_ISS_ISS_pers_not_empty) { - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - S[0] = 0.0F; - S[3] = -UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - S[6] = UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - S[1] = UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - S[4] = 0.0F; - S[7] = -UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - S[2] = -UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - S[5] = UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - S[8] = 0.0F; - hr_quat_ISS2hr_idx_0 = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - rtb_Switch1 = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - for (i = 0; i < 3; i++) { - x[(int32_T)(3 * i)] = hr_quat_ISS2hr_idx_0 * (real32_T)b_2[i] + S[i]; - rtb_Product1_1[(int32_T)(i << 2)] = (real32_T)b_2[(int32_T)(3 * i)] * - rtb_Switch1 - S[(int32_T)(3 * i)]; - x[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 3)] * - hr_quat_ISS2hr_idx_0 + S[(int32_T)(i + 3)]; - rtb_Product1_1[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 1)] * rtb_Switch1 - S[(int32_T) - ((int32_T)(3 * i) + 1)]; - x[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 6)] * - hr_quat_ISS2hr_idx_0 + S[(int32_T)(i + 6)]; - rtb_Product1_1[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 2)] * rtb_Switch1 - S[(int32_T) - ((int32_T)(3 * i) + 2)]; - x[(int32_T)(9 + i)] = -UnitDelay_DSTATE_n_ml_quat_ISS2[i]; - rtb_Product1_1[(int32_T)(3 + (int32_T)(i << 2))] = - -UnitDelay_DSTATE_n_ml_quat_ISS2[i]; - } - - for (i = 0; i < 3; i++) { - hr_quat_ISS2hr_idx_1 = 0.0F; - for (b_m = 0; b_m < 3; b_m++) { - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)(i << 2)] * x[b_m]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 1)] * x[(int32_T)(b_m - + 3)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 2)] * x[(int32_T)(b_m - + 6)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 3)] * x[(int32_T)(b_m - + 9)]; - hr_quat_ISS2hr_idx_1 += rtb_Assignment_b[(int32_T)((int32_T)(3 * b_m) - + i)] * est_estimator_U_handrail_msg->cvs_handrail_local_pos[b_m]; - } - - est_estimator_DW->hr_P_hr_ISS_ISS_pers[i] = - UnitDelay_DSTATE_n_ml_P_cam_ISS[i] + hr_quat_ISS2hr_idx_1; - } - - est_estimator_DW->hr_P_hr_ISS_ISS_pers_not_empty = true; - } - - if (!est_estimator_DW->hr_quat_ISS2hr_pers_not_empty) { - iecjopppiecjmgln_quatmult(UnitDelay_DSTATE_n_ml_quat_ISS2, - est_estimator_U_handrail_msg->cvs_handrail_local_quat, - est_estimator_DW->hr_quat_ISS2hr_pers); - est_estimator_DW->hr_quat_ISS2hr_pers_not_empty = true; - } - - if ((int32_T) - est_estimator_U_handrail_msg->cvs_handrail_update_global_pose_flag == - 1) { - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - S[0] = 0.0F; - S[3] = -UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - S[6] = UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - S[1] = UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - S[4] = 0.0F; - S[7] = -UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - S[2] = -UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - S[5] = UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - S[8] = 0.0F; - hr_quat_ISS2hr_idx_0 = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - rtb_Switch1 = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - for (i = 0; i < 3; i++) { - x[(int32_T)(3 * i)] = hr_quat_ISS2hr_idx_0 * (real32_T)b_2[i] + S[i]; - rtb_Product1_1[(int32_T)(i << 2)] = (real32_T)b_2[(int32_T)(3 * i)] * - rtb_Switch1 - S[(int32_T)(3 * i)]; - x[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 3)] * - hr_quat_ISS2hr_idx_0 + S[(int32_T)(i + 3)]; - rtb_Product1_1[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 1)] * rtb_Switch1 - S[(int32_T) - ((int32_T)(3 * i) + 1)]; - x[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 6)] * - hr_quat_ISS2hr_idx_0 + S[(int32_T)(i + 6)]; - rtb_Product1_1[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 2)] * rtb_Switch1 - S[(int32_T) - ((int32_T)(3 * i) + 2)]; - x[(int32_T)(9 + i)] = -UnitDelay_DSTATE_n_ml_quat_ISS2[i]; - rtb_Product1_1[(int32_T)(3 + (int32_T)(i << 2))] = - -UnitDelay_DSTATE_n_ml_quat_ISS2[i]; - } - - for (i = 0; i < 3; i++) { - hr_quat_ISS2hr_idx_1 = 0.0F; - for (b_m = 0; b_m < 3; b_m++) { - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)(i << 2)] * x[b_m]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 1)] * x[(int32_T)(b_m - + 3)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 2)] * x[(int32_T)(b_m - + 6)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_Product1_1[(int32_T)((int32_T)(i << 2) + 3)] * x[(int32_T)(b_m - + 9)]; - hr_quat_ISS2hr_idx_1 += rtb_Assignment_b[(int32_T)((int32_T)(3 * b_m) - + i)] * est_estimator_U_handrail_msg->cvs_handrail_local_pos[b_m]; - } - - est_estimator_DW->hr_P_hr_ISS_ISS_pers[i] = - UnitDelay_DSTATE_n_ml_P_cam_ISS[i] + hr_quat_ISS2hr_idx_1; - } - - iecjopppiecjmgln_quatmult(UnitDelay_DSTATE_n_ml_quat_ISS2, - est_estimator_U_handrail_msg->cvs_handrail_local_quat, - est_estimator_DW->hr_quat_ISS2hr_pers); - } - - memset(&rtb_hr_global_landmarks[0], 0, (uint32_T)(150U * sizeof(real32_T))); - num_original = 0; - while ((num_original < 50) && (!((int32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[num_original] == 0))) - { - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - S[0] = 0.0F; - S[3] = -est_estimator_U_handrail_msg->cvs_handrail_local_quat[2]; - S[6] = est_estimator_U_handrail_msg->cvs_handrail_local_quat[1]; - S[1] = est_estimator_U_handrail_msg->cvs_handrail_local_quat[2]; - S[4] = 0.0F; - S[7] = -est_estimator_U_handrail_msg->cvs_handrail_local_quat[0]; - S[2] = -est_estimator_U_handrail_msg->cvs_handrail_local_quat[1]; - S[5] = est_estimator_U_handrail_msg->cvs_handrail_local_quat[0]; - S[8] = 0.0F; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct rotation matrix from quaternion - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - C[0] = 0.0F; - C[3] = -est_estimator_DW->hr_quat_ISS2hr_pers[2]; - C[6] = est_estimator_DW->hr_quat_ISS2hr_pers[1]; - C[1] = est_estimator_DW->hr_quat_ISS2hr_pers[2]; - C[4] = 0.0F; - C[7] = -est_estimator_DW->hr_quat_ISS2hr_pers[0]; - C[2] = -est_estimator_DW->hr_quat_ISS2hr_pers[1]; - C[5] = est_estimator_DW->hr_quat_ISS2hr_pers[0]; - C[8] = 0.0F; - hr_quat_ISS2hr_idx_1 = est_estimator_DW->hr_quat_ISS2hr_pers[3]; - hr_quat_ISS2hr_idx_0 = est_estimator_DW->hr_quat_ISS2hr_pers[3]; - rtb_Switch1 = est_estimator_U_handrail_msg->cvs_handrail_local_quat[3]; - hr_quat_ISS2hr_idx_2 = - est_estimator_U_handrail_msg->cvs_handrail_local_quat[3]; - for (i = 0; i < 3; i++) { - x[(int32_T)(3 * i)] = hr_quat_ISS2hr_idx_1 * (real32_T)b_2[i] + C[i]; - tmp_8[(int32_T)(i << 2)] = (real32_T)b_2[(int32_T)(3 * i)] * - hr_quat_ISS2hr_idx_0 - C[(int32_T)(3 * i)]; - rtb_Product1_1[(int32_T)(3 * i)] = rtb_Switch1 * (real32_T)b_2[i] + - S[i]; - rtb_Product1_2[(int32_T)(i << 2)] = (real32_T)b_2[(int32_T)(3 * i)] * - hr_quat_ISS2hr_idx_2 - S[(int32_T)(3 * i)]; - x[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 3)] * - hr_quat_ISS2hr_idx_1 + C[(int32_T)(i + 3)]; - tmp_8[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)b_2[(int32_T) - ((int32_T)(3 * i) + 1)] * hr_quat_ISS2hr_idx_0 - C[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_Product1_1[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)b_2 - [(int32_T)(i + 3)] * rtb_Switch1 + S[(int32_T)(i + 3)]; - rtb_Product1_2[(int32_T)(1 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 1)] * hr_quat_ISS2hr_idx_2 - S - [(int32_T)((int32_T)(3 * i) + 1)]; - x[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)b_2[(int32_T)(i + 6)] * - hr_quat_ISS2hr_idx_1 + C[(int32_T)(i + 6)]; - tmp_8[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)b_2[(int32_T) - ((int32_T)(3 * i) + 2)] * hr_quat_ISS2hr_idx_0 - C[(int32_T) - ((int32_T)(3 * i) + 2)]; - rtb_Product1_1[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)b_2 - [(int32_T)(i + 6)] * rtb_Switch1 + S[(int32_T)(i + 6)]; - rtb_Product1_2[(int32_T)(2 + (int32_T)(i << 2))] = (real32_T)b_2 - [(int32_T)((int32_T)(3 * i) + 2)] * hr_quat_ISS2hr_idx_2 - S - [(int32_T)((int32_T)(3 * i) + 2)]; - x[(int32_T)(9 + i)] = -est_estimator_DW->hr_quat_ISS2hr_pers[i]; - tmp_8[(int32_T)(3 + (int32_T)(i << 2))] = - -est_estimator_DW->hr_quat_ISS2hr_pers[i]; - rtb_Product1_1[(int32_T)(9 + i)] = - -est_estimator_U_handrail_msg->cvs_handrail_local_quat[i]; - rtb_Product1_2[(int32_T)(3 + (int32_T)(i << 2))] = - -est_estimator_U_handrail_msg->cvs_handrail_local_quat[i]; - } - - for (i = 0; i < 3; i++) { - rtb_P_B_ISS_ISS[i] = est_estimator_U_handrail_msg->cvs_observations - [(int32_T)((int32_T)(50 * i) + num_original)] - - est_estimator_U_handrail_msg->cvs_handrail_local_pos[i]; - for (b_m = 0; b_m < 3; b_m++) { - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - rtb_Assignment_o[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += rtb_Product1_2 - [(int32_T)(b_m << 2)] * rtb_Product1_1[i]; - rtb_Assignment_o[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_8 - [(int32_T)(i << 2)] * x[b_m]; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += rtb_Product1_2 - [(int32_T)((int32_T)(b_m << 2) + 1)] * rtb_Product1_1[(int32_T)(i - + 3)]; - rtb_Assignment_o[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_8 - [(int32_T)((int32_T)(i << 2) + 1)] * x[(int32_T)(b_m + 3)]; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += rtb_Product1_2 - [(int32_T)((int32_T)(b_m << 2) + 2)] * rtb_Product1_1[(int32_T)(i - + 6)]; - rtb_Assignment_o[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_8 - [(int32_T)((int32_T)(i << 2) + 2)] * x[(int32_T)(b_m + 6)]; - rtb_Product1_0[(int32_T)(i + (int32_T)(3 * b_m))] += rtb_Product1_2 - [(int32_T)((int32_T)(b_m << 2) + 3)] * rtb_Product1_1[(int32_T)(i - + 9)]; - rtb_Assignment_o[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_8 - [(int32_T)((int32_T)(i << 2) + 3)] * x[(int32_T)(b_m + 9)]; - } - } - - for (i = 0; i < 3; i++) { - rtb_Sum1_k3[i] = rtb_Product1_0[(int32_T)(i + 6)] * rtb_P_B_ISS_ISS[2] - + (rtb_Product1_0[(int32_T)(i + 3)] * rtb_P_B_ISS_ISS[1] + - rtb_Product1_0[i] * rtb_P_B_ISS_ISS[0]); - } - - for (i = 0; i < 3; i++) { - rtb_hr_global_landmarks[(int32_T)(num_original + (int32_T)(50 * i))] = - ((rtb_Assignment_o[(int32_T)(i + 3)] * rtb_Sum1_k3[1] + - rtb_Assignment_o[i] * rtb_Sum1_k3[0]) + rtb_Assignment_o[(int32_T) - (i + 6)] * rtb_Sum1_k3[2]) + est_estimator_DW-> - hr_P_hr_ISS_ISS_pers[i]; - } - - num_original++; - } - - // MATLAB Function: '/Compute Residual and H' - // ':1:4' - // MATLAB Function 'camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/HR_Compute_Residual_and_H1/Compute Residual and H': ':1' - // ':1:3' - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - if ((int32_T)est_estimator_U_handrail_msg->cvs_3d_knowledge_flag == 1) { - handrail_knowledge_dims = 3; - } else { - handrail_knowledge_dims = 2; - } - - // Get tf matrix that converts from global position (world frame) - // to local position (camera frame) - for (i = 0; i < 12; i++) { - camera_ml_tf_global[i] = (real_T)t[i]; - } - - fkfcbaiengdjgdje_quaternion_to_rotation(UnitDelay_DSTATE_n_ml_quat_ISS2, - rtb_Product1_0); - for (i = 0; i < 3; i++) { - C[(int32_T)(3 * i)] = (real32_T)-(real_T)rtb_Product1_0[(int32_T)(3 * i)]; - camera_ml_tf_global[(int32_T)(3 * i)] = (real_T)rtb_Product1_0[(int32_T) - (3 * i)]; - C[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)-(real_T)rtb_Product1_0 - [(int32_T)((int32_T)(3 * i) + 1)]; - camera_ml_tf_global[(int32_T)(1 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)]; - C[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)-(real_T)rtb_Product1_0 - [(int32_T)((int32_T)(3 * i) + 2)]; - camera_ml_tf_global[(int32_T)(2 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i < 3; i++) { - camera_ml_tf_global[(int32_T)(9 + i)] = (real_T)((C[(int32_T)(i + 3)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] + C[i] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[0]) + C[(int32_T)(i + 6)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[2]); - } - - // Remove the invalid features from the observations and landmarks - // Observations from the camera frame - // Landmarks from the world frame - // Convert global positions of the landmarks to the local positions - br = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[nx] != 0) { - br++; - } - } - - C_sizes_idx_1 = br; - br = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[nx] != 0) { - e_data[br] = (int8_T)(int32_T)(nx + 1); - br++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[(int32_T)(3 * i)] = rtb_hr_global_landmarks[(int32_T) - ((int32_T)e_data[i] - 1)]; - varargin_1_data[(int32_T)(1 + (int32_T)(3 * i))] = - rtb_hr_global_landmarks[(int32_T)((int32_T)e_data[i] + 49)]; - varargin_1_data[(int32_T)(2 + (int32_T)(3 * i))] = - rtb_hr_global_landmarks[(int32_T)((int32_T)e_data[i] + 99)]; - } - - if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else { - num_original = 0; - if (C_sizes_idx_1 > 0) { - num_original = C_sizes_idx_1; - } - } - - empty_non_axis_sizes = (num_original == 0); - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - br = 3; - } else { - br = 0; - } - - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - nx = 1; - } else { - nx = 0; - } - - ar = (int32_T)(br + nx); - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - rtb_global_points[(int32_T)(b_m + (int32_T)(ar * i))] = - varargin_1_data[(int32_T)((int32_T)(br * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(nx - 1); b_m++) { - rtb_global_points[(int32_T)((int32_T)(b_m + br) + (int32_T)(ar * i))] = - 1.0F; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)(ar * b_m)] * (real32_T) - camera_ml_tf_global[i]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 1)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 3)]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 2)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 6)]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 3)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 9)]; - } - } - - // Landmarks in the camera frame - // Create rotation matrix to convert between bases - // R rotation matrix that rotates from the camera frame into the handrail frame - // with the axis_body vector as the z axis unit vector - fkfcbaiengdjgdje_quaternion_to_rotation - (est_estimator_U_handrail_msg->cvs_handrail_local_quat, S); - - // R = eye(3); - num_original = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[nx] != 0) { - num_original++; - } - } - - C_sizes_idx_1 = num_original; - num_original = 0; - for (ar = 0; ar < 50; ar++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[ar] != 0) { - e_data[num_original] = (int8_T)(int32_T)(ar + 1); - num_original++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[(int32_T)(3 * i)] = - est_estimator_U_handrail_msg->cvs_observations[(int32_T)((int32_T) - e_data[i] - 1)] - camera_landmarks_data[(int32_T)(3 * i)]; - varargin_1_data[(int32_T)(1 + (int32_T)(3 * i))] = - est_estimator_U_handrail_msg->cvs_observations[(int32_T)((int32_T) - e_data[i] + 49)] - camera_landmarks_data[(int32_T)((int32_T)(3 * i) + - 1)]; - varargin_1_data[(int32_T)(2 + (int32_T)(3 * i))] = - est_estimator_U_handrail_msg->cvs_observations[(int32_T)((int32_T) - e_data[i] + 99)] - camera_landmarks_data[(int32_T)((int32_T)(3 * i) + - 2)]; - } - - if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else { - num_original = 0; - if (C_sizes_idx_1 > 0) { - num_original = C_sizes_idx_1; - } - } - - empty_non_axis_sizes = (num_original == 0); - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - br = 3; - } else { - br = 0; - } - - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - nx = 1; - } else { - nx = 0; - } - - ar = (int32_T)(br + nx); - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - d_result_data[(int32_T)(b_m + (int32_T)(ar * i))] = varargin_1_data - [(int32_T)((int32_T)(br * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(nx - 1); b_m++) { - d_result_data[(int32_T)((int32_T)(b_m + br) + (int32_T)(ar * i))] = - 1.0F; - } - } - - for (i = 0; i < 3; i++) { - global_tf_camera1[(int32_T)(i << 2)] = S[(int32_T)(3 * i)]; - global_tf_camera1[(int32_T)(1 + (int32_T)(i << 2))] = S[(int32_T) - ((int32_T)(3 * i) + 1)]; - global_tf_camera1[(int32_T)(2 + (int32_T)(i << 2))] = S[(int32_T) - ((int32_T)(3 * i) + 2)]; - global_tf_camera1[(int32_T)(12 + i)] = 0.0F; - } - - global_tf_camera1[3] = 0.0F; - global_tf_camera1[7] = 0.0F; - global_tf_camera1[11] = 0.0F; - global_tf_camera1[15] = 1.0F; - if (ar == 1) { - z_est_sizes_idx_1 = num_original; - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - newerr_data[(int32_T)(i + (int32_T)(b_m << 2))] = 0.0F; - newerr_data[(int32_T)(i + (int32_T)(b_m << 2))] += - global_tf_camera1[i] * d_result_data[b_m]; - newerr_data[(int32_T)(i + (int32_T)(b_m << 2))] += - global_tf_camera1[(int32_T)(i + 4)] * d_result_data[(int32_T)(1 + - b_m)]; - newerr_data[(int32_T)(i + (int32_T)(b_m << 2))] += - global_tf_camera1[(int32_T)(i + 8)] * d_result_data[(int32_T)(2 + - b_m)]; - newerr_data[(int32_T)(i + (int32_T)(b_m << 2))] += - global_tf_camera1[(int32_T)(i + 12)] * d_result_data[(int32_T)(3 + - b_m)]; - } - } - } else { - z_est_sizes_idx_1 = (int32_T)(int8_T)num_original; - for (i = 0; i <= (int32_T)(z_est_sizes_idx_1 - 1); i++) { - newerr_data[(int32_T)(i << 2)] = 0.0F; - newerr_data[(int32_T)(1 + (int32_T)(i << 2))] = 0.0F; - newerr_data[(int32_T)(2 + (int32_T)(i << 2))] = 0.0F; - newerr_data[(int32_T)(3 + (int32_T)(i << 2))] = 0.0F; - } - - if (num_original != 0) { - num_original = (int32_T)((int32_T)(num_original - 1) << 2); - for (br = 0; br <= num_original; br += 4) { - for (nx = (int32_T)(br + 1); nx <= (int32_T)(br + 4); nx++) { - newerr_data[(int32_T)(nx - 1)] = 0.0F; - } - } - - br = 0; - for (nx = 0; nx <= num_original; nx += 4) { - ar = 0; - for (i = br; (int32_T)(i + 1) <= (int32_T)(br + 4); i++) { - if (d_result_data[i] != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = nx; (int32_T)(O_sizes_idx_0 + 1) <= - (int32_T)(nx + 4); O_sizes_idx_0++) { - s12_iter++; - newerr_data[O_sizes_idx_0] += global_tf_camera1[(int32_T) - (s12_iter - 1)] * d_result_data[i]; - } - } - - ar += 4; - } - - br += 4; - } - } - } - - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[br] != 0) { - nx++; - } - } - - C_sizes_idx_1 = nx; - for (i = 0; i <= (int32_T)(z_est_sizes_idx_1 - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(handrail_knowledge_dims - 1); b_m++) { - est_estimator_B->x_data[(int32_T)(b_m + (int32_T) - (handrail_knowledge_dims * i))] = newerr_data[(int32_T)((int32_T)(i << - 2) + b_m)]; - } - } - - nx = (int32_T)(handrail_knowledge_dims * z_est_sizes_idx_1); - numFeatures = (uint8_T)(int32_T)(C_sizes_idx_1 * handrail_knowledge_dims); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - r_data[br] = est_estimator_B->x_data[br]; - } - - for (i = 0; i < 12; i++) { - next_ml_tf_global[i] = (real_T)t[i]; - } - - // move to camera frame and with half a frame rotation - // TODO: we should use the velocity and omega from the time of the - // registration pulse - // camera_angle = eulers_to_quat(cam_omega(1), cam_omega(2), cam_omega(3)); - rtb_ml_vel_aug_0[0] = rtb_ml_vel_aug[0]; - rtb_ml_vel_aug_0[1] = rtb_ml_vel_aug[1]; - rtb_ml_vel_aug_0[2] = rtb_ml_vel_aug[2]; - gdjmmglnmgdjlfkf_quat_rotation_vec(rtb_ml_vel_aug_0, - est_estimator_P->tun_abp_q_body2perchcam, tmp); - kngldbimhdbaimgd_quat_propagate_step(UnitDelay_DSTATE_n_ml_quat_ISS2, tmp, - est_estimator_P->tun_ase_ml_forward_projection_time, - rtb_VectorConcatenate_o); - ecjedbaiaiekohln_quaternion_to_rotation(rtb_VectorConcatenate_o, (real_T *) - &next_ml_tf_global[0]); - for (i = 0; i < 3; i++) { - C[(int32_T)(3 * i)] = (real32_T)-camera_ml_tf_global[(int32_T)(3 * i)]; - C[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)-camera_ml_tf_global - [(int32_T)((int32_T)(3 * i) + 1)]; - C[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)-camera_ml_tf_global - [(int32_T)((int32_T)(3 * i) + 2)]; - rtb_P_B_ISS_ISS[i] = est_estimator_P->tun_ase_ml_forward_projection_time - * (real32_T)rtb_ml_omega_aug[i] + UnitDelay_DSTATE_n_ml_P_cam_ISS[i]; - } - - for (i = 0; i < 3; i++) { - next_ml_tf_global[(int32_T)(9 + i)] = (real_T)((C[(int32_T)(i + 3)] * - rtb_P_B_ISS_ISS[1] + C[i] * rtb_P_B_ISS_ISS[0]) + C[(int32_T)(i + 6)] * - rtb_P_B_ISS_ISS[2]); - } - - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[br] != 0) { - nx++; - } - } - - C_sizes_idx_1 = nx; - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[br] != 0) { - e_data[nx] = (int8_T)(int32_T)(br + 1); - nx++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[(int32_T)(3 * i)] = rtb_hr_global_landmarks[(int32_T) - ((int32_T)e_data[i] - 1)]; - varargin_1_data[(int32_T)(1 + (int32_T)(3 * i))] = - rtb_hr_global_landmarks[(int32_T)((int32_T)e_data[i] + 49)]; - varargin_1_data[(int32_T)(2 + (int32_T)(3 * i))] = - rtb_hr_global_landmarks[(int32_T)((int32_T)e_data[i] + 99)]; - } - - if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else { - num_original = 0; - if (C_sizes_idx_1 > 0) { - num_original = C_sizes_idx_1; - } - } - - rtb_Compare = (num_original == 0); - if (rtb_Compare || (!(C_sizes_idx_1 == 0))) { - br = 3; - } else { - br = 0; - } - - if (rtb_Compare || (!(C_sizes_idx_1 == 0))) { - nx = 1; - } else { - nx = 0; - } - - // Landmarks in the camera frame - // z_next = next_landmarks; - // Find the difference between the current landmark locations and the - // distorted landmark locations due to time error, and rotate into the - // handrail frame - ar = (int32_T)(br + nx); - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - rtb_global_points[(int32_T)(b_m + (int32_T)(ar * i))] = - varargin_1_data[(int32_T)((int32_T)(br * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(nx - 1); b_m++) { - rtb_global_points[(int32_T)((int32_T)(b_m + br) + (int32_T)(ar * i))] = - 1.0F; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] = - camera_landmarks_data[(int32_T)((int32_T)(3 * b_m) + i)] - - (((rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 1)] * (real32_T) - next_ml_tf_global[(int32_T)(i + 3)] + rtb_global_points[(int32_T) - (ar * b_m)] * (real32_T)next_ml_tf_global[i]) + - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 2)] * (real32_T) - next_ml_tf_global[(int32_T)(i + 6)]) + rtb_global_points[(int32_T) - ((int32_T)(ar * b_m) + 3)] * (real32_T)next_ml_tf_global[(int32_T) - (i + 9)]); - } - } - - c_sz_idx_0 = (int8_T)num_original; - z_est_sizes_idx_1 = (int32_T)(int8_T)num_original; - for (i = 0; i <= (int32_T)(z_est_sizes_idx_1 - 1); i++) { - y_data[(int32_T)(3 * i)] = 0.0F; - y_data[(int32_T)(1 + (int32_T)(3 * i))] = 0.0F; - y_data[(int32_T)(2 + (int32_T)(3 * i))] = 0.0F; - } - - if (num_original != 0) { - num_original = (int32_T)((int32_T)(num_original - 1) * 3); - for (br = 0; br <= num_original; br += 3) { - for (nx = (int32_T)(br + 1); nx <= (int32_T)(br + 3); nx++) { - y_data[(int32_T)(nx - 1)] = 0.0F; - } - } - - br = 0; - for (nx = 0; nx <= num_original; nx += 3) { - ar = 0; - for (i = br; (int32_T)(i + 1) <= (int32_T)(br + 3); i++) { - if (next_landmarks_data[i] != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = nx; (int32_T)(O_sizes_idx_0 + 1) <= (int32_T) - (nx + 3); O_sizes_idx_0++) { - s12_iter++; - y_data[O_sizes_idx_0] += S[(int32_T)(s12_iter - 1)] * - next_landmarks_data[i]; - } - } - - ar += 3; - } - - br += 3; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(z_est_sizes_idx_1 - 1); b_m++) { - landmark_error_rail_frame_data[(int32_T)(b_m + (int32_T)((int32_T) - c_sz_idx_0 * i))] = y_data[(int32_T)((int32_T)(3 * b_m) + i)]; - } - } - - nx = 0; - for (ar = 0; ar < 50; ar++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[ar] != 0) { - nx++; - } - } - - C_sizes_idx_1 = nx; - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(z_est_sizes_idx_1 - 1); b_m++) { - est_estimator_B->x_data[(int32_T)(b_m + (int32_T)((int32_T)c_sz_idx_0 * - i))] = landmark_error_rail_frame_data[(int32_T)((int32_T)((int32_T) - c_sz_idx_0 * i) + b_m)]; - } - } - - nx = (int32_T)((int32_T)c_sz_idx_0 * handrail_knowledge_dims); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - varargin_1_data[br] = est_estimator_B->x_data[br]; - } - - nohlcjekmohddjmg_abs(varargin_1_data, (int32_T)(uint8_T)(int32_T) - (C_sizes_idx_1 * handrail_knowledge_dims), - next_landmarks_data, &i); - - // compute Jacobian - br = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[nx] != 0) { - br++; - } - } - - z_est_sizes_idx_1 = (int32_T)(br * handrail_knowledge_dims); - C_sizes_idx_1 = (int32_T)(br * handrail_knowledge_dims); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - r_vec_data[i] = 0.0; - } - - num_original = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[br] != 0) { - num_original++; - } - } - - H_sizes[0] = (int32_T)(handrail_knowledge_dims * num_original); - H_sizes[1] = 6; - C_sizes_idx_1 = (int32_T)((int32_T)(handrail_knowledge_dims * num_original) - * 6); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - H_data[i] = 0.0F; - } - - num_original = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_handrail_msg->cvs_valid_flag[br] != 0) { - num_original++; - } - } - - br = num_original; - for (num_original = 0; num_original <= (int32_T)(br - 1); num_original++) - { - // temp = 1.0 ./ camera_landmarks(3, i) * [eye(2, 2) -z_est(:, i)]; - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - // rotate to new coordinate system - nx = (int32_T)((int32_T)((int32_T)(1 + num_original) * - handrail_knowledge_dims) - handrail_knowledge_dims); - if ((int32_T)(nx + 1) > (int32_T)((int32_T)(1 + num_original) * - handrail_knowledge_dims)) { - nx = 0; - } - - tmp_7[0] = 0.0F; - tmp_7[3] = -camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) - + 2)]; - tmp_7[6] = camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) + - 1)]; - tmp_7[1] = camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) + - 2)]; - tmp_7[4] = 0.0F; - tmp_7[7] = -camera_landmarks_data[(int32_T)(3 * num_original)]; - tmp_7[2] = -camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) - + 1)]; - tmp_7[5] = camera_landmarks_data[(int32_T)(3 * num_original)]; - tmp_7[8] = 0.0F; - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m < 3; b_m++) { - C[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - C[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_7[(int32_T)(3 * b_m)] * - (real32_T)v[i]; - C[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_7[(int32_T)((int32_T)(3 * - b_m) + 1)] * (real32_T)v[(int32_T)(i + 3)]; - C[(int32_T)(i + (int32_T)(3 * b_m))] += tmp_7[(int32_T)((int32_T)(3 * - b_m) + 2)] * (real32_T)v[(int32_T)(i + 6)]; - rtb_Assignment_b[(int32_T)(i + (int32_T)(3 * b_m))] = (real32_T) - ((camera_ml_tf_global[(int32_T)((int32_T)(3 * b_m) + 1)] * (real_T) - c_a[(int32_T)(i + 3)] + camera_ml_tf_global[(int32_T)(3 * b_m)] * - (real_T)c_a[i]) + camera_ml_tf_global[(int32_T)((int32_T)(3 * - b_m) + 2)] * (real_T)c_a[(int32_T)(i + 6)]); - } - } - - for (i = 0; i < 3; i++) { - v_0[(int32_T)(3 * i)] = C[(int32_T)(3 * i)]; - v_0[(int32_T)(3 * (int32_T)(i + 3))] = rtb_Assignment_b[(int32_T)(3 * - i)]; - v_0[(int32_T)(1 + (int32_T)(3 * i))] = C[(int32_T)((int32_T)(3 * i) + - 1)]; - v_0[(int32_T)(1 + (int32_T)(3 * (int32_T)(i + 3)))] = - rtb_Assignment_b[(int32_T)((int32_T)(3 * i) + 1)]; - v_0[(int32_T)(2 + (int32_T)(3 * i))] = C[(int32_T)((int32_T)(3 * i) + - 2)]; - v_0[(int32_T)(2 + (int32_T)(3 * (int32_T)(i + 3)))] = - rtb_Assignment_b[(int32_T)((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m < 6; b_m++) { - S_0[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - S_0[(int32_T)(i + (int32_T)(3 * b_m))] += v_0[(int32_T)(3 * b_m)] * - S[i]; - S_0[(int32_T)(i + (int32_T)(3 * b_m))] += v_0[(int32_T)((int32_T)(3 * - b_m) + 1)] * S[(int32_T)(i + 3)]; - S_0[(int32_T)(i + (int32_T)(3 * b_m))] += v_0[(int32_T)((int32_T)(3 * - b_m) + 2)] * S[(int32_T)(i + 6)]; - } - } - - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m <= (int32_T)(handrail_knowledge_dims - 1); b_m++) { - H_data[(int32_T)((int32_T)(nx + b_m) + (int32_T)(H_sizes[0] * i))] = - S_0[(int32_T)((int32_T)(3 * i) + b_m)]; - } - } - - // drop last row, since we don't have knowledge along handrail axis - // make the confidence depend on mapped landmarked error which is function - // of distance - // [??] Should the camera_landmarks distance be the RSS, not just Z? - // r_vec is measurment noise - nx = (int32_T)((int32_T)((int32_T)(1 + num_original) * - handrail_knowledge_dims) - handrail_knowledge_dims); - ar = (int32_T)((int32_T)(1 + num_original) * handrail_knowledge_dims); - if ((int32_T)(nx + 1) > ar) { - nx = 0; - ar = 0; - } - - i = (int32_T)((int32_T)((int32_T)((int32_T)(1 + num_original) * - handrail_knowledge_dims) - handrail_knowledge_dims) + 1); - s12_iter = (int32_T)((int32_T)(1 + num_original) * - handrail_knowledge_dims); - if (i > s12_iter) { - i = 1; - s12_iter = 0; - } - - O_sizes_idx_0 = (int32_T)((int32_T)(int16_T)(int32_T)((int32_T)(int16_T) - s12_iter - (int32_T)(int16_T)i) + 1); - C_sizes_idx_1 = (int32_T)(int16_T)(int32_T)((int32_T)(int16_T)s12_iter - - (int32_T)(int16_T)i); - for (b_m = 0; b_m <= C_sizes_idx_1; b_m++) { - p_data[b_m] = (int16_T)(int32_T)((int32_T)(int16_T)(int32_T)((int32_T) - (int16_T)i + b_m) - 1); - } - - hr_quat_ISS2hr_idx_1 = est_estimator_P->tun_ase_hr_distance_r * - (real32_T)fabs((real_T)camera_landmarks_data[2]) + - est_estimator_P->tun_ase_hr_r_mag; - C_sizes_idx_1 = (int32_T)(ar - nx); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[i] = next_landmarks_data[(int32_T)(nx + i)] + - hr_quat_ISS2hr_idx_1; - } - - djmgjecbcbiengln_power(varargin_1_data, (int32_T)(ar - nx), - rtb_hr_global_landmarks, &i); - for (i = 0; i <= (int32_T)(O_sizes_idx_0 - 1); i++) { - r_vec_data[(int32_T)p_data[i]] = (real_T)rtb_hr_global_landmarks[i]; - } - } - - // Compress the r and H - jmohiecblfcjnohl_qr(H_data, H_sizes, est_estimator_B->q1_data, q1_sizes, - T_H_data, T_H_sizes); - C_sizes_idx_1 = q1_sizes[0]; - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - H_data[(int32_T)(b_m + (int32_T)(C_sizes_idx_1 * i))] = - est_estimator_B->q1_data[(int32_T)((int32_T)(q1_sizes[0] * i) + b_m)]; - } - } - - num_original = q1_sizes[0]; - br = q1_sizes[0]; - for (i = 0; i <= (int32_T)(br - 1); i++) { - for (b_m = 0; b_m < 6; b_m++) { - b_a_data[(int32_T)(b_m + (int32_T)(6 * i))] = H_data[(int32_T) - ((int32_T)(C_sizes_idx_1 * b_m) + i)]; - } - } - - if ((q1_sizes[0] == 1) || ((int32_T)numFeatures == 1)) { - for (i = 0; i < 6; i++) { - rtb_Product_j[i] = 0.0F; - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - rtb_Product_j[i] += b_a_data[(int32_T)((int32_T)(6 * b_m) + i)] * - r_data[b_m]; - } - } - } else { - for (handrail_knowledge_dims = 0; handrail_knowledge_dims < 6; - handrail_knowledge_dims++) { - rtb_Product_j[handrail_knowledge_dims] = 0.0F; - } - - handrail_knowledge_dims = 0; - for (num_original = 0; (int32_T)(num_original + 1) <= C_sizes_idx_1; - num_original++) { - if (r_data[num_original] != 0.0F) { - br = handrail_knowledge_dims; - for (nx = 0; nx < 6; nx++) { - br++; - rtb_Product_j[nx] += b_a_data[(int32_T)(br - 1)] * - r_data[num_original]; - } - } - - handrail_knowledge_dims += 6; - } - } - - jekfopppngdbhlng_diag(r_vec_data, z_est_sizes_idx_1, - est_estimator_B->tmp_data, H_sizes); - num_original = H_sizes[1]; - for (i = 0; i < 6; i++) { - br = H_sizes[1]; - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - b_a_data[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 <= (int32_T)(C_sizes_idx_1 - 1); i_0++) { - b_a_data[(int32_T)(i + (int32_T)(6 * b_m))] += H_data[(int32_T) - ((int32_T)(C_sizes_idx_1 * i) + i_0)] * (real32_T) - est_estimator_B->tmp_data[(int32_T)((int32_T)(H_sizes[0] * b_m) + - i_0)]; - } - } - } - - if ((H_sizes[1] == 1) || (q1_sizes[0] == 1)) { - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 6; b_m++) { - rtb_R_mat[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 <= (int32_T)(num_original - 1); i_0++) { - rtb_R_mat[(int32_T)(i + (int32_T)(6 * b_m))] += b_a_data[(int32_T) - ((int32_T)(6 * i_0) + i)] * H_data[(int32_T)((int32_T) - (C_sizes_idx_1 * b_m) + i_0)]; - } - } - } - } else { - memset(&rtb_R_mat[0], 0, (uint32_T)(36U * sizeof(real32_T))); - for (handrail_knowledge_dims = 0; handrail_knowledge_dims <= 31; - handrail_knowledge_dims += 6) { - for (br = handrail_knowledge_dims; (int32_T)(br + 1) <= (int32_T) - (handrail_knowledge_dims + 6); br++) { - rtb_R_mat[br] = 0.0F; - } - } - - handrail_knowledge_dims = 0; - for (br = 0; br <= 31; br += 6) { - nx = 0; - ar = (int32_T)(handrail_knowledge_dims + num_original); - for (i = handrail_knowledge_dims; (int32_T)(i + 1) <= ar; i++) { - if (est_estimator_B->q1_data[(int32_T)(i % q1_sizes[0] + (int32_T) - (q1_sizes[0] * div_nzp_s32_floor(i, q1_sizes[0])))] != 0.0F) { - s12_iter = nx; - for (O_sizes_idx_0 = br; (int32_T)(O_sizes_idx_0 + 1) <= (int32_T) - (br + 6); O_sizes_idx_0++) { - s12_iter++; - rtb_R_mat[O_sizes_idx_0] += est_estimator_B->q1_data[(int32_T)(i - % q1_sizes[0] + (int32_T)(q1_sizes[0] * div_nzp_s32_floor(i, - q1_sizes[0])))] * b_a_data[(int32_T)(s12_iter - 1)]; - } - } - - nx += 6; - } - - handrail_knowledge_dims += num_original; - } - } - - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Residual and H' - - // Convert H to be in terms of the entire state vector - // ':1:3' - est_estimator_B->error_out = 0; - for (i = 0; i < 6; i++) { - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Residual and H' - - est_estimator_B->r_out[i] = rtb_Product_j[i]; - - // MATLAB Function: '/Compute Residual and H' - for (b_m = 0; b_m < 6; b_m++) { - // SignalConversion: '/Signal Conversion' - est_estimator_B->H_out[(int32_T)(b_m + (int32_T)(6 * i))] = T_H_data - [(int32_T)((int32_T)(T_H_sizes[0] * i) + b_m)]; - } - } - - // MATLAB Function: '/Compute Residual and H' - for (i = 0; i < 111; i++) { - for (b_m = 0; b_m < 6; b_m++) { - // SignalConversion: '/Signal Conversion' - est_estimator_B->H_out[(int32_T)(b_m + (int32_T)(6 * (int32_T)(i + 6)))] - = 0.0F; - } - } - - // SignalConversion: '/Signal Conversion' - memcpy(&est_estimator_B->R_mat[0], &rtb_R_mat[0], (uint32_T)(36U * sizeof - (real32_T))); - - // Sum: '/Sum' - UnitDelay_DSTATE_aug_state_enum = (uint32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[0]; - for (ar = 0; ar < 49; ar++) { - UnitDelay_DSTATE_aug_state_enum += (uint32_T) - est_estimator_U_handrail_msg->cvs_valid_flag[(int32_T)(ar + 1)]; - } - - // SignalConversion: '/Signal Conversion' incorporates: - // Sum: '/Sum' - - numFeatures = (uint8_T)UnitDelay_DSTATE_aug_state_enum; - - // SignalConversion: '/Signal Conversion' incorporates: - // Constant: '/Constant' - - memcpy(&rtb_UnitDelay18[0], &est_estimator_P->Constant_Value[0], (uint32_T) - (50U * sizeof(real_T))); - - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Global positions of Handrail Features' - - hr_P_hr_ISS_ISS[0] = est_estimator_DW->hr_P_hr_ISS_ISS_pers[0]; - hr_P_hr_ISS_ISS[1] = est_estimator_DW->hr_P_hr_ISS_ISS_pers[1]; - hr_P_hr_ISS_ISS[2] = est_estimator_DW->hr_P_hr_ISS_ISS_pers[2]; - - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Global positions of Handrail Features' - - hr_quat_ISS2hr_idx_0 = est_estimator_DW->hr_quat_ISS2hr_pers[0]; - hr_quat_ISS2hr_idx_1 = est_estimator_DW->hr_quat_ISS2hr_pers[1]; - hr_quat_ISS2hr_idx_2 = est_estimator_DW->hr_quat_ISS2hr_pers[2]; - hr_quat_ISS2hr_idx_3 = est_estimator_DW->hr_quat_ISS2hr_pers[3]; - - // End of Outputs for SubSystem: '/HR_Compute_Residual_and_H1' - } else { - // Outputs for IfAction SubSystem: '/ML_Compute_Residual_and_H' incorporates: - // ActionPort: '/Action Port' - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - - rtb_Compare = (est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_map); - - // Switch: '/Switch1' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant3' - - if (rtb_Compare) { - rtb_Switch1 = est_estimator_P->tun_ase_navcam_inv_focal_length; - } else { - rtb_Switch1 = est_estimator_P->tun_ase_dockcam_inv_focal_length; - } - - // End of Switch: '/Switch1' - - // MATLAB Function: '/Compute Residual and H' incorporates: - // Inport: '/landmark_msg' - - // MATLAB Function 'camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/ML_Compute_Residual_and_H/Compute Residual and H': ':1' - // ':1:3' - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - br = 0; - for (num_original = 0; num_original < 50; num_original++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[num_original] - != 0) { - br++; - } - } - - C_sizes_idx_1 = br; - br = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[nx] != 0) { - e_data[br] = (int8_T)(int32_T)(nx + 1); - br++; - } - } - - O_sizes_idx_0 = C_sizes_idx_1; - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - r_data_0[i] = est_estimator_U_landmark_msg->cvs_observations[(int32_T) - ((int32_T)e_data[i] - 1)] * rtb_Switch1; - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - r_data_0[(int32_T)(i + C_sizes_idx_1)] = - est_estimator_U_landmark_msg->cvs_observations[(int32_T)((int32_T) - e_data[i] + 49)] * rtb_Switch1; - } - - for (i = 0; i < 12; i++) { - camera_ml_tf_global[i] = (real_T)g_0[i]; - } - - fkfcbaiengdjgdje_quaternion_to_rotation(UnitDelay_DSTATE_n_ml_quat_ISS2, - rtb_Product1_0); - for (i = 0; i < 3; i++) { - C[(int32_T)(3 * i)] = (real32_T)-(real_T)rtb_Product1_0[(int32_T)(3 * i)]; - camera_ml_tf_global[(int32_T)(3 * i)] = (real_T)rtb_Product1_0[(int32_T) - (3 * i)]; - C[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)-(real_T)rtb_Product1_0 - [(int32_T)((int32_T)(3 * i) + 1)]; - camera_ml_tf_global[(int32_T)(1 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)]; - C[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)-(real_T)rtb_Product1_0 - [(int32_T)((int32_T)(3 * i) + 2)]; - camera_ml_tf_global[(int32_T)(2 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i < 3; i++) { - camera_ml_tf_global[(int32_T)(9 + i)] = (real_T)((C[(int32_T)(i + 3)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] + C[i] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[0]) + C[(int32_T)(i + 6)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[2]); - } - - num_original = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[nx] != 0) { - num_original++; - } - } - - C_sizes_idx_1 = num_original; - num_original = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[nx] != 0) { - e_data[num_original] = (int8_T)(int32_T)(nx + 1); - num_original++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[(int32_T)(3 * i)] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] - 1)]; - varargin_1_data[(int32_T)(1 + (int32_T)(3 * i))] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] + 49)]; - varargin_1_data[(int32_T)(2 + (int32_T)(3 * i))] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] + 99)]; - } - - if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else if (!(C_sizes_idx_1 == 0)) { - num_original = C_sizes_idx_1; - } else { - num_original = 0; - if (C_sizes_idx_1 > 0) { - num_original = C_sizes_idx_1; - } - } - - empty_non_axis_sizes = (num_original == 0); - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - br = 3; - } else { - br = 0; - } - - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - nx = 1; - } else { - nx = 0; - } - - ar = (int32_T)(br + nx); - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - rtb_global_points[(int32_T)(b_m + (int32_T)(ar * i))] = - varargin_1_data[(int32_T)((int32_T)(br * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(nx - 1); b_m++) { - rtb_global_points[(int32_T)((int32_T)(b_m + br) + (int32_T)(ar * i))] = - 1.0F; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)(ar * b_m)] * (real32_T) - camera_ml_tf_global[i]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 1)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 3)]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 2)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 6)]; - camera_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 3)] * (real32_T) - camera_ml_tf_global[(int32_T)(i + 9)]; - } - } - - // Landmarks in the camera frame - handrail_knowledge_dims = 0; - z_est_sizes_idx_1 = num_original; - for (i = 0; i <= (int32_T)(num_original - 1); i++) { - z_est_data[(int32_T)(i << 1)] = camera_landmarks_data[(int32_T)(3 * i)] / - camera_landmarks_data[(int32_T)((int32_T)(3 * i) + 2)]; - z_est_data[(int32_T)(1 + (int32_T)(i << 1))] = camera_landmarks_data - [(int32_T)((int32_T)(3 * i) + 1)] / camera_landmarks_data[(int32_T) - ((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i <= (int32_T)(O_sizes_idx_0 - 1); i++) { - omega_error_data[(int32_T)(i << 1)] = r_data_0[i] - z_est_data[(int32_T) - (i << 1)]; - omega_error_data[(int32_T)(1 + (int32_T)(i << 1))] = r_data_0[(int32_T) - (i + O_sizes_idx_0)] - z_est_data[(int32_T)((int32_T)(i << 1) + 1)]; - } - - nx = (int32_T)(O_sizes_idx_0 << 1); - s12_iter = (int32_T)(int8_T)(int32_T)(O_sizes_idx_0 << 1); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - r_data_0[br] = omega_error_data[br]; - } - - // forward propagate the saved state based on velocity to add an error - // term for the timing of the registration pulse - for (i = 0; i < 12; i++) { - next_ml_tf_global[i] = (real_T)g_0[i]; - } - - // move to camera frame and with half a frame rotation - // TODO: we should use the velocity and omega from the time of the - // registration pulse - rtb_ml_vel_aug_0[0] = rtb_ml_vel_aug[0]; - rtb_ml_vel_aug_0[1] = rtb_ml_vel_aug[1]; - rtb_ml_vel_aug_0[2] = rtb_ml_vel_aug[2]; - - // Switch: '/Switch' - if (rtb_Compare) { - // MATLAB Function: '/Compute Residual and H' incorporates: - // Constant: '/Constant' - - rtb_Compare_2[0] = est_estimator_P->tun_abp_q_body2navcam[0]; - rtb_Compare_2[1] = est_estimator_P->tun_abp_q_body2navcam[1]; - rtb_Compare_2[2] = est_estimator_P->tun_abp_q_body2navcam[2]; - rtb_Compare_2[3] = est_estimator_P->tun_abp_q_body2navcam[3]; - } else { - // MATLAB Function: '/Compute Residual and H' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - - rtb_Compare_2[0] = est_estimator_P->tun_abp_q_body2dockcam[0]; - rtb_Compare_2[1] = est_estimator_P->tun_abp_q_body2dockcam[1]; - rtb_Compare_2[2] = est_estimator_P->tun_abp_q_body2dockcam[2]; - rtb_Compare_2[3] = est_estimator_P->tun_abp_q_body2dockcam[3]; - } - - // End of Switch: '/Switch' - - // MATLAB Function: '/Compute Residual and H' incorporates: - // Constant: '/Constant' - // Inport: '/landmark_msg' - // RelationalOperator: '/Compare' - - gdjmmglnmgdjlfkf_quat_rotation_vec(rtb_ml_vel_aug_0, rtb_Compare_2, tmp); - baieimopcbaiaaai_eulers_to_quat - (est_estimator_P->tun_ase_ml_forward_projection_time * (real32_T)tmp[0], - est_estimator_P->tun_ase_ml_forward_projection_time * (real32_T)tmp[1], - est_estimator_P->tun_ase_ml_forward_projection_time * (real32_T)tmp[2], - rtb_Product1); - iecjopppiecjmgln_quatmult(rtb_Product1, UnitDelay_DSTATE_n_ml_quat_ISS2, - rtb_Compare_2); - fkfcbaiengdjgdje_quaternion_to_rotation(rtb_Compare_2, rtb_Product1_0); - for (i = 0; i < 3; i++) { - next_ml_tf_global[(int32_T)(3 * i)] = (real_T)rtb_Product1_0[(int32_T)(3 - * i)]; - C[(int32_T)(3 * i)] = (real32_T)-camera_ml_tf_global[(int32_T)(3 * i)]; - next_ml_tf_global[(int32_T)(1 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)]; - C[(int32_T)(1 + (int32_T)(3 * i))] = (real32_T)-camera_ml_tf_global - [(int32_T)((int32_T)(3 * i) + 1)]; - next_ml_tf_global[(int32_T)(2 + (int32_T)(3 * i))] = (real_T) - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)]; - C[(int32_T)(2 + (int32_T)(3 * i))] = (real32_T)-camera_ml_tf_global - [(int32_T)((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i < 3; i++) { - next_ml_tf_global[(int32_T)(9 + i)] = (real_T)(((C[(int32_T)(i + 3)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] + C[i] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[0]) + C[(int32_T)(i + 6)] * - UnitDelay_DSTATE_n_ml_P_cam_ISS[2]) + - est_estimator_P->tun_ase_ml_forward_projection_time * (real32_T) - rtb_ml_omega_aug[i]); - } - - nx = 0; - for (ar = 0; ar < 50; ar++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[ar] != 0) { - nx++; - } - } - - C_sizes_idx_1 = nx; - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[br] != 0) { - e_data[nx] = (int8_T)(int32_T)(br + 1); - nx++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - varargin_1_data[(int32_T)(3 * i)] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] - 1)]; - varargin_1_data[(int32_T)(1 + (int32_T)(3 * i))] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] + 49)]; - varargin_1_data[(int32_T)(2 + (int32_T)(3 * i))] = - est_estimator_U_landmark_msg->cvs_landmarks[(int32_T)((int32_T) - e_data[i] + 99)]; - } - - if (!(C_sizes_idx_1 == 0)) { - nx = C_sizes_idx_1; - } else if (!(C_sizes_idx_1 == 0)) { - nx = C_sizes_idx_1; - } else { - nx = 0; - if (C_sizes_idx_1 > 0) { - nx = C_sizes_idx_1; - } - } - - empty_non_axis_sizes = (nx == 0); - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - num_original = 3; - } else { - num_original = 0; - } - - if (empty_non_axis_sizes || (!(C_sizes_idx_1 == 0))) { - br = 1; - } else { - br = 0; - } - - ar = (int32_T)(num_original + br); - for (i = 0; i <= (int32_T)(nx - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - rtb_global_points[(int32_T)(b_m + (int32_T)(ar * i))] = - varargin_1_data[(int32_T)((int32_T)(num_original * i) + b_m)]; - } - } - - for (i = 0; i <= (int32_T)(nx - 1); i++) { - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - rtb_global_points[(int32_T)((int32_T)(b_m + num_original) + (int32_T) - (ar * i))] = 1.0F; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(nx - 1); b_m++) { - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] = 0.0F; - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)(ar * b_m)] * (real32_T) - next_ml_tf_global[i]; - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 1)] * (real32_T) - next_ml_tf_global[(int32_T)(i + 3)]; - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 2)] * (real32_T) - next_ml_tf_global[(int32_T)(i + 6)]; - next_landmarks_data[(int32_T)(i + (int32_T)(3 * b_m))] += - rtb_global_points[(int32_T)((int32_T)(ar * b_m) + 3)] * (real32_T) - next_ml_tf_global[(int32_T)(i + 9)]; - } - } - - // Landmarks in the camera frame - for (i = 0; i <= (int32_T)(z_est_sizes_idx_1 - 1); i++) { - omega_error_data[(int32_T)(i << 1)] = z_est_data[(int32_T)(i << 1)] - - next_landmarks_data[(int32_T)(3 * i)] / next_landmarks_data[(int32_T) - ((int32_T)(3 * i) + 2)]; - omega_error_data[(int32_T)(1 + (int32_T)(i << 1))] = z_est_data[(int32_T) - ((int32_T)(i << 1) + 1)] - next_landmarks_data[(int32_T)((int32_T)(3 * - i) + 1)] / next_landmarks_data[(int32_T)((int32_T)(3 * i) + 2)]; - } - - nx = (int32_T)(z_est_sizes_idx_1 << 1); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - y_data_0[br] = omega_error_data[br]; - } - - iecjbieccbailfcb_abs(y_data_0, (int32_T)(int8_T)(int32_T)(O_sizes_idx_0 << - 1), omega_error_data, &i); - - // compute Jacobian - z_est_sizes_idx_1 = (int32_T)(O_sizes_idx_0 << 1); - C_sizes_idx_1 = (int32_T)(O_sizes_idx_0 << 1); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - r_vec_data_0[i] = 0.0; - } - - H_sizes[0] = (int32_T)(O_sizes_idx_0 << 1); - H_sizes[1] = 6; - C_sizes_idx_1 = (int32_T)((int32_T)(O_sizes_idx_0 << 1) * 6); - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - H_data_0[i] = 0.0F; - } - - for (num_original = 0; num_original <= (int32_T)(O_sizes_idx_0 - 1); - num_original++) { - hr_quat_ISS2hr_idx_0 = 1.0F / camera_landmarks_data[(int32_T)((int32_T) - (3 * num_original) + 2)]; - h[0] = 1.0F; - h[1] = 0.0F; - h[4] = -z_est_data[(int32_T)(num_original << 1)]; - h[2] = 0.0F; - h[3] = 1.0F; - h[5] = -z_est_data[(int32_T)((int32_T)(num_original << 1) + 1)]; - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - tmp_7[0] = 0.0F; - tmp_7[3] = -camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) - + 2)]; - tmp_7[6] = camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) + - 1)]; - tmp_7[1] = camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) + - 2)]; - tmp_7[4] = 0.0F; - tmp_7[7] = -camera_landmarks_data[(int32_T)(3 * num_original)]; - tmp_7[2] = -camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) - + 1)]; - tmp_7[5] = camera_landmarks_data[(int32_T)(3 * num_original)]; - tmp_7[8] = 0.0F; - for (i = 0; i < 3; i++) { - hr_quat_ISS2hr_idx_1 = h[(int32_T)(i << 1)] * hr_quat_ISS2hr_idx_0; - temp_0[(int32_T)(i << 1)] = -hr_quat_ISS2hr_idx_1; - temp[(int32_T)(i << 1)] = hr_quat_ISS2hr_idx_1; - hr_quat_ISS2hr_idx_1 = h[(int32_T)((int32_T)(i << 1) + 1)] * - hr_quat_ISS2hr_idx_0; - temp_0[(int32_T)(1 + (int32_T)(i << 1))] = -hr_quat_ISS2hr_idx_1; - temp[(int32_T)(1 + (int32_T)(i << 1))] = hr_quat_ISS2hr_idx_1; - } - - for (i = 0; i < 2; i++) { - for (b_m = 0; b_m < 3; b_m++) { - temp_1[(int32_T)(i + (int32_T)(b_m << 1))] = 0.0F; - temp_2[(int32_T)(i + (int32_T)(b_m << 1))] = 0.0F; - temp_1[(int32_T)(i + (int32_T)(b_m << 1))] += tmp_7[(int32_T)(3 * - b_m)] * temp[i]; - temp_2[(int32_T)(i + (int32_T)(b_m << 1))] += (real32_T) - camera_ml_tf_global[(int32_T)(3 * b_m)] * temp_0[i]; - temp_1[(int32_T)(i + (int32_T)(b_m << 1))] += tmp_7[(int32_T) - ((int32_T)(3 * b_m) + 1)] * temp[(int32_T)(i + 2)]; - temp_2[(int32_T)(i + (int32_T)(b_m << 1))] += (real32_T) - camera_ml_tf_global[(int32_T)((int32_T)(3 * b_m) + 1)] * temp_0 - [(int32_T)(i + 2)]; - temp_1[(int32_T)(i + (int32_T)(b_m << 1))] += tmp_7[(int32_T) - ((int32_T)(3 * b_m) + 2)] * temp[(int32_T)(i + 4)]; - temp_2[(int32_T)(i + (int32_T)(b_m << 1))] += (real32_T) - camera_ml_tf_global[(int32_T)((int32_T)(3 * b_m) + 2)] * temp_0 - [(int32_T)(i + 4)]; - } - } - - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 1) - 2); - for (b_m = 0; b_m < 3; b_m++) { - H_data_0[(int32_T)(i + (int32_T)(H_sizes[0] * b_m))] = temp_1[(int32_T) - (b_m << 1)]; - H_data_0[(int32_T)((int32_T)(i + (int32_T)(H_sizes[0] * b_m)) + 1)] = - temp_1[(int32_T)((int32_T)(b_m << 1) + 1)]; - } - - for (b_m = 0; b_m < 3; b_m++) { - H_data_0[(int32_T)(i + (int32_T)(H_sizes[0] * (int32_T)(b_m + 3)))] = - temp_2[(int32_T)(b_m << 1)]; - H_data_0[(int32_T)((int32_T)(i + (int32_T)(H_sizes[0] * (int32_T)(b_m - + 3))) + 1)] = temp_2[(int32_T)((int32_T)(b_m << 1) + 1)]; - } - - // account for distortion model - // if ru > 1e-5 - // distorted = O(i, :) * tan(ru * ase_distortion) / d2 / ru; - // radius = norm(distorted); - // rd = radius * ase_inv_focal_length; - // t = tan(ase_distortion * rd); - // f1 = f1 * 1 / (d2 * radius) * (distorted(2)^2 * t / rd + ase_distortion * distorted(1)^2 * (1 + t^2)); - // f2 = f2 * 1 / (d2 * radius) * (distorted(1)^2 * t / rd + ase_distortion * distorted(2)^2 * (1 + t^2)); - // end - if (rtb_Compare) { - // make the confidence depend on mapped landmarked error which is function - // of distance - hr_quat_ISS2hr_idx_0 = est_estimator_P->tun_ase_map_error / - camera_landmarks_data[(int32_T)((int32_T)(3 * num_original) + 2)] + - est_estimator_P->tun_ase_vis_r_mag; - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 1) - 2); - rtb_Switch1_1[0] = (hr_quat_ISS2hr_idx_0 + omega_error_data[i]) * - rtb_Switch1; - rtb_Switch1_1[1] = (omega_error_data[(int32_T)(1 + i)] + - hr_quat_ISS2hr_idx_0) * rtb_Switch1; - nohdcbaibiecnohl_power(rtb_Switch1_1, tmp_0); - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 1) - 2); - r_vec_data_0[i] = (real_T)tmp_0[0]; - r_vec_data_0[(int32_T)(1 + i)] = (real_T)tmp_0[1]; - } else { - // AR update - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 1) - 2); - rtb_Switch1_0[0] = (est_estimator_P->tun_ase_dock_r_mag + - omega_error_data[i]) * rtb_Switch1; - rtb_Switch1_0[1] = (omega_error_data[(int32_T)(1 + i)] + - est_estimator_P->tun_ase_dock_r_mag) * rtb_Switch1; - nohdcbaibiecnohl_power(rtb_Switch1_0, tmp_0); - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 1) - 2); - r_vec_data_0[i] = (real_T)tmp_0[0]; - r_vec_data_0[(int32_T)(1 + i)] = (real_T)tmp_0[1]; - } - } - - // r_out = zeros(size(L, 1),1); - for (i = 0; i < 50; i++) { - rtb_UnitDelay18[i] = (rtNaN); - } - - // only ignore observations via Malanobis check in the following - // conditions: - // 1. The estimator is converged - // 2. The estiamtor is performing a ML update - // 3. The mahalanobis check hasn't previously rejected the maximum number - // of consecutive frame allowed to be rejected set via tun_max_mahal_reject_frames - if ((rtb_Saturation_n == est_estimator_P->ase_status_converged) && - rtb_Compare && (est_estimator_DW->num_consecutive_rejected_meas < - (real_T)est_estimator_P->tun_max_mahal_reject_frames)) - { - // S = H*P_in(16:21, 16:21)*H'; - // for i=1:size(S, 1) - // S(i, i) = S(i, i) + r_vec(i); - // end - // S_inv = pinv(S); - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[br] != 0) { - nx++; - } - } - - br = (int32_T)(((real_T)nx * 2.0 + 1.0) / 2.0); - for (nx = 0; nx <= (int32_T)(br - 1); nx++) { - ar = (int32_T)(nx << 1); - for (i = 0; i < 6; i++) { - x[(int32_T)(i << 1)] = H_data_0[(int32_T)((int32_T)(H_sizes[0] * i) - + ar)]; - x[(int32_T)(1 + (int32_T)(i << 1))] = H_data_0[(int32_T)((int32_T) - ((int32_T)(H_sizes[0] * i) + ar) + 1)]; - } - - for (i = 0; i < 2; i++) { - for (b_m = 0; b_m < 6; b_m++) { - rtb_Product1_1[(int32_T)(i + (int32_T)(b_m << 1))] = 0.0F; - for (i_0 = 0; i_0 < 6; i_0++) { - rtb_Product1_1[(int32_T)(i + (int32_T)(b_m << 1))] += - est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T) - ((int32_T)(15 + b_m) * 117) + i_0) + 15)] * x[(int32_T) - ((int32_T)(i_0 << 1) + i)]; - } - - rtb_Product1_2[(int32_T)(b_m + (int32_T)(6 * i))] = H_data_0 - [(int32_T)((int32_T)(i + ar) + (int32_T)(H_sizes[0] * b_m))]; - } - } - - for (i = 0; i < 2; i++) { - for (b_m = 0; b_m < 2; b_m++) { - rtb_Product1[(int32_T)(i + (int32_T)(b_m << 1))] = 0.0F; - for (i_0 = 0; i_0 < 6; i_0++) { - rtb_Product1[(int32_T)(i + (int32_T)(b_m << 1))] += - rtb_Product1_1[(int32_T)((int32_T)(i_0 << 1) + i)] * - rtb_Product1_2[(int32_T)((int32_T)(6 * b_m) + i_0)]; - } - } - } - - H[0] = rtb_Product1[0] + (real32_T)r_vec_data_0[ar]; - H[1] = rtb_Product1[1]; - H[2] = rtb_Product1[2]; - H[3] = (real32_T)r_vec_data_0[(int32_T)(ar + 1)] + rtb_Product1[3]; - iekfiecjknopophd_pinv(H, rtb_Product1); - rtb_UnitDelay18[(int32_T)((int32_T)(((real_T)(int32_T)(ar + 1) + 1.0) / - 2.0) - 1)] = (real_T)(real32_T)sqrt((real_T)((r_data_0[(int32_T)(1 + - ar)] * rtb_Product1[1] + r_data_0[ar] * rtb_Product1[0]) * - r_data_0[ar] + (r_data_0[(int32_T)(1 + ar)] * rtb_Product1[3] + - r_data_0[ar] * rtb_Product1[2]) * r_data_0[(int32_T) - (1 + ar)])); - } - - nx = 0; - for (br = 0; br < 50; br++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[br] != 0) { - nx++; - } - } - - if (1 > nx) { - C_sizes_idx_1 = 0; - } else { - C_sizes_idx_1 = nx; - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - invalid_vec_data[i] = (rtb_UnitDelay18[i] > (real_T) - est_estimator_P->tun_ase_mahal_distance_max); - } - - // toss if nothing is within std. dev. of expected, ransac prob. failed - // if sum(mahal_dists < 3) == 0 - // invalid_vec(:) = true; - // end - br = 0; - for (nx = 0; nx < 50; nx++) { - if ((int32_T)est_estimator_U_landmark_msg->cvs_valid_flag[nx] != 0) { - br++; - } - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - g_result_data[(int32_T)(i << 1)] = invalid_vec_data[i]; - } - - for (i = 0; i <= (int32_T)(C_sizes_idx_1 - 1); i++) { - g_result_data[(int32_T)(1 + (int32_T)(i << 1))] = invalid_vec_data[i]; - } - - nx = (int32_T)(C_sizes_idx_1 << 1); - c_sz_idx_0 = (int8_T)(int32_T)(br << 1); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - invalid_data[br] = g_result_data[br]; - } - - mgdbbiekfknonglf_nullAssignment(r_data_0, &s12_iter, invalid_data, - (int32_T)c_sz_idx_0); - imohcjmoimopimoh_nullAssignment(H_data_0, H_sizes, invalid_data, - (int32_T)c_sz_idx_0); - ophlcjmgkfcbmohl_nullAssignment(r_vec_data_0, &z_est_sizes_idx_1, - invalid_data, (int32_T)c_sz_idx_0); - } - - i = (int32_T)rt_roundd_snf((real_T)s12_iter / 2.0); - if (i < 256) { - if (i >= 0) { - numFeatures = (uint8_T)i; - } else { - numFeatures = 0U; - } - } else { - numFeatures = MAX_uint8_T; - } - - if (rtb_Compare) { - rtb_Switch1 = est_estimator_P->tun_ase_min_ml_meas; - - // Sparse map update - } else { - rtb_Switch1 = est_estimator_P->tun_ase_min_ar_meas; - - // AR update - } - - // toss if not enough measurements, (Mahalanobis check removed all valid features) - // features - if (((real_T)s12_iter / 2.0 < (real_T)rtb_Switch1) || ((real_T)s12_iter < - (real_T)(int8_T)(int32_T)(O_sizes_idx_0 << 1) / 2.0)) { - numFeatures = 0U; - handrail_knowledge_dims = 1; - for (i = 0; i < 6; i++) { - rtb_Product_j[i] = 1.0F; - } - - ngdjjecbgdbaglfc_eye(rtb_H_out); - biekcjmgdbaadbim_eye(rtb_R_mat); - est_estimator_DW->num_consecutive_rejected_meas++; - } else { - est_estimator_DW->num_consecutive_rejected_meas = 0.0; - - // Compress the r and H - jmglopphppphkfkf_qr(H_data_0, H_sizes, est_estimator_B->q1_data_c, - q1_sizes, T_H_data_0, T_H_sizes); - C_sizes_idx_1 = q1_sizes[0]; - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - H_data_0[(int32_T)(b_m + (int32_T)(C_sizes_idx_1 * i))] = - est_estimator_B->q1_data_c[(int32_T)((int32_T)(q1_sizes[0] * i) + - b_m)]; - } - } - - num_original = q1_sizes[0]; - br = q1_sizes[0]; - for (i = 0; i <= (int32_T)(br - 1); i++) { - for (b_m = 0; b_m < 6; b_m++) { - a_data_0[(int32_T)(b_m + (int32_T)(6 * i))] = H_data_0[(int32_T) - ((int32_T)(C_sizes_idx_1 * b_m) + i)]; - } - } - - if ((q1_sizes[0] == 1) || (s12_iter == 1)) { - for (i = 0; i < 6; i++) { - rtb_Product_j[i] = 0.0F; - for (b_m = 0; b_m <= (int32_T)(num_original - 1); b_m++) { - rtb_Product_j[i] += a_data_0[(int32_T)((int32_T)(6 * b_m) + i)] * - r_data_0[b_m]; - } - } - } else { - for (nx = 0; nx < 6; nx++) { - rtb_Product_j[nx] = 0.0F; - } - - ar = 0; - for (i = 0; (int32_T)(i + 1) <= C_sizes_idx_1; i++) { - if (r_data_0[i] != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = 0; O_sizes_idx_0 < 6; O_sizes_idx_0++) { - s12_iter++; - rtb_Product_j[O_sizes_idx_0] += a_data_0[(int32_T)(s12_iter - 1)] - * r_data_0[i]; - } - } - - ar += 6; - } - } - - moppbaaafkfkimgd_diag(r_vec_data_0, z_est_sizes_idx_1, - est_estimator_B->tmp_data_m, H_sizes); - num_original = H_sizes[1]; - for (i = 0; i < 6; i++) { - br = H_sizes[1]; - for (b_m = 0; b_m <= (int32_T)(br - 1); b_m++) { - a_data_0[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 <= (int32_T)(C_sizes_idx_1 - 1); i_0++) { - a_data_0[(int32_T)(i + (int32_T)(6 * b_m))] += H_data_0[(int32_T) - ((int32_T)(C_sizes_idx_1 * i) + i_0)] * (real32_T) - est_estimator_B->tmp_data_m[(int32_T)((int32_T)(H_sizes[0] * b_m) - + i_0)]; - } - } - } - - if ((H_sizes[1] == 1) || (q1_sizes[0] == 1)) { - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 6; b_m++) { - rtb_R_mat[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 <= (int32_T)(num_original - 1); i_0++) { - rtb_R_mat[(int32_T)(i + (int32_T)(6 * b_m))] += a_data_0 - [(int32_T)((int32_T)(6 * i_0) + i)] * H_data_0[(int32_T) - ((int32_T)(C_sizes_idx_1 * b_m) + i_0)]; - } - } - } - } else { - memset(&rtb_R_mat[0], 0, (uint32_T)(36U * sizeof(real32_T))); - for (br = 0; br <= 31; br += 6) { - for (nx = br; (int32_T)(nx + 1) <= (int32_T)(br + 6); nx++) { - rtb_R_mat[nx] = 0.0F; - } - } - - br = 0; - for (nx = 0; nx <= 31; nx += 6) { - ar = 0; - b_m = (int32_T)(br + num_original); - for (i = br; (int32_T)(i + 1) <= b_m; i++) { - if (est_estimator_B->q1_data_c[(int32_T)(i % q1_sizes[0] + - (int32_T)(q1_sizes[0] * div_nzp_s32_floor(i, q1_sizes[0])))] - != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = nx; (int32_T)(O_sizes_idx_0 + 1) <= - (int32_T)(nx + 6); O_sizes_idx_0++) { - s12_iter++; - rtb_R_mat[O_sizes_idx_0] += est_estimator_B->q1_data_c - [(int32_T)(i % q1_sizes[0] + (int32_T)(q1_sizes[0] * - div_nzp_s32_floor(i, q1_sizes[0])))] * a_data_0[(int32_T) - (s12_iter - 1)]; - } - } - - ar += 6; - } - - br += num_original; - } - } - - // Convert H to be in terms of the entire state vector - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 6; b_m++) { - rtb_H_out[(int32_T)(b_m + (int32_T)(6 * i))] = T_H_data_0[(int32_T) - ((int32_T)(T_H_sizes[0] * i) + b_m)]; - } - } - - for (i = 0; i < 111; i++) { - for (b_m = 0; b_m < 6; b_m++) { - rtb_H_out[(int32_T)(b_m + (int32_T)(6 * (int32_T)(i + 6)))] = 0.0F; - } - } - } - - // ':1:3' - for (i = 0; i < 6; i++) { - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Residual and H' - - est_estimator_B->r_out[i] = rtb_Product_j[i]; - } - - // SignalConversion: '/Signal Conversion' incorporates: - // MATLAB Function: '/Compute Residual and H' - - est_estimator_B->error_out = handrail_knowledge_dims; - - // SignalConversion: '/Signal Conversion' - memcpy(&est_estimator_B->H_out[0], &rtb_H_out[0], (uint32_T)(702U * sizeof - (real32_T))); - - // SignalConversion: '/Signal Conversion' - memcpy(&est_estimator_B->R_mat[0], &rtb_R_mat[0], (uint32_T)(36U * sizeof - (real32_T))); - - // SignalConversion: '/Signal Conversion' - hr_P_hr_ISS_ISS[0] = UnitDelay_DSTATE_n_hr_P_hr_ISS_; - hr_P_hr_ISS_ISS[1] = UnitDelay_DSTATE_n_hr_P_hr_IS_0; - hr_P_hr_ISS_ISS[2] = UnitDelay_DSTATE_n_hr_P_hr_IS_1; - - // SignalConversion: '/Signal Conversion' - hr_quat_ISS2hr_idx_0 = UnitDelay_DSTATE_n_hr_quat_ISS2; - hr_quat_ISS2hr_idx_1 = UnitDelay_DSTATE_n_hr_quat_IS_0; - hr_quat_ISS2hr_idx_2 = UnitDelay_DSTATE_n_hr_quat_IS_1; - hr_quat_ISS2hr_idx_3 = UnitDelay_DSTATE_n_hr_quat_IS_2; - - // End of Outputs for SubSystem: '/ML_Compute_Residual_and_H' - } - - // End of If: '/IF' - - // Product: '/Product' - for (i = 0; i < 6; i++) { - rtb_Product_j[i] = est_estimator_B->r_out[i] * est_estimator_B->r_out[i]; - } - - // End of Product: '/Product' - - // Sum: '/Sum of Elements' - rtb_Switch1 = rtb_Product_j[0]; - for (ar = 0; ar < 5; ar++) { - rtb_Switch1 += rtb_Product_j[(int32_T)(ar + 1)]; - } - - // End of Sum: '/Sum of Elements' - - // Sqrt: '/Sqrt' - rtb_Switch1 = (real32_T)sqrt((real_T)rtb_Switch1); - - // S-Function (ex_compute_delta_state_and_cov): '/ex_compute_delta_state_and_cov' - rtb_ex_compute_delta_state_a_pg = compute_delta_state_and_cov((real32_T*) - &est_estimator_B->r_out[0], est_estimator_B->error_out, (real32_T*) - &est_estimator_B->H_out[0], 6, 117, est_estimator_P->Constant_Value_h, - (real32_T*)&est_estimator_B->R_mat[0], (real32_T*) - &est_estimator_B->Assignment[0], &rtb_ex_compute_delta_state_an_p[0], - &est_estimator_B->Switch1[0]); - - // S-Function (ex_apply_delta_state): '/ex_apply_delta_state' - apply_delta_state((real32_T*)&rtb_ex_compute_delta_state_an_p[0], 117, - est_estimator_P->Constant_Value_ln, (real32_T*) - &rtb_Merge_o[0], (real32_T*)&accel[0], (real32_T*) - &UnitDelay_DSTATE_n_V_B_ISS_ISS[0], (real32_T*) - &UnitDelay_DSTATE_n_accel_bias[0], (real32_T*) - &UnitDelay_DSTATE_n_P_EST_ISS_IS[0], (real32_T*) - &UnitDelay_DSTATE_n_ml_quat_ISS2[0], (real32_T*) - &UnitDelay_DSTATE_n_ml_P_cam_ISS[0], - UnitDelay_DSTATE_n_kfl_status, (real32_T*) - &UnitDelay_DSTATE_n_of_quat_ISS2[0], (real32_T*) - &UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &rtb_ex_apply_delta_state_o1_d[0], - &rtb_ex_apply_delta_state_o2_i[0], - &rtb_ex_apply_delta_state_o3_e[0], - &rtb_ex_apply_delta_state_o4_g[0], - &rtb_ex_apply_delta_state_o5_i[0], - &rtb_ex_apply_delta_state_o6_d[0], - &rtb_ex_apply_delta_state_o7_p[0], - &rtb_ex_apply_delta_state_o8_j, - &rtb_ex_apply_delta_state_o9_c[0], - &rtb_ex_apply_delta_state_o10_i[0]); - - // Logic: '/Logical Operator1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant' - // Product: '/Divide' - // RelationalOperator: '/Relational Operator1' - // Sum: '/Add' - // UnitDelay: '/Unit Delay2' - - empty_non_axis_sizes = ((est_estimator_P->ase_minumum_resid_thresh > (real_T) - ((est_estimator_P->UnitDelay2_InitialCondition - rtb_Switch1) / - rtb_Switch1)) || (rtb_ex_compute_delta_state_a_pg != 0)); - - // Switch: '/Switch' incorporates: - // BusAssignment: '/Bus Assignment1' - // BusAssignment: '/Bus Assignment1' - // Constant: '/Constant2' - // Constant: '/Constant3' - - if (empty_non_axis_sizes) { - numFeatures = rtb_numFeatures_f; - memcpy(&rtb_UnitDelay18[0], &UnitDelay_DSTATE_n_ml_mahal_dis[0], (uint32_T) - (50U * sizeof(real_T))); - hr_P_hr_ISS_ISS[0] = UnitDelay_DSTATE_n_hr_P_hr_ISS_; - hr_P_hr_ISS_ISS[1] = UnitDelay_DSTATE_n_hr_P_hr_IS_0; - hr_P_hr_ISS_ISS[2] = UnitDelay_DSTATE_n_hr_P_hr_IS_1; - hr_quat_ISS2hr_idx_0 = UnitDelay_DSTATE_n_hr_quat_ISS2; - hr_quat_ISS2hr_idx_1 = UnitDelay_DSTATE_n_hr_quat_IS_0; - hr_quat_ISS2hr_idx_2 = UnitDelay_DSTATE_n_hr_quat_IS_1; - hr_quat_ISS2hr_idx_3 = UnitDelay_DSTATE_n_hr_quat_IS_2; - } else { - rtb_Merge_o[0] = rtb_ex_apply_delta_state_o1_d[0]; - rtb_Merge_o[1] = rtb_ex_apply_delta_state_o1_d[1]; - rtb_Merge_o[2] = rtb_ex_apply_delta_state_o1_d[2]; - rtb_Merge_o[3] = rtb_ex_apply_delta_state_o1_d[3]; - accel[0] = rtb_ex_apply_delta_state_o2_i[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = rtb_ex_apply_delta_state_o3_e[0]; - UnitDelay_DSTATE_n_accel_bias[0] = rtb_ex_apply_delta_state_o4_g[0]; - accel[1] = rtb_ex_apply_delta_state_o2_i[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = rtb_ex_apply_delta_state_o3_e[1]; - UnitDelay_DSTATE_n_accel_bias[1] = rtb_ex_apply_delta_state_o4_g[1]; - accel[2] = rtb_ex_apply_delta_state_o2_i[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = rtb_ex_apply_delta_state_o3_e[2]; - UnitDelay_DSTATE_n_accel_bias[2] = rtb_ex_apply_delta_state_o4_g[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = rtb_ex_apply_delta_state_o6_d[0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = rtb_ex_apply_delta_state_o6_d[1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = rtb_ex_apply_delta_state_o6_d[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = rtb_ex_apply_delta_state_o6_d[3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = rtb_ex_apply_delta_state_o7_p[0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = rtb_ex_apply_delta_state_o7_p[1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = rtb_ex_apply_delta_state_o7_p[2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], - &rtb_ex_apply_delta_state_o9_c[0], (uint32_T)(sizeof(real32_T) << - 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &rtb_ex_apply_delta_state_o10_i[0], (uint32_T)(48U * sizeof - (real32_T))); - UnitDelay_DSTATE_n_kfl_status = rtb_ex_apply_delta_state_o8_j; - rtb_Switch_m = est_estimator_P->Constant2_Value_n4; - memcpy(&UnitDelay_DSTATE_n_of_mahal_dis[0], - &est_estimator_P->Constant3_Value[0], (uint32_T)(50U * sizeof - (real_T))); - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = rtb_ex_apply_delta_state_o5_i[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = rtb_ex_apply_delta_state_o5_i[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = rtb_ex_apply_delta_state_o5_i[2]; - } - - // End of Switch: '/Switch' - - // Switch: '/Switch1' - for (i = 0; i < 13689; i++) { - rtb_Switch1 = est_estimator_B->Switch1[i]; - if (empty_non_axis_sizes) { - rtb_Switch1 = est_estimator_B->Assignment[i]; - } - - est_estimator_B->Switch1[i] = rtb_Switch1; - } - - // End of Switch: '/Switch1' - - // SignalConversion: '/OutportBufferForstate_out' incorporates: - // SignalConversion: '/Signal Conversion' - - rtb_Merge2.quat_ISS2B[0] = rtb_Merge_o[0]; - rtb_Merge2.quat_ISS2B[1] = rtb_Merge_o[1]; - rtb_Merge2.quat_ISS2B[2] = rtb_Merge_o[2]; - rtb_Merge2.quat_ISS2B[3] = rtb_Merge_o[3]; - rtb_Merge2.omega_B_ISS_B[0] = rtb_ImpAsg_InsertedFor_Out1_at_[0]; - rtb_Merge2.gyro_bias[0] = accel[0]; - rtb_Merge2.V_B_ISS_ISS[0] = UnitDelay_DSTATE_n_V_B_ISS_ISS[0]; - rtb_Merge2.A_B_ISS_ISS[0] = rtb_Sum_k1; - rtb_Merge2.accel_bias[0] = UnitDelay_DSTATE_n_accel_bias[0]; - rtb_Merge2.P_B_ISS_ISS[0] = rtb_Sum_l; - rtb_Merge2.omega_B_ISS_B[1] = rtb_ImpAsg_InsertedFor_Out1_at_[1]; - rtb_Merge2.gyro_bias[1] = accel[1]; - rtb_Merge2.V_B_ISS_ISS[1] = UnitDelay_DSTATE_n_V_B_ISS_ISS[1]; - rtb_Merge2.A_B_ISS_ISS[1] = rtb_Gain1_f; - rtb_Merge2.accel_bias[1] = UnitDelay_DSTATE_n_accel_bias[1]; - rtb_Merge2.P_B_ISS_ISS[1] = UnitDelay_DSTATE_n_P_B_ISS_ISS_; - rtb_Merge2.omega_B_ISS_B[2] = rtb_ImpAsg_InsertedFor_Out1_at_[2]; - rtb_Merge2.gyro_bias[2] = accel[2]; - rtb_Merge2.V_B_ISS_ISS[2] = UnitDelay_DSTATE_n_V_B_ISS_ISS[2]; - rtb_Merge2.A_B_ISS_ISS[2] = UnitDelay_DSTATE_n_A_B_ISS_ISS_; - rtb_Merge2.accel_bias[2] = UnitDelay_DSTATE_n_accel_bias[2]; - rtb_Merge2.P_B_ISS_ISS[2] = UnitDelay_DSTATE_n_P_B_ISS_IS_0; - rtb_Merge2.confidence = rtb_Saturation_n; - rtb_Merge2.aug_state_enum = rtb_BitwiseOperator; - rtb_Merge2.ml_quat_ISS2cam[0] = UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - rtb_Merge2.ml_quat_ISS2cam[1] = UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - rtb_Merge2.ml_quat_ISS2cam[2] = UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - rtb_Merge2.ml_quat_ISS2cam[3] = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - rtb_Merge2.ml_P_cam_ISS_ISS[0] = UnitDelay_DSTATE_n_ml_P_cam_ISS[0]; - rtb_Merge2.ml_P_cam_ISS_ISS[1] = UnitDelay_DSTATE_n_ml_P_cam_ISS[1]; - rtb_Merge2.ml_P_cam_ISS_ISS[2] = UnitDelay_DSTATE_n_ml_P_cam_ISS[2]; - memcpy(&rtb_Merge2.of_quat_ISS2cam[0], &UnitDelay_DSTATE_n_of_quat_ISS2[0], - (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&rtb_Merge2.of_P_cam_ISS_ISS[0], &UnitDelay_DSTATE_n_of_P_cam_ISS[0], - (uint32_T)(48U * sizeof(real32_T))); - memcpy(&rtb_Merge2.cov_diag[0], &UnitDelay_DSTATE_n_cov_diag[0], (uint32_T) - (117U * sizeof(ase_cov_datatype))); - rtb_Merge2.kfl_status = UnitDelay_DSTATE_n_kfl_status; - rtb_Merge2.update_OF_tracks_cnt = rtb_Switch_m; - rtb_Merge2.update_ML_features_cnt = numFeatures; - memcpy(&rtb_Merge2.of_mahal_distance[0], &UnitDelay_DSTATE_n_of_mahal_dis[0], - (uint32_T)(50U * sizeof(real_T))); - memcpy(&rtb_Merge2.ml_mahal_distance[0], &rtb_UnitDelay18[0], (uint32_T)(50U - * sizeof(real_T))); - rtb_Merge2.hr_P_hr_ISS_ISS[0] = hr_P_hr_ISS_ISS[0]; - rtb_Merge2.hr_P_hr_ISS_ISS[1] = hr_P_hr_ISS_ISS[1]; - rtb_Merge2.hr_P_hr_ISS_ISS[2] = hr_P_hr_ISS_ISS[2]; - rtb_Merge2.hr_quat_ISS2hr[0] = hr_quat_ISS2hr_idx_0; - rtb_Merge2.hr_quat_ISS2hr[1] = hr_quat_ISS2hr_idx_1; - rtb_Merge2.hr_quat_ISS2hr[2] = hr_quat_ISS2hr_idx_2; - rtb_Merge2.hr_quat_ISS2hr[3] = hr_quat_ISS2hr_idx_3; - rtb_Merge2.P_EST_ISS_ISS[0] = UnitDelay_DSTATE_n_P_EST_ISS_IS[0]; - rtb_Merge2.P_EST_ISS_ISS[1] = UnitDelay_DSTATE_n_P_EST_ISS_IS[1]; - rtb_Merge2.P_EST_ISS_ISS[2] = UnitDelay_DSTATE_n_P_EST_ISS_IS[2]; - - // End of Outputs for SubSystem: '/ML Update' - // End of Outputs for SubSystem: '/Absolute_Update' - break; - - case 1: - // Outputs for IfAction SubSystem: '/Optical_Flow_Update' incorporates: - // ActionPort: '/Action Port' - - // Outputs for Iterator SubSystem: '/OF Update' incorporates: - // WhileIterator: '/While Iterator' - - // InitializeConditions for UnitDelay: '/Unit Delay' incorporates: - // UnitDelay: '/Unit Delay' - - rtb_Merge_o[0] = est_estimator_P->UnitDelay_InitialCondition_p.quat_ISS2B[0]; - rtb_Merge_o[1] = est_estimator_P->UnitDelay_InitialCondition_p.quat_ISS2B[1]; - rtb_Merge_o[2] = est_estimator_P->UnitDelay_InitialCondition_p.quat_ISS2B[2]; - rtb_Merge_o[3] = est_estimator_P->UnitDelay_InitialCondition_p.quat_ISS2B[3]; - rtb_ImpAsg_InsertedFor_Out1_at_[0] = - est_estimator_P->UnitDelay_InitialCondition_p.omega_B_ISS_B[0]; - accel[0] = est_estimator_P->UnitDelay_InitialCondition_p.gyro_bias[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = - est_estimator_P->UnitDelay_InitialCondition_p.V_B_ISS_ISS[0]; - rtb_Sum_k1 = est_estimator_P->UnitDelay_InitialCondition_p.A_B_ISS_ISS[0]; - UnitDelay_DSTATE_n_accel_bias[0] = - est_estimator_P->UnitDelay_InitialCondition_p.accel_bias[0]; - rtb_Sum_l = est_estimator_P->UnitDelay_InitialCondition_p.P_B_ISS_ISS[0]; - rtb_ImpAsg_InsertedFor_Out1_at_[1] = - est_estimator_P->UnitDelay_InitialCondition_p.omega_B_ISS_B[1]; - accel[1] = est_estimator_P->UnitDelay_InitialCondition_p.gyro_bias[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = - est_estimator_P->UnitDelay_InitialCondition_p.V_B_ISS_ISS[1]; - rtb_Gain1_f = est_estimator_P->UnitDelay_InitialCondition_p.A_B_ISS_ISS[1]; - UnitDelay_DSTATE_n_accel_bias[1] = - est_estimator_P->UnitDelay_InitialCondition_p.accel_bias[1]; - UnitDelay_DSTATE_n_P_B_ISS_ISS_ = - est_estimator_P->UnitDelay_InitialCondition_p.P_B_ISS_ISS[1]; - rtb_ImpAsg_InsertedFor_Out1_at_[2] = - est_estimator_P->UnitDelay_InitialCondition_p.omega_B_ISS_B[2]; - accel[2] = est_estimator_P->UnitDelay_InitialCondition_p.gyro_bias[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = - est_estimator_P->UnitDelay_InitialCondition_p.V_B_ISS_ISS[2]; - UnitDelay_DSTATE_n_A_B_ISS_ISS_ = - est_estimator_P->UnitDelay_InitialCondition_p.A_B_ISS_ISS[2]; - UnitDelay_DSTATE_n_accel_bias[2] = - est_estimator_P->UnitDelay_InitialCondition_p.accel_bias[2]; - UnitDelay_DSTATE_n_P_B_ISS_IS_0 = - est_estimator_P->UnitDelay_InitialCondition_p.P_B_ISS_ISS[2]; - rtb_Switch_m = est_estimator_P->UnitDelay_InitialCondition_p.confidence; - UnitDelay_DSTATE_aug_state_enum = - est_estimator_P->UnitDelay_InitialCondition_p.aug_state_enum; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_quat_ISS2cam[0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_quat_ISS2cam[1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_quat_ISS2cam[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_quat_ISS2cam[3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_P_cam_ISS_ISS[0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_P_cam_ISS_ISS[1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = - est_estimator_P->UnitDelay_InitialCondition_p.ml_P_cam_ISS_ISS[2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], - &est_estimator_P->UnitDelay_InitialCondition_p.of_quat_ISS2cam[0], - (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &est_estimator_P->UnitDelay_InitialCondition_p.of_P_cam_ISS_ISS[0], - (uint32_T)(48U * sizeof(real32_T))); - memcpy(&UnitDelay_DSTATE_n_cov_diag[0], - &est_estimator_P->UnitDelay_InitialCondition_p.cov_diag[0], (uint32_T) - (117U * sizeof(ase_cov_datatype))); - UnitDelay_DSTATE_n_kfl_status = - est_estimator_P->UnitDelay_InitialCondition_p.kfl_status; - rtb_numFeatures_f = - est_estimator_P->UnitDelay_InitialCondition_p.update_OF_tracks_cnt; - rtb_Saturation_n = - est_estimator_P->UnitDelay_InitialCondition_p.update_ML_features_cnt; - memcpy(&UnitDelay_DSTATE_n_of_mahal_dis[0], - &est_estimator_P->UnitDelay_InitialCondition_p.of_mahal_distance[0], - (uint32_T)(50U * sizeof(real_T))); - memcpy(&UnitDelay_DSTATE_n_ml_mahal_dis[0], - &est_estimator_P->UnitDelay_InitialCondition_p.ml_mahal_distance[0], - (uint32_T)(50U * sizeof(real_T))); - UnitDelay_DSTATE_n_hr_P_hr_ISS_ = - est_estimator_P->UnitDelay_InitialCondition_p.hr_P_hr_ISS_ISS[0]; - UnitDelay_DSTATE_n_hr_P_hr_IS_0 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_P_hr_ISS_ISS[1]; - UnitDelay_DSTATE_n_hr_P_hr_IS_1 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_P_hr_ISS_ISS[2]; - UnitDelay_DSTATE_n_hr_quat_ISS2 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_quat_ISS2hr[0]; - UnitDelay_DSTATE_n_hr_quat_IS_0 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_quat_ISS2hr[1]; - UnitDelay_DSTATE_n_hr_quat_IS_1 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_quat_ISS2hr[2]; - UnitDelay_DSTATE_n_hr_quat_IS_2 = - est_estimator_P->UnitDelay_InitialCondition_p.hr_quat_ISS2hr[3]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = - est_estimator_P->UnitDelay_InitialCondition_p.P_EST_ISS_ISS[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = - est_estimator_P->UnitDelay_InitialCondition_p.P_EST_ISS_ISS[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = - est_estimator_P->UnitDelay_InitialCondition_p.P_EST_ISS_ISS[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay1' incorporates: - // UnitDelay: '/Unit Delay1' - - for (i = 0; i < 13689; i++) { - est_estimator_B->Switch1[i] = - est_estimator_P->UnitDelay1_InitialCondition_m; - } - - // End of InitializeConditions for UnitDelay: '/Unit Delay1' - - // Switch: '/Switch2' incorporates: - // BusAssignment: '/Bus Assignment' - // BusAssignment: '/Bus Assignment' - // BusCreator: '/Bus Creator2' - // UnitDelay: '/Unit Delay10' - // UnitDelay: '/Unit Delay11' - // UnitDelay: '/Unit Delay12' - // UnitDelay: '/Unit Delay13' - // UnitDelay: '/Unit Delay14' - // UnitDelay: '/Unit Delay15' - // UnitDelay: '/Unit Delay3' - // UnitDelay: '/Unit Delay6' - // UnitDelay: '/Unit Delay7' - // UnitDelay: '/Unit Delay8' - - if (!(1 > est_estimator_P->Switch2_Threshold_b)) { - rtb_Merge_o[0] = rtb_Product1[0]; - rtb_Merge_o[1] = rtb_Product1[1]; - rtb_Merge_o[2] = rtb_Product1[2]; - rtb_Merge_o[3] = rtb_Product1[3]; - rtb_ImpAsg_InsertedFor_Out1_at_[0] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - accel[0] = est_estimator_DW->UnitDelay3_DSTATE[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = rtb_Sum2[0]; - rtb_Sum_k1 = rtb_Sum1_k3[0]; - UnitDelay_DSTATE_n_accel_bias[0] = est_estimator_DW->UnitDelay6_DSTATE[0]; - rtb_Sum_l = est_estimator_DW->UnitDelay7_DSTATE[0]; - rtb_ImpAsg_InsertedFor_Out1_at_[1] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - accel[1] = est_estimator_DW->UnitDelay3_DSTATE[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = rtb_Sum2[1]; - rtb_Gain1_f = rtb_Sum1_k3[1]; - UnitDelay_DSTATE_n_accel_bias[1] = est_estimator_DW->UnitDelay6_DSTATE[1]; - UnitDelay_DSTATE_n_P_B_ISS_ISS_ = est_estimator_DW->UnitDelay7_DSTATE[1]; - rtb_ImpAsg_InsertedFor_Out1_at_[2] = hr_quat_ISS2hr_idx_0; - accel[2] = est_estimator_DW->UnitDelay3_DSTATE[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = rtb_Sum2[2]; - UnitDelay_DSTATE_n_A_B_ISS_ISS_ = rtb_Sum1_k3[2]; - UnitDelay_DSTATE_n_accel_bias[2] = est_estimator_DW->UnitDelay6_DSTATE[2]; - UnitDelay_DSTATE_n_P_B_ISS_IS_0 = est_estimator_DW->UnitDelay7_DSTATE[2]; - rtb_Switch_m = est_estimator_DW->UnitDelay8_DSTATE; - UnitDelay_DSTATE_aug_state_enum = rtb_BusAssignment_aug_state_enu; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = est_estimator_DW->UnitDelay10_DSTATE - [0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = est_estimator_DW->UnitDelay10_DSTATE - [1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = est_estimator_DW->UnitDelay10_DSTATE - [2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = est_estimator_DW->UnitDelay10_DSTATE - [3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = est_estimator_DW->UnitDelay11_DSTATE - [0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = est_estimator_DW->UnitDelay11_DSTATE - [1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = est_estimator_DW->UnitDelay11_DSTATE - [2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], - &est_estimator_DW->UnitDelay12_DSTATE[0], (uint32_T)(sizeof - (real32_T) << 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &est_estimator_DW->UnitDelay13_DSTATE[0], (uint32_T)(48U * sizeof - (real32_T))); - memcpy(&UnitDelay_DSTATE_n_cov_diag[0], - &est_estimator_DW->UnitDelay14_DSTATE[0], (uint32_T)(117U * sizeof - (ase_cov_datatype))); - UnitDelay_DSTATE_n_kfl_status = est_estimator_DW->UnitDelay15_DSTATE; - rtb_numFeatures_f = numFeatures; - rtb_Saturation_n = rtb_BusCreator2_update_ML_featu; - memcpy(&UnitDelay_DSTATE_n_of_mahal_dis[0], &rtb_UnitDelay18[0], (uint32_T) - (50U * sizeof(real_T))); - memcpy(&UnitDelay_DSTATE_n_ml_mahal_dis[0], &rtb_UnitDelay19[0], (uint32_T) - (50U * sizeof(real_T))); - UnitDelay_DSTATE_n_hr_P_hr_ISS_ = rtb_Add_e[0]; - UnitDelay_DSTATE_n_hr_P_hr_IS_0 = rtb_Add_e[1]; - UnitDelay_DSTATE_n_hr_P_hr_IS_1 = rtb_Add_e[2]; - UnitDelay_DSTATE_n_hr_quat_ISS2 = rtb_UnitDelay25[0]; - UnitDelay_DSTATE_n_hr_quat_IS_0 = rtb_UnitDelay25[1]; - UnitDelay_DSTATE_n_hr_quat_IS_1 = rtb_UnitDelay25[2]; - UnitDelay_DSTATE_n_hr_quat_IS_2 = rtb_UnitDelay25[3]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = rtb_P_B_ISS_ISS[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = rtb_P_B_ISS_ISS[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = rtb_P_B_ISS_ISS[2]; - } - - // End of Switch: '/Switch2' - - // MATLAB Function: '/compute_of_global_points' incorporates: - // Inport: '/optical_flow_msg' - - // MATLAB Function 'camera_update/Optical_Flow_Update/OF Update/compute_of_global_points': ':1' - // Code is taken from Zack Morrato's prototyped Optical Flow Kalman Filter. - // This code computes the residual and measurment matrices for the optical - // flow measurments. - // - // ':1:8' - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // Code is taken from Zack Morrato's prototyped Optical Flow Kalman Filter. - // This code computes the residual and measurment matrices for the optical - // flow measurments. - // for some reason matlab doesn't like creating matrix sizes based on "unbounded" parameters - // if ase_of_num_aug ~= 5 - // error('eml_compute_of_global_points is optimized for four augmented states.'); - // end - // ase_of_num_aug = 4; - memset(&est_estimator_B->of_measured[0], 0, (uint32_T)(1600U * sizeof - (real32_T))); - for (i = 0; i < 200; i++) { - rtb_global_points[i] = 1.0F; - } - - memset(&rtb_valid_out[0], 0, (uint32_T)(800U * sizeof(int32_T))); - for (i = 0; i < 1600; i++) { - est_estimator_B->of_measured_in[i] = - est_estimator_U_cvs_optical_flow_msg_n->cvs_observations[i] * - est_estimator_P->tun_ase_vocam_inv_focal_length; - } - - // Convert measurements to normalized units - // Note for some reason of_measured and valid can not match? Why? - // Select out only features that are seen by three or more cameras - fkngdjekgdjepphl_sum(est_estimator_U_cvs_optical_flow_msg_n->cvs_valid_flag, - rtb_UnitDelay18); - br = 0; - for (nx = 0; nx < 50; nx++) { - rtb_Compare = (rtb_UnitDelay18[nx] >= 3.0); - if (rtb_Compare) { - br++; - } - - rtb_Compare_ic[nx] = rtb_Compare; - } - - handrail_knowledge_dims = br; - num_original = 0; - for (br = 0; br < 50; br++) { - if (rtb_Compare_ic[br]) { - num_original++; - } - } - - C_sizes_idx_1 = num_original; - num_original = 0; - for (br = 0; br < 50; br++) { - if (rtb_Compare_ic[br]) { - e_data[num_original] = (int8_T)(int32_T)(br + 1); - num_original++; - } - } - - for (i = 0; i < 16; i++) { - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - rtb_valid_out[(int32_T)(b_m + (int32_T)(50 * i))] = (int32_T) - est_estimator_U_cvs_optical_flow_msg_n->cvs_valid_flag[(int32_T) - ((int32_T)((int32_T)(50 * i) + (int32_T)e_data[b_m]) - 1)]; - } - } - - // reshape the measurements so the order is by feature, x measure then y - // measure, with every camera observation one after the other. - num_original = (int32_T)(handrail_knowledge_dims << 5); - nx = 0; - for (br = 0; br < 50; br++) { - if (rtb_Compare_ic[br]) { - nx++; - } - } - - C_sizes_idx_1 = nx; - nx = 0; - for (br = 0; br < 50; br++) { - if (rtb_Compare_ic[br]) { - e_data[nx] = (int8_T)(int32_T)(br + 1); - nx++; - } - } - - of_measured_in_sizes[0] = C_sizes_idx_1; - of_measured_in_sizes[1] = 2; - of_measured_in_sizes[2] = 16; - for (i = 0; i < 16; i++) { - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - est_estimator_B->of_measured_in_data[(int32_T)(b_m + (int32_T)((int32_T) - (C_sizes_idx_1 << 1) * i))] = est_estimator_B->of_measured_in[(int32_T) - ((int32_T)((int32_T)(100 * i) + (int32_T)e_data[b_m]) - 1)]; - } - - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - est_estimator_B->of_measured_in_data[(int32_T)((int32_T)(b_m + - C_sizes_idx_1) + (int32_T)((int32_T)(C_sizes_idx_1 << 1) * i))] = - est_estimator_B->of_measured_in[(int32_T)((int32_T)((int32_T)(100 * i) - + (int32_T)e_data[b_m]) + 49)]; - } - } - - aaiepphlecjelnoh_permute(est_estimator_B->of_measured_in_data, - of_measured_in_sizes, est_estimator_B->of_measured_in, x_sizes); - nx = (int32_T)(x_sizes[1] << 5); - tmp_h = (int16_T)(int32_T)(handrail_knowledge_dims << 4); - ar = (int32_T)(int16_T)(int32_T)(handrail_knowledge_dims << 4); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - est_estimator_B->of_measured_in_data[br] = est_estimator_B-> - of_measured_in[br]; - } - - for (i = 0; i <= (int32_T)(ar - 1); i++) { - est_estimator_B->b_x_data[(int32_T)(i << 1)] = - est_estimator_B->of_measured_in_data[i]; - est_estimator_B->b_x_data[(int32_T)(1 + (int32_T)(i << 1))] = - est_estimator_B->of_measured_in_data[(int32_T)(i + (int32_T)tmp_h)]; - } - - nx = (int32_T)((int32_T)tmp_h << 1); - for (br = 0; (int32_T)(br + 1) <= nx; br++) { - est_estimator_B->of_measured_in_data[br] = est_estimator_B->b_x_data[br]; - } - - if (1 > num_original) { - num_original = 0; - } - - tmp_h = (int16_T)(int32_T)((int32_T)(int16_T)num_original - 1); - br = (int32_T)((int32_T)tmp_h + 1); - C_sizes_idx_1 = (int32_T)tmp_h; - for (i = 0; i <= C_sizes_idx_1; i++) { - h_data[i] = (int16_T)i; - } - - for (i = 0; i <= (int32_T)(br - 1); i++) { - est_estimator_B->of_measured[(int32_T)h_data[i]] = - est_estimator_B->of_measured_in_data[i]; - } - - // Extract the 4x4 transforms which convert points from global frame to camera - // frame. I'll be stacking them vertically so they can all be evaluated - // simultaneously. - imohknglphlfpphd_repmat(rtb_camera_tf_global); - for (num_original = 0; num_original < 16; num_original++) { - br = (int32_T)((int32_T)((int32_T)(1 + num_original) << 2) - 3); - nx = (int32_T)((int32_T)((int32_T)(1 + num_original) << 2) - 1); - if (nx < br) { - z_est_sizes_idx_1 = 0; - } else { - z_est_sizes_idx_1 = (int32_T)((int32_T)(nx - br) + 1); - C_sizes_idx_1 = (int32_T)(nx - br); - for (i = 0; i <= C_sizes_idx_1; i++) { - rot_indices_data[i] = (int8_T)(int32_T)(br + i); - } - } - - for (i = 0; i <= (int32_T)(z_est_sizes_idx_1 - 1); i++) { - c_data_1[i] = (int8_T)(int32_T)((int32_T)rot_indices_data[i] - 1); - } - - H[0] = UnitDelay_DSTATE_n_of_quat_ISS2[num_original]; - H[1] = UnitDelay_DSTATE_n_of_quat_ISS2[(int32_T)(num_original + 16)]; - H[2] = UnitDelay_DSTATE_n_of_quat_ISS2[(int32_T)(num_original + 32)]; - H[3] = UnitDelay_DSTATE_n_of_quat_ISS2[(int32_T)(num_original + 48)]; - fkfcbaiengdjgdje_quaternion_to_rotation(H, rtb_Product1_0); - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(z_est_sizes_idx_1 - 1); b_m++) { - rtb_camera_tf_global[(int32_T)((int32_T)c_data_1[b_m] + (int32_T)(i << - 6))] = rtb_Product1_0[(int32_T)((int32_T)(z_est_sizes_idx_1 * i) + - b_m)]; - } - } - - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(z_est_sizes_idx_1 - 1); b_m++) { - c_a_data[(int32_T)(b_m + (int32_T)(z_est_sizes_idx_1 * i))] = - -rtb_camera_tf_global[(int32_T)((int32_T)((int32_T)(i << 6) + - (int32_T)rot_indices_data[b_m]) - 1)]; - } - } - - br = (int32_T)(int8_T)z_est_sizes_idx_1; - b_m = (int32_T)(int8_T)z_est_sizes_idx_1; - for (i = 0; i <= (int32_T)(br - 1); i++) { - C_data[i] = 0.0F; - } - - if (z_est_sizes_idx_1 != 0) { - br = 0; - while ((z_est_sizes_idx_1 > 0) && (br <= 0)) { - for (nx = 1; nx <= z_est_sizes_idx_1; nx++) { - C_data[(int32_T)(nx - 1)] = 0.0F; - } - - br = z_est_sizes_idx_1; - } - - br = 3; - nx = 0; - while ((z_est_sizes_idx_1 > 0) && (nx <= 0)) { - ar = 0; - for (i = (int32_T)(br - 3); (int32_T)(i + 1) <= br; i++) { - if (UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)((int32_T)(i << 4) + - num_original)] != 0.0F) { - s12_iter = ar; - for (O_sizes_idx_0 = 0; (int32_T)(O_sizes_idx_0 + 1) <= - z_est_sizes_idx_1; O_sizes_idx_0++) { - s12_iter++; - C_data[O_sizes_idx_0] += UnitDelay_DSTATE_n_of_P_cam_ISS - [(int32_T)((int32_T)(i << 4) + num_original)] * c_a_data - [(int32_T)(s12_iter - 1)]; - } - } - - ar += z_est_sizes_idx_1; - } - - br += 3; - nx = z_est_sizes_idx_1; - } - } - - for (i = 0; i <= (int32_T)(b_m - 1); i++) { - rtb_camera_tf_global[(int32_T)((int32_T)rot_indices_data[i] + 191)] = - C_data[i]; - } - } - - // Generate the transforms from camera 1 into all the other cameras. This is - // used for point triangulation. - hdbihlngknopkfcj_repmat(camera_tf_camera1); - for (i = 0; i < 4; i++) { - rtb_VectorConcatenate[(int32_T)(i << 2)] = rtb_camera_tf_global[(int32_T) - (i << 6)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(i << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)(i << 6) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(i << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)(i << 6) + 2)]; - rtb_VectorConcatenate[(int32_T)(3 + (int32_T)(i << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)(i << 6) + 3)]; - } - - biecdbieglngohlf_pinv(rtb_VectorConcatenate, global_tf_camera1); - - // Used to be inv - for (num_original = 0; num_original < 16; num_original++) { - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 2) - 4); - for (b_m = 0; b_m < 4; b_m++) { - rtb_VectorConcatenate[(int32_T)(b_m << 2)] = rtb_camera_tf_global - [(int32_T)((int32_T)(b_m << 6) + i)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(b_m << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)((int32_T)(b_m << 6) + i) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(b_m << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)((int32_T)(b_m << 6) + i) + 2)]; - rtb_VectorConcatenate[(int32_T)(3 + (int32_T)(b_m << 2))] = - rtb_camera_tf_global[(int32_T)((int32_T)((int32_T)(b_m << 6) + i) + 3)]; - } - - i = (int32_T)((int32_T)((int32_T)(1 + num_original) << 2) - 4); - for (b_m = 0; b_m < 4; b_m++) { - for (i_0 = 0; i_0 < 4; i_0++) { - camera_tf_camera1[(int32_T)((int32_T)(b_m + i) + (int32_T)(i_0 << 6))] - = (real_T)(((global_tf_camera1[(int32_T)((int32_T)(i_0 << 2) + 1)] * - rtb_VectorConcatenate[(int32_T)(b_m + 4)] + - global_tf_camera1[(int32_T)(i_0 << 2)] * - rtb_VectorConcatenate[b_m]) + global_tf_camera1 - [(int32_T)((int32_T)(i_0 << 2) + 2)] * - rtb_VectorConcatenate[(int32_T)(b_m + 8)]) + - global_tf_camera1[(int32_T)((int32_T)(i_0 << 2) + 3)] * - rtb_VectorConcatenate[(int32_T)(b_m + 12)]); - } - } - } - - // Triangulate all of the points - // for use interanlly, allocate outside for speed - memset(&A[0], 0, (uint32_T)(96U * sizeof(real32_T))); - memset(&b_1[0], 0, (uint32_T)(sizeof(real32_T) << 5U)); - for (num_original = 0; (int32_T)(num_original + 1) <= - handrail_knowledge_dims; num_original++) { - // Solving for location of the point in the 3D world - // Forming A and b matrices, used in a Ax=b to solve for alpha, beta, rho, - // an inverse depth parameterization of the points location in the 1st - // camera. - for (i = 0; i < 16; i++) { - rtb_valid_out_0[i] = rtb_valid_out[(int32_T)((int32_T)(50 * i) + - num_original)]; - } - - num_augs = baaanophjmgddbie_sum(rtb_valid_out_0); - aug_ind = 1.0; - for (nx = 0; nx <= (int32_T)((int32_T)num_augs - 1); nx++) { - while (!(rtb_valid_out[(int32_T)((int32_T)((int32_T)((int32_T)aug_ind - - 1) * 50) + num_original)] != 0)) { - aug_ind++; - } - - ind0 = 4.0 * aug_ind - 4.0; - tmp_9 = rt_roundd_snf(((real_T)(int32_T)(num_original + 1) - 1.0) * 2.0 * - 16.0 + 2.0 * aug_ind); - if (tmp_9 < 2.147483648E+9) { - if (tmp_9 >= -2.147483648E+9) { - i = (int32_T)tmp_9; - } else { - i = MIN_int32_T; - } - } else { - i = MAX_int32_T; - } - - br = (int32_T)(i - 2); - tmp_9 = (1.0 + (real_T)nx) * 2.0; - A[(int32_T)((int32_T)(tmp_9 + -1.0) - 1)] = (real32_T)camera_tf_camera1 - [(int32_T)((int32_T)(ind0 + 1.0) - 1)] - (real32_T)camera_tf_camera1 - [(int32_T)((int32_T)(ind0 + 3.0) - 1)] * est_estimator_B-> - of_measured[br]; - A[(int32_T)((int32_T)(tmp_9 + -1.0) + 31)] = (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 1.0) + 63)] - (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 3.0) + 63)] * - est_estimator_B->of_measured[br]; - A[(int32_T)((int32_T)(tmp_9 + -1.0) + 63)] = (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 1.0) + 191)] - (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 3.0) + 191)] * - est_estimator_B->of_measured[br]; - A[(int32_T)((int32_T)tmp_9 - 1)] = (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 2.0) - 1)] - (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 3.0) - 1)] * est_estimator_B->of_measured[(int32_T) - (1 + br)]; - A[(int32_T)((int32_T)tmp_9 + 31)] = (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 2.0) + 63)] - (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 3.0) + 63)] * est_estimator_B->of_measured[(int32_T) - (1 + br)]; - A[(int32_T)((int32_T)tmp_9 + 63)] = (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 2.0) + 191)] - (real32_T)camera_tf_camera1[(int32_T) - ((int32_T)(ind0 + 3.0) + 191)] * est_estimator_B->of_measured[(int32_T) - (1 + br)]; - tmp_9 = (1.0 + (real_T)nx) * 2.0; - b_1[(int32_T)((int32_T)(tmp_9 + -1.0) - 1)] = (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 3.0) + 127)] * - est_estimator_B->of_measured[br] - (real32_T)camera_tf_camera1 - [(int32_T)((int32_T)(ind0 + 1.0) + 127)]; - b_1[(int32_T)((int32_T)tmp_9 - 1)] = (real32_T)camera_tf_camera1 - [(int32_T)((int32_T)(ind0 + 3.0) + 127)] * - est_estimator_B->of_measured[(int32_T)(1 + br)] - (real32_T) - camera_tf_camera1[(int32_T)((int32_T)(ind0 + 2.0) + 127)]; - aug_ind++; - } - - // let's weight the oldest augmentation more heavily - tmp_9 = 2.0 * num_augs; - aug_ind = 2.0 * num_augs; - for (i = 0; i < 3; i++) { - rtb_Product_j[(int32_T)(i << 1)] = A[(int32_T)((int32_T)((int32_T)(tmp_9 - + -1.0) + (int32_T)(i << 5)) - 1)] * 5.0F; - rtb_Product_j[(int32_T)(1 + (int32_T)(i << 1))] = A[(int32_T)((int32_T) - ((int32_T)(i << 5) + (int32_T)tmp_9) - 1)] * 5.0F; - } - - for (i = 0; i < 3; i++) { - A[(int32_T)((int32_T)((int32_T)(aug_ind + -1.0) + (int32_T)(i << 5)) - 1)] - = rtb_Product_j[(int32_T)(i << 1)]; - A[(int32_T)((int32_T)((int32_T)aug_ind + (int32_T)(i << 5)) - 1)] = - rtb_Product_j[(int32_T)((int32_T)(i << 1) + 1)]; - } - - tmp_9 = 2.0 * num_augs; - aug_ind = 2.0 * num_augs; - hr_quat_ISS2hr_idx_0 = b_1[(int32_T)((int32_T)tmp_9 - 1)] * 5.0F; - b_1[(int32_T)((int32_T)(aug_ind + -1.0) - 1)] = b_1[(int32_T)((int32_T) - (tmp_9 + -1.0) - 1)] * 5.0F; - b_1[(int32_T)((int32_T)aug_ind - 1)] = hr_quat_ISS2hr_idx_0; - aug_ind = num_augs * 2.0; - num_augs *= 2.0; - if (1.0 > num_augs) { - br = 0; - } else { - br = (int32_T)num_augs; - } - - if (1.0 > aug_ind) { - C_sizes_idx_1 = 0; - } else { - C_sizes_idx_1 = (int32_T)aug_ind; - } - - q1_sizes[0] = C_sizes_idx_1; - q1_sizes[1] = 3; - for (i = 0; i < 3; i++) { - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - A_data[(int32_T)(b_m + (int32_T)(C_sizes_idx_1 * i))] = A[(int32_T) - ((int32_T)(i << 5) + b_m)]; - } - } - - glfcngdjgdjmmglf_pinv(A_data, q1_sizes, a_data, T_H_sizes); - if ((T_H_sizes[1] == 1) || (br == 1)) { - for (i = 0; i <= (int32_T)(br - 1); i++) { - b_data_0[i] = b_1[i]; - } - - for (i = 0; i < 3; i++) { - hr_P_hr_ISS_ISS[i] = 0.0F; - C_sizes_idx_1 = T_H_sizes[1]; - for (b_m = 0; b_m <= (int32_T)(C_sizes_idx_1 - 1); b_m++) { - hr_P_hr_ISS_ISS[i] += a_data[(int32_T)((int32_T)(T_H_sizes[0] * b_m) - + i)] * b_data_0[b_m]; - } - } - } else { - hr_P_hr_ISS_ISS[0] = 0.0F; - hr_P_hr_ISS_ISS[1] = 0.0F; - hr_P_hr_ISS_ISS[2] = 0.0F; - ar = 0; - for (i = 0; (int32_T)(i + 1) <= T_H_sizes[1]; i++) { - if (b_1[i] != 0.0F) { - s12_iter = (int32_T)(ar + 1); - hr_P_hr_ISS_ISS[0] += a_data[(int32_T)(s12_iter - 1)] * b_1[i]; - s12_iter++; - hr_P_hr_ISS_ISS[1] += a_data[(int32_T)(s12_iter - 1)] * b_1[i]; - s12_iter++; - hr_P_hr_ISS_ISS[2] += a_data[(int32_T)(s12_iter - 1)] * b_1[i]; - } - - ar += 3; - } - } - - // solve Ax = B with guass newton - if ((real_T)hr_P_hr_ISS_ISS[2] < 1.0E-5) { - // This is a point that is truly in our face because we have no - // parallax on it. I only see this happen when using perfect camera - // positions and the robot is stationary (our noise usually makes it so - // that inverse depth is non zero). - hr_P_hr_ISS_ISS[2] = 1.0E-5F; - } - - // Converting the inverse depth parameterization into the global coordinate - // frame. - hr_quat_ISS2hr_idx_0 = hr_P_hr_ISS_ISS[0] / hr_P_hr_ISS_ISS[2]; - rtb_Switch1 = hr_P_hr_ISS_ISS[1] / hr_P_hr_ISS_ISS[2]; - hr_quat_ISS2hr_idx_1 = 1.0F / hr_P_hr_ISS_ISS[2]; - for (i = 0; i < 3; i++) { - rtb_global_points[(int32_T)(i + (int32_T)(num_original << 2))] = 0.0F; - rtb_global_points[(int32_T)(i + (int32_T)(num_original << 2))] += - global_tf_camera1[i] * hr_quat_ISS2hr_idx_0; - rtb_global_points[(int32_T)(i + (int32_T)(num_original << 2))] += - global_tf_camera1[(int32_T)(i + 4)] * rtb_Switch1; - rtb_global_points[(int32_T)(i + (int32_T)(num_original << 2))] += - global_tf_camera1[(int32_T)(i + 8)] * hr_quat_ISS2hr_idx_1; - rtb_global_points[(int32_T)(i + (int32_T)(num_original << 2))] += - global_tf_camera1[(int32_T)(i + 12)]; - } - } - - // End of MATLAB Function: '/compute_of_global_points' - - // Switch: '/Switch3' - // ':1:8' - rtb_Compare = (1 > est_estimator_P->Switch3_Threshold_d); - for (i = 0; i < 13689; i++) { - rtb_Switch1 = est_estimator_B->Switch1[i]; - if (!rtb_Compare) { - rtb_Switch1 = est_estimator_B->Assignment[i]; - } - - est_estimator_B->Switch1[i] = rtb_Switch1; - } - - // End of Switch: '/Switch3' - - // S-Function (ex_of_residual_and_h): '/ex_of_residual_and_h' - rtb_ex_of_residual_and_h_o1 = of_residual_and_h((real32_T*) - &est_estimator_B->of_measured[0], (real32_T*)&rtb_global_points[0], - (real32_T*)&rtb_camera_tf_global[0], (int32_T*)&rtb_valid_out[0], - handrail_knowledge_dims, ASE_OF_NUM_AUG, ASE_OF_NUM_FEATURES, - est_estimator_P->tun_ase_mahal_distance_max, - est_estimator_P->tun_ase_of_r_mag, - est_estimator_P->tun_ase_navcam_inv_focal_length, - est_estimator_P->tun_ase_navcam_distortion, (real32_T*) - &est_estimator_B->Switch1[0], &rtb_ex_of_residual_and_h_o2[0], - &est_estimator_B->ex_of_residual_and_h_o3[0], &rtb_ex_of_residual_and_h_o4, - &rtb_ex_of_residual_and_h_o5, &rtb_ex_of_residual_and_h_o6[0], - &est_estimator_B->ex_of_residual_and_h_o7[0]); - - // Product: '/Product' - for (i = 0; i < 96; i++) { - A[i] = rtb_ex_of_residual_and_h_o2[i] * rtb_ex_of_residual_and_h_o2[i]; - } - - // End of Product: '/Product' - - // Sum: '/Sum of Elements' - rtb_Switch1 = A[0]; - for (ar = 0; ar < 95; ar++) { - rtb_Switch1 += A[(int32_T)(ar + 1)]; - } - - // End of Sum: '/Sum of Elements' - - // Sqrt: '/Sqrt' - rtb_Switch1 = (real32_T)sqrt((real_T)rtb_Switch1); - - // S-Function (ex_compute_delta_state_and_cov): '/ex_compute_delta_state_and_cov' - rtb_ex_compute_delta_state_an_g = compute_delta_state_and_cov((real32_T*) - &rtb_ex_of_residual_and_h_o2[0], rtb_ex_of_residual_and_h_o1, (real32_T*) - &est_estimator_B->ex_of_residual_and_h_o3[0], 96, 117, - rtb_ex_of_residual_and_h_o4, (real32_T*) - &est_estimator_B->ex_of_residual_and_h_o7[0], (real32_T*) - &est_estimator_B->Switch1[0], &rtb_ex_compute_delta_state_and_[0], - &est_estimator_B->ex_compute_delta_state_an_e[0]); - - // S-Function (ex_apply_delta_state): '/ex_apply_delta_state' - apply_delta_state((real32_T*)&rtb_ex_compute_delta_state_and_[0], 117, - est_estimator_P->Constant_Value_c, (real32_T*) - &rtb_Merge_o[0], (real32_T*)&accel[0], (real32_T*) - &UnitDelay_DSTATE_n_V_B_ISS_ISS[0], (real32_T*) - &UnitDelay_DSTATE_n_accel_bias[0], (real32_T*) - &UnitDelay_DSTATE_n_P_EST_ISS_IS[0], (real32_T*) - &UnitDelay_DSTATE_n_ml_quat_ISS2[0], (real32_T*) - &UnitDelay_DSTATE_n_ml_P_cam_ISS[0], - UnitDelay_DSTATE_n_kfl_status, (real32_T*) - &UnitDelay_DSTATE_n_of_quat_ISS2[0], (real32_T*) - &UnitDelay_DSTATE_n_of_P_cam_ISS[0], - &rtb_ex_apply_delta_state_o1[0], - &rtb_ex_apply_delta_state_o2[0], - &rtb_ex_apply_delta_state_o3[0], - &rtb_ex_apply_delta_state_o4[0], - &rtb_ex_apply_delta_state_o5[0], - &rtb_ex_apply_delta_state_o6[0], - &rtb_ex_apply_delta_state_o7[0], - &rtb_ex_apply_delta_state_o8, - &rtb_ex_apply_delta_state_o9[0], - &rtb_ex_apply_delta_state_o10[0]); - - // Logic: '/Logical Operator1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // Product: '/Divide' - // RelationalOperator: '/Relational Operator1' - // Sum: '/Add' - // UnitDelay: '/Unit Delay2' - - rtb_Compare = ((est_estimator_P->ase_minumum_resid_thresh > (real_T) - ((est_estimator_P->UnitDelay2_InitialCondition_f - - rtb_Switch1) / rtb_Switch1)) || - (rtb_ex_compute_delta_state_an_g != 0) || - (rtb_ex_of_residual_and_h_o1 != 0)); - - // Switch: '/Switch' incorporates: - // BusAssignment: '/Bus Assignment1' - // BusAssignment: '/Bus Assignment1' - // Constant: '/Constant' - // Constant: '/Constant2' - // DataTypeConversion: '/Data Type Conversion' - - if (!rtb_Compare) { - rtb_Merge_o[0] = rtb_ex_apply_delta_state_o1[0]; - rtb_Merge_o[1] = rtb_ex_apply_delta_state_o1[1]; - rtb_Merge_o[2] = rtb_ex_apply_delta_state_o1[2]; - rtb_Merge_o[3] = rtb_ex_apply_delta_state_o1[3]; - accel[0] = rtb_ex_apply_delta_state_o2[0]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[0] = rtb_ex_apply_delta_state_o3[0]; - UnitDelay_DSTATE_n_accel_bias[0] = rtb_ex_apply_delta_state_o4[0]; - accel[1] = rtb_ex_apply_delta_state_o2[1]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[1] = rtb_ex_apply_delta_state_o3[1]; - UnitDelay_DSTATE_n_accel_bias[1] = rtb_ex_apply_delta_state_o4[1]; - accel[2] = rtb_ex_apply_delta_state_o2[2]; - UnitDelay_DSTATE_n_V_B_ISS_ISS[2] = rtb_ex_apply_delta_state_o3[2]; - UnitDelay_DSTATE_n_accel_bias[2] = rtb_ex_apply_delta_state_o4[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[0] = rtb_ex_apply_delta_state_o6[0]; - UnitDelay_DSTATE_n_ml_quat_ISS2[1] = rtb_ex_apply_delta_state_o6[1]; - UnitDelay_DSTATE_n_ml_quat_ISS2[2] = rtb_ex_apply_delta_state_o6[2]; - UnitDelay_DSTATE_n_ml_quat_ISS2[3] = rtb_ex_apply_delta_state_o6[3]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[0] = rtb_ex_apply_delta_state_o7[0]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[1] = rtb_ex_apply_delta_state_o7[1]; - UnitDelay_DSTATE_n_ml_P_cam_ISS[2] = rtb_ex_apply_delta_state_o7[2]; - memcpy(&UnitDelay_DSTATE_n_of_quat_ISS2[0], &rtb_ex_apply_delta_state_o9[0], - (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&UnitDelay_DSTATE_n_of_P_cam_ISS[0], &rtb_ex_apply_delta_state_o10 - [0], (uint32_T)(48U * sizeof(real32_T))); - UnitDelay_DSTATE_n_kfl_status = rtb_ex_apply_delta_state_o8; - rtb_numFeatures_f = rtb_ex_of_residual_and_h_o5; - rtb_Saturation_n = est_estimator_P->Constant_Value_dd; - for (i = 0; i < 50; i++) { - UnitDelay_DSTATE_n_of_mahal_dis[i] = (real_T) - rtb_ex_of_residual_and_h_o6[i]; - UnitDelay_DSTATE_n_ml_mahal_dis[i] = est_estimator_P->Constant2_Value[i]; - } - - UnitDelay_DSTATE_n_P_EST_ISS_IS[0] = rtb_ex_apply_delta_state_o5[0]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[1] = rtb_ex_apply_delta_state_o5[1]; - UnitDelay_DSTATE_n_P_EST_ISS_IS[2] = rtb_ex_apply_delta_state_o5[2]; - } - - // End of Switch: '/Switch' - - // Switch: '/Switch1' - for (i = 0; i < 13689; i++) { - rtb_Switch1 = est_estimator_B->Switch1[i]; - if (!rtb_Compare) { - rtb_Switch1 = est_estimator_B->ex_compute_delta_state_an_e[i]; - } - - est_estimator_B->Switch1[i] = rtb_Switch1; - } - - // End of Switch: '/Switch1' - - // SignalConversion: '/OutportBufferForstate_out' incorporates: - // SignalConversion: '/Signal Conversion' - - rtb_Merge2.quat_ISS2B[0] = rtb_Merge_o[0]; - rtb_Merge2.quat_ISS2B[1] = rtb_Merge_o[1]; - rtb_Merge2.quat_ISS2B[2] = rtb_Merge_o[2]; - rtb_Merge2.quat_ISS2B[3] = rtb_Merge_o[3]; - rtb_Merge2.omega_B_ISS_B[0] = rtb_ImpAsg_InsertedFor_Out1_at_[0]; - rtb_Merge2.gyro_bias[0] = accel[0]; - rtb_Merge2.V_B_ISS_ISS[0] = UnitDelay_DSTATE_n_V_B_ISS_ISS[0]; - rtb_Merge2.A_B_ISS_ISS[0] = rtb_Sum_k1; - rtb_Merge2.accel_bias[0] = UnitDelay_DSTATE_n_accel_bias[0]; - rtb_Merge2.P_B_ISS_ISS[0] = rtb_Sum_l; - rtb_Merge2.omega_B_ISS_B[1] = rtb_ImpAsg_InsertedFor_Out1_at_[1]; - rtb_Merge2.gyro_bias[1] = accel[1]; - rtb_Merge2.V_B_ISS_ISS[1] = UnitDelay_DSTATE_n_V_B_ISS_ISS[1]; - rtb_Merge2.A_B_ISS_ISS[1] = rtb_Gain1_f; - rtb_Merge2.accel_bias[1] = UnitDelay_DSTATE_n_accel_bias[1]; - rtb_Merge2.P_B_ISS_ISS[1] = UnitDelay_DSTATE_n_P_B_ISS_ISS_; - rtb_Merge2.omega_B_ISS_B[2] = rtb_ImpAsg_InsertedFor_Out1_at_[2]; - rtb_Merge2.gyro_bias[2] = accel[2]; - rtb_Merge2.V_B_ISS_ISS[2] = UnitDelay_DSTATE_n_V_B_ISS_ISS[2]; - rtb_Merge2.A_B_ISS_ISS[2] = UnitDelay_DSTATE_n_A_B_ISS_ISS_; - rtb_Merge2.accel_bias[2] = UnitDelay_DSTATE_n_accel_bias[2]; - rtb_Merge2.P_B_ISS_ISS[2] = UnitDelay_DSTATE_n_P_B_ISS_IS_0; - rtb_Merge2.confidence = rtb_Switch_m; - rtb_Merge2.aug_state_enum = UnitDelay_DSTATE_aug_state_enum; - rtb_Merge2.ml_quat_ISS2cam[0] = UnitDelay_DSTATE_n_ml_quat_ISS2[0]; - rtb_Merge2.ml_quat_ISS2cam[1] = UnitDelay_DSTATE_n_ml_quat_ISS2[1]; - rtb_Merge2.ml_quat_ISS2cam[2] = UnitDelay_DSTATE_n_ml_quat_ISS2[2]; - rtb_Merge2.ml_quat_ISS2cam[3] = UnitDelay_DSTATE_n_ml_quat_ISS2[3]; - rtb_Merge2.ml_P_cam_ISS_ISS[0] = UnitDelay_DSTATE_n_ml_P_cam_ISS[0]; - rtb_Merge2.ml_P_cam_ISS_ISS[1] = UnitDelay_DSTATE_n_ml_P_cam_ISS[1]; - rtb_Merge2.ml_P_cam_ISS_ISS[2] = UnitDelay_DSTATE_n_ml_P_cam_ISS[2]; - memcpy(&rtb_Merge2.of_quat_ISS2cam[0], &UnitDelay_DSTATE_n_of_quat_ISS2[0], - (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&rtb_Merge2.of_P_cam_ISS_ISS[0], &UnitDelay_DSTATE_n_of_P_cam_ISS[0], - (uint32_T)(48U * sizeof(real32_T))); - memcpy(&rtb_Merge2.cov_diag[0], &UnitDelay_DSTATE_n_cov_diag[0], (uint32_T) - (117U * sizeof(ase_cov_datatype))); - rtb_Merge2.kfl_status = UnitDelay_DSTATE_n_kfl_status; - rtb_Merge2.update_OF_tracks_cnt = rtb_numFeatures_f; - rtb_Merge2.update_ML_features_cnt = rtb_Saturation_n; - memcpy(&rtb_Merge2.of_mahal_distance[0], &UnitDelay_DSTATE_n_of_mahal_dis[0], - (uint32_T)(50U * sizeof(real_T))); - memcpy(&rtb_Merge2.ml_mahal_distance[0], &UnitDelay_DSTATE_n_ml_mahal_dis[0], - (uint32_T)(50U * sizeof(real_T))); - rtb_Merge2.hr_P_hr_ISS_ISS[0] = UnitDelay_DSTATE_n_hr_P_hr_ISS_; - rtb_Merge2.hr_P_hr_ISS_ISS[1] = UnitDelay_DSTATE_n_hr_P_hr_IS_0; - rtb_Merge2.hr_P_hr_ISS_ISS[2] = UnitDelay_DSTATE_n_hr_P_hr_IS_1; - rtb_Merge2.hr_quat_ISS2hr[0] = UnitDelay_DSTATE_n_hr_quat_ISS2; - rtb_Merge2.hr_quat_ISS2hr[1] = UnitDelay_DSTATE_n_hr_quat_IS_0; - rtb_Merge2.hr_quat_ISS2hr[2] = UnitDelay_DSTATE_n_hr_quat_IS_1; - rtb_Merge2.hr_quat_ISS2hr[3] = UnitDelay_DSTATE_n_hr_quat_IS_2; - rtb_Merge2.P_EST_ISS_ISS[0] = UnitDelay_DSTATE_n_P_EST_ISS_IS[0]; - rtb_Merge2.P_EST_ISS_ISS[1] = UnitDelay_DSTATE_n_P_EST_ISS_IS[1]; - rtb_Merge2.P_EST_ISS_ISS[2] = UnitDelay_DSTATE_n_P_EST_ISS_IS[2]; - - // End of Outputs for SubSystem: '/OF Update' - // End of Outputs for SubSystem: '/Optical_Flow_Update' - break; - - case 2: - // Outputs for IfAction SubSystem: '/If Action Subsystem1' incorporates: - // ActionPort: '/Action Port' - - // SignalConversion: '/Signal Conversion' incorporates: - // BusAssignment: '/Bus Assignment' - // BusAssignment: '/Bus Assignment1' - // BusCreator: '/Bus Creator2' - // Constant: '/Constant' - // Constant: '/Constant1' - // S-Function (sfix_bitop): '/Bitwise Operator' - // UnitDelay: '/Unit Delay10' - // UnitDelay: '/Unit Delay11' - // UnitDelay: '/Unit Delay12' - // UnitDelay: '/Unit Delay13' - // UnitDelay: '/Unit Delay14' - // UnitDelay: '/Unit Delay15' - // UnitDelay: '/Unit Delay3' - // UnitDelay: '/Unit Delay6' - // UnitDelay: '/Unit Delay7' - // UnitDelay: '/Unit Delay8' - - rtb_Merge2.quat_ISS2B[0] = rtb_Product1[0]; - rtb_Merge2.quat_ISS2B[1] = rtb_Product1[1]; - rtb_Merge2.quat_ISS2B[2] = rtb_Product1[2]; - rtb_Merge2.quat_ISS2B[3] = rtb_Product1[3]; - rtb_Merge2.omega_B_ISS_B[0] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - rtb_Merge2.gyro_bias[0] = est_estimator_DW->UnitDelay3_DSTATE[0]; - rtb_Merge2.V_B_ISS_ISS[0] = rtb_Sum2[0]; - rtb_Merge2.A_B_ISS_ISS[0] = rtb_Sum1_k3[0]; - rtb_Merge2.accel_bias[0] = est_estimator_DW->UnitDelay6_DSTATE[0]; - rtb_Merge2.P_B_ISS_ISS[0] = est_estimator_DW->UnitDelay7_DSTATE[0]; - rtb_Merge2.omega_B_ISS_B[1] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - rtb_Merge2.gyro_bias[1] = est_estimator_DW->UnitDelay3_DSTATE[1]; - rtb_Merge2.V_B_ISS_ISS[1] = rtb_Sum2[1]; - rtb_Merge2.A_B_ISS_ISS[1] = rtb_Sum1_k3[1]; - rtb_Merge2.accel_bias[1] = est_estimator_DW->UnitDelay6_DSTATE[1]; - rtb_Merge2.P_B_ISS_ISS[1] = est_estimator_DW->UnitDelay7_DSTATE[1]; - rtb_Merge2.omega_B_ISS_B[2] = hr_quat_ISS2hr_idx_0; - rtb_Merge2.gyro_bias[2] = est_estimator_DW->UnitDelay3_DSTATE[2]; - rtb_Merge2.V_B_ISS_ISS[2] = rtb_Sum2[2]; - rtb_Merge2.A_B_ISS_ISS[2] = rtb_Sum1_k3[2]; - rtb_Merge2.accel_bias[2] = est_estimator_DW->UnitDelay6_DSTATE[2]; - rtb_Merge2.P_B_ISS_ISS[2] = est_estimator_DW->UnitDelay7_DSTATE[2]; - rtb_Merge2.confidence = est_estimator_DW->UnitDelay8_DSTATE; - rtb_Merge2.aug_state_enum = rtb_BusAssignment_aug_state_enu; - rtb_Merge2.ml_quat_ISS2cam[0] = est_estimator_DW->UnitDelay10_DSTATE[0]; - rtb_Merge2.ml_quat_ISS2cam[1] = est_estimator_DW->UnitDelay10_DSTATE[1]; - rtb_Merge2.ml_quat_ISS2cam[2] = est_estimator_DW->UnitDelay10_DSTATE[2]; - rtb_Merge2.ml_quat_ISS2cam[3] = est_estimator_DW->UnitDelay10_DSTATE[3]; - rtb_Merge2.ml_P_cam_ISS_ISS[0] = est_estimator_DW->UnitDelay11_DSTATE[0]; - rtb_Merge2.ml_P_cam_ISS_ISS[1] = est_estimator_DW->UnitDelay11_DSTATE[1]; - rtb_Merge2.ml_P_cam_ISS_ISS[2] = est_estimator_DW->UnitDelay11_DSTATE[2]; - memcpy(&rtb_Merge2.of_quat_ISS2cam[0], &est_estimator_DW-> - UnitDelay12_DSTATE[0], (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&rtb_Merge2.of_P_cam_ISS_ISS[0], - &est_estimator_DW->UnitDelay13_DSTATE[0], (uint32_T)(48U * sizeof - (real32_T))); - memcpy(&rtb_Merge2.cov_diag[0], &est_estimator_DW->UnitDelay14_DSTATE[0], - (uint32_T)(117U * sizeof(ase_cov_datatype))); - rtb_Merge2.kfl_status = (uint16_T)(int32_T)((int32_T) - est_estimator_DW->UnitDelay15_DSTATE & (int32_T) - est_estimator_P->BitwiseOperator_BitMask_c); - rtb_Merge2.update_OF_tracks_cnt = est_estimator_P->Constant_Value_oa; - rtb_Merge2.update_ML_features_cnt = est_estimator_P->Constant1_Value_f; - memcpy(&rtb_Merge2.of_mahal_distance[0], &rtb_UnitDelay18[0], (uint32_T)(50U - * sizeof(real_T))); - memcpy(&rtb_Merge2.ml_mahal_distance[0], &rtb_UnitDelay19[0], (uint32_T)(50U - * sizeof(real_T))); - rtb_Merge2.hr_P_hr_ISS_ISS[0] = rtb_Add_e[0]; - rtb_Merge2.hr_P_hr_ISS_ISS[1] = rtb_Add_e[1]; - rtb_Merge2.hr_P_hr_ISS_ISS[2] = rtb_Add_e[2]; - rtb_Merge2.hr_quat_ISS2hr[0] = rtb_UnitDelay25[0]; - rtb_Merge2.hr_quat_ISS2hr[1] = rtb_UnitDelay25[1]; - rtb_Merge2.hr_quat_ISS2hr[2] = rtb_UnitDelay25[2]; - rtb_Merge2.hr_quat_ISS2hr[3] = rtb_UnitDelay25[3]; - rtb_Merge2.P_EST_ISS_ISS[0] = rtb_P_B_ISS_ISS[0]; - rtb_Merge2.P_EST_ISS_ISS[1] = rtb_P_B_ISS_ISS[1]; - rtb_Merge2.P_EST_ISS_ISS[2] = rtb_P_B_ISS_ISS[2]; - memcpy(&est_estimator_B->Switch1[0], &est_estimator_B->Assignment[0], - (uint32_T)(13689U * sizeof(real32_T))); - - // End of Outputs for SubSystem: '/If Action Subsystem1' - break; - } - - // End of If: '/If' - - // If: '/If' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - // Inport: '/Vision Registration' - // Inport: '/cmc_msg' - // Inport: '/P_in' - // Logic: '/Logical Operator' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator3' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // S-Function (sfix_bitop): '/Bitwise Operator1' - // SignalConversion: '/Signal Conversion' - - if (((est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_perching) && ((int32_T) - est_estimator_U_VisionRegistration->cvs_handrail_pulse != 0)) || - (((int32_T)est_estimator_U_VisionRegistration->cvs_landmark_pulse != 0) && - ((est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_map) || - (est_estimator_U_cmc_msg_o->localization_mode_cmd == - est_estimator_P->ase_local_mode_docking)))) { - // Outputs for IfAction SubSystem: '/If Action Subsystem2' incorporates: - // ActionPort: '/Action Port' - - rtb_VectorConcatenate_c[0] = est_estimator_P->Constant3_Value_oe; - - // Gain: '/Gain' incorporates: - // Constant: '/Constant5' - // Constant: '/Constant3' - - rtb_VectorConcatenate_c[1] = est_estimator_P->Gain_Gain_d * - est_estimator_P->Constant5_Value[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[2] = est_estimator_P->Constant5_Value[1]; - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[3] = est_estimator_P->Gain1_Gain_n * - est_estimator_P->Constant5_Value[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[4] = est_estimator_P->Constant5_Value[2]; - rtb_VectorConcatenate_c[5] = est_estimator_P->Constant2_Value_m2; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant5' - // Constant: '/Constant2' - - rtb_VectorConcatenate_c[6] = est_estimator_P->Gain2_Gain_k * - est_estimator_P->Constant5_Value[0]; - - // Gain: '/Gain3' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[7] = est_estimator_P->Gain3_Gain_f * - est_estimator_P->Constant5_Value[1]; - - // Gain: '/Gain4' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[8] = est_estimator_P->Gain4_Gain_b * - est_estimator_P->Constant5_Value[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[9] = est_estimator_P->Constant5_Value[0]; - rtb_VectorConcatenate_c[10] = est_estimator_P->Constant1_Value_i; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant5' - // Constant: '/Constant1' - - rtb_VectorConcatenate_c[11] = est_estimator_P->Gain5_Gain_f * - est_estimator_P->Constant5_Value[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[12] = est_estimator_P->Constant5_Value[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[13] = est_estimator_P->Constant5_Value[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' incorporates: - // Constant: '/Constant5' - - rtb_VectorConcatenate_c[14] = est_estimator_P->Constant5_Value[2]; - rtb_VectorConcatenate_c[15] = est_estimator_P->Constant_Value_m; - rtb_VectorConcatenate_p[0] = est_estimator_P->Constant3_Value_op; - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant3' - - rtb_Add_e[0] = est_estimator_P->Gain1_Gain_o * rtb_Merge2.omega_B_ISS_B[0]; - rtb_Add_e[1] = est_estimator_P->Gain1_Gain_o * rtb_Merge2.omega_B_ISS_B[1]; - rtb_Add_e[2] = est_estimator_P->Gain1_Gain_o * rtb_Merge2.omega_B_ISS_B[2]; - - // Gain: '/Gain' - rtb_VectorConcatenate_p[1] = est_estimator_P->Gain_Gain_ng * rtb_Add_e[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' - rtb_VectorConcatenate_p[2] = rtb_Add_e[1]; - - // Gain: '/Gain1' - rtb_VectorConcatenate_p[3] = est_estimator_P->Gain1_Gain_a * rtb_Add_e[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' - rtb_VectorConcatenate_p[4] = rtb_Add_e[2]; - rtb_VectorConcatenate_p[5] = est_estimator_P->Constant2_Value_o; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant2' - - rtb_VectorConcatenate_p[6] = est_estimator_P->Gain2_Gain_b * rtb_Add_e[0]; - - // Gain: '/Gain3' - rtb_VectorConcatenate_p[7] = est_estimator_P->Gain3_Gain_o * rtb_Add_e[1]; - - // Gain: '/Gain4' - rtb_VectorConcatenate_p[8] = est_estimator_P->Gain4_Gain_i * rtb_Add_e[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' - rtb_VectorConcatenate_p[9] = rtb_Add_e[0]; - rtb_VectorConcatenate_p[10] = est_estimator_P->Constant1_Value_b; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant1' - - rtb_VectorConcatenate_p[11] = est_estimator_P->Gain5_Gain_c * rtb_Add_e[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' - rtb_VectorConcatenate_p[12] = rtb_Add_e[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' - rtb_VectorConcatenate_p[13] = rtb_Add_e[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' - rtb_VectorConcatenate_p[14] = rtb_Add_e[2]; - rtb_VectorConcatenate_p[15] = est_estimator_P->Constant_Value_px; - - // Product: '/Product2' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant3' - // Constant: '/Constant' - // Product: '/Product' - // Sum: '/Add' - - for (i = 0; i < 16; i++) { - rtb_VectorConcatenate[i] = (est_estimator_P->Constant3_Value_da * - rtb_VectorConcatenate_c[i] * - est_estimator_P->tun_ase_ml_forward_projection_time + - rtb_VectorConcatenate_p[i]) * est_estimator_P->Constant1_Value_a * - est_estimator_P->tun_ase_ml_forward_projection_time; - } - - // End of Product: '/Product2' - - // MATLAB Function: '/MATLAB Function' - est_estimator_MATLABFunction(rtb_VectorConcatenate, - &est_estimator_B->sf_MATLABFunction_l0); - - // Product: '/Product5' incorporates: - // Constant: '/Constant' - - hr_quat_ISS2hr_idx_1 = est_estimator_P->tun_ase_ml_forward_projection_time * - est_estimator_P->tun_ase_ml_forward_projection_time * - est_estimator_P->tun_ase_ml_forward_projection_time; - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m < 4; b_m++) { - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - global_tf_camera1[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_hq[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - global_tf_camera1[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_p[(int32_T)(i << 2)] * - rtb_VectorConcatenate_c[b_m]; - rtb_VectorConcatenate_hq[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_c[(int32_T)(i << 2)] * - rtb_VectorConcatenate_p[b_m]; - global_tf_camera1[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate_c[(int32_T)(b_m + 4)]; - rtb_VectorConcatenate_hq[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate_p[(int32_T)(b_m + 4)]; - global_tf_camera1[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate_c[(int32_T)(b_m + 8)]; - rtb_VectorConcatenate_hq[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate_p[(int32_T)(b_m + 8)]; - global_tf_camera1[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate_c[(int32_T)(b_m + 12)]; - rtb_VectorConcatenate_hq[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate_p[(int32_T)(b_m + 12)]; - } - } - - // Sum: '/Add1' incorporates: - // Constant: '/Constant2' - // Product: '/Product1' - // Product: '/Product3' - // Product: '/Product4' - // Product: '/Product5' - // Sum: '/Add2' - - for (i = 0; i < 4; i++) { - rtb_VectorConcatenate[(int32_T)(i << 2)] = (global_tf_camera1[(int32_T)(i << - 2)] - rtb_VectorConcatenate_hq[(int32_T)(i << 2)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_mp + - est_estimator_B->sf_MATLABFunction_l0.y[(int32_T)(i << 2)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(i << 2))] = - (global_tf_camera1[(int32_T)((int32_T)(i << 2) + 1)] - - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 1)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_mp + - est_estimator_B->sf_MATLABFunction_l0.y[(int32_T)((int32_T)(i << 2) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(i << 2))] = - (global_tf_camera1[(int32_T)((int32_T)(i << 2) + 2)] - - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 2)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_mp + - est_estimator_B->sf_MATLABFunction_l0.y[(int32_T)((int32_T)(i << 2) + 2)]; - rtb_VectorConcatenate[(int32_T)(3 + (int32_T)(i << 2))] = - (global_tf_camera1[(int32_T)((int32_T)(i << 2) + 3)] - - rtb_VectorConcatenate_hq[(int32_T)((int32_T)(i << 2) + 3)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_mp + - est_estimator_B->sf_MATLABFunction_l0.y[(int32_T)((int32_T)(i << 2) + 3)]; - } - - // End of Sum: '/Add1' - - // Product: '/Product1' - for (i = 0; i < 4; i++) { - rtb_Sum_k1 = rtb_VectorConcatenate[(int32_T)(i + 12)] * - rtb_Merge2.quat_ISS2B[3] + (rtb_VectorConcatenate[(int32_T)(i + 8)] * - rtb_Merge2.quat_ISS2B[2] + (rtb_VectorConcatenate[(int32_T)(i + 4)] * - rtb_Merge2.quat_ISS2B[1] + rtb_VectorConcatenate[i] * - rtb_Merge2.quat_ISS2B[0])); - rtb_Product1[i] = rtb_Sum_k1; - } - - // If: '/If' incorporates: - // Inport: '/In1' - - if (rtb_Product1[3] < 0.0F) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize(rtb_Product1, rtb_UnitDelay25, - (P_Normalize_est_estimator_T *)&est_estimator_P->Normalize_i); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_UnitDelay25[0] = rtb_Product1[0]; - rtb_UnitDelay25[1] = rtb_Product1[1]; - rtb_UnitDelay25[2] = rtb_Product1[2]; - rtb_UnitDelay25[3] = rtb_Product1[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Switch1 = (real32_T)sqrt((real_T)(((rtb_UnitDelay25[0] * - rtb_UnitDelay25[0] + rtb_UnitDelay25[1] * rtb_UnitDelay25[1]) + - rtb_UnitDelay25[2] * rtb_UnitDelay25[2]) + rtb_UnitDelay25[3] * - rtb_UnitDelay25[3])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_Switch1 > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize_p(rtb_UnitDelay25, rtb_Switch1, rtb_Product1); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Product1[0] = rtb_UnitDelay25[0]; - rtb_Product1[1] = rtb_UnitDelay25[1]; - rtb_Product1[2] = rtb_UnitDelay25[2]; - rtb_Product1[3] = rtb_UnitDelay25[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Switch1 = rtb_Product1[3] * rtb_Product1[3] * - est_estimator_P->Gain_Gain_c - (real32_T) - est_estimator_P->Constant1_Value_m; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_m[i]; - } - - rtb_Assignment_b[0] = rtb_Switch1; - rtb_Assignment_b[4] = rtb_Switch1; - rtb_Assignment_b[8] = rtb_Switch1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' - rtb_Switch1 = est_estimator_P->Gain1_Gain_nq * rtb_Product1[3]; - - // MultiPortSwitch: '/Multiport Switch1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant4' - - switch ((int32_T)est_estimator_U_cmc_msg_o->localization_mode_cmd) { - case 0: - rtb_Add_e[0] = est_estimator_P->tun_abp_p_navcam_imu_est[0]; - rtb_Add_e[1] = est_estimator_P->tun_abp_p_navcam_imu_est[1]; - rtb_Add_e[2] = est_estimator_P->tun_abp_p_navcam_imu_est[2]; - break; - - case 1: - rtb_Add_e[0] = est_estimator_P->tun_abp_p_dockcam_imu_est[0]; - rtb_Add_e[1] = est_estimator_P->tun_abp_p_dockcam_imu_est[1]; - rtb_Add_e[2] = est_estimator_P->tun_abp_p_dockcam_imu_est[2]; - break; - - default: - rtb_Add_e[0] = est_estimator_P->tun_abp_p_perchcam_imu_est[0]; - rtb_Add_e[1] = est_estimator_P->tun_abp_p_perchcam_imu_est[1]; - rtb_Add_e[2] = est_estimator_P->tun_abp_p_perchcam_imu_est[2]; - break; - } - - // End of MultiPortSwitch: '/Multiport Switch1' - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_a[0] = (real32_T)est_estimator_P->Constant3_Value_m; - tmp_a[1] = rtb_Product1[2]; - tmp_a[2] = est_estimator_P->Gain_Gain_fr * rtb_Product1[1]; - tmp_a[3] = est_estimator_P->Gain1_Gain_c * rtb_Product1[2]; - tmp_a[4] = (real32_T)est_estimator_P->Constant3_Value_m; - tmp_a[5] = rtb_Product1[0]; - tmp_a[6] = rtb_Product1[1]; - tmp_a[7] = est_estimator_P->Gain2_Gain_hc * rtb_Product1[0]; - tmp_a[8] = (real32_T)est_estimator_P->Constant3_Value_m; - - // Math: '/Math Function' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = rtb_Product1[0] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 3)] = rtb_Product1[1] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 6)] = rtb_Product1[2] * rtb_Product1[i]; - } - - // End of Math: '/Math Function' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Assignment_n[(int32_T)(3 * i)] = (rtb_Assignment_b[i] - rtb_Switch1 * - tmp_a[i]) + rtb_Product1_0[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_g; - rtb_Assignment_n[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 3)] - tmp_a[(int32_T)(i + 3)] * rtb_Switch1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_g; - rtb_Assignment_n[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 6)] - tmp_a[(int32_T)(i + 6)] * rtb_Switch1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_g; - } - - // End of Sum: '/Sum1' - - // Product: '/Product' - for (i = 0; i < 3; i++) { - rtb_ImpAsg_InsertedFor_Out1_a_d[i] = rtb_Assignment_n[(int32_T)(i + 6)] * - rtb_Add_e[2] + (rtb_Assignment_n[(int32_T)(i + 3)] * rtb_Add_e[1] + - rtb_Assignment_n[i] * rtb_Add_e[0]); - } - - // MultiPortSwitch: '/Multiport Switch' incorporates: - // Constant: '/Constant6' - // Constant: '/Constant7' - // Constant: '/Constant8' - - switch ((int32_T)est_estimator_U_cmc_msg_o->localization_mode_cmd) { - case 0: - rtb_UnitDelay25[0] = est_estimator_P->tun_abp_q_body2navcam[0]; - rtb_UnitDelay25[1] = est_estimator_P->tun_abp_q_body2navcam[1]; - rtb_UnitDelay25[2] = est_estimator_P->tun_abp_q_body2navcam[2]; - rtb_UnitDelay25[3] = est_estimator_P->tun_abp_q_body2navcam[3]; - break; - - case 1: - rtb_UnitDelay25[0] = est_estimator_P->tun_abp_q_body2dockcam[0]; - rtb_UnitDelay25[1] = est_estimator_P->tun_abp_q_body2dockcam[1]; - rtb_UnitDelay25[2] = est_estimator_P->tun_abp_q_body2dockcam[2]; - rtb_UnitDelay25[3] = est_estimator_P->tun_abp_q_body2dockcam[3]; - break; - - default: - rtb_UnitDelay25[0] = est_estimator_P->tun_abp_q_body2perchcam[0]; - rtb_UnitDelay25[1] = est_estimator_P->tun_abp_q_body2perchcam[1]; - rtb_UnitDelay25[2] = est_estimator_P->tun_abp_q_body2perchcam[2]; - rtb_UnitDelay25[3] = est_estimator_P->tun_abp_q_body2perchcam[3]; - break; - } - - // End of MultiPortSwitch: '/Multiport Switch' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Switch1 = rtb_UnitDelay25[3] * rtb_UnitDelay25[3] * - est_estimator_P->Gain_Gain_cu - (real32_T) - est_estimator_P->Constant1_Value_n; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_b[i]; - } - - rtb_Assignment_b[0] = rtb_Switch1; - rtb_Assignment_b[4] = rtb_Switch1; - rtb_Assignment_b[8] = rtb_Switch1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' - rtb_Switch1 = est_estimator_P->Gain1_Gain_l * rtb_UnitDelay25[3]; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant3' - - memcpy(&rtb_Assignment6[0], &est_estimator_P->Constant3_Value_l2[0], - (uint32_T)(90U * sizeof(ase_cov_datatype))); - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - rtb_Merge2_1[0] = (real32_T)est_estimator_P->Constant3_Value_d; - rtb_Merge2_1[1] = rtb_UnitDelay25[2]; - rtb_Merge2_1[2] = est_estimator_P->Gain_Gain_ne * rtb_UnitDelay25[1]; - rtb_Merge2_1[3] = est_estimator_P->Gain1_Gain_j4 * rtb_UnitDelay25[2]; - rtb_Merge2_1[4] = (real32_T)est_estimator_P->Constant3_Value_d; - rtb_Merge2_1[5] = rtb_UnitDelay25[0]; - rtb_Merge2_1[6] = rtb_UnitDelay25[1]; - rtb_Merge2_1[7] = est_estimator_P->Gain2_Gain_n * rtb_UnitDelay25[0]; - rtb_Merge2_1[8] = (real32_T)est_estimator_P->Constant3_Value_d; - - // Product: '/Product1' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - - for (i = 0; i < 3; i++) { - S[i] = rtb_UnitDelay25[i] * rtb_UnitDelay25[0]; - S[(int32_T)(i + 3)] = rtb_UnitDelay25[i] * rtb_UnitDelay25[1]; - S[(int32_T)(i + 6)] = rtb_UnitDelay25[i] * rtb_UnitDelay25[2]; - } - - // End of Product: '/Product1' - - // SignalConversion: '/TmpSignal ConversionAtAssignment6Inport2' incorporates: - // Assignment: '/Assignment6' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_b[0] = (real32_T)est_estimator_P->Constant3_Value_pv; - tmp_b[1] = rtb_ImpAsg_InsertedFor_Out1_a_d[2]; - tmp_b[2] = est_estimator_P->Gain_Gain_fx * rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - tmp_b[3] = est_estimator_P->Gain1_Gain_ik * rtb_ImpAsg_InsertedFor_Out1_a_d - [2]; - tmp_b[4] = (real32_T)est_estimator_P->Constant3_Value_pv; - tmp_b[5] = rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - tmp_b[6] = rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - tmp_b[7] = est_estimator_P->Gain2_Gain_m * rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - tmp_b[8] = (real32_T)est_estimator_P->Constant3_Value_pv; - for (i = 0; i < 3; i++) { - // Assignment: '/Assignment' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Sum: '/Sum1' - - rtb_Assignment6[(int32_T)(6 * i)] = (rtb_Assignment_b[(int32_T)(3 * i)] - - rtb_Merge2_1[(int32_T)(3 * i)] * rtb_Switch1) + S[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_p0; - rtb_Assignment6[(int32_T)(1 + (int32_T)(6 * i))] = (rtb_Assignment_b - [(int32_T)((int32_T)(3 * i) + 1)] - rtb_Merge2_1[(int32_T)((int32_T)(3 * - i) + 1)] * rtb_Switch1) + S[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_p0; - rtb_Assignment6[(int32_T)(2 + (int32_T)(6 * i))] = (rtb_Assignment_b - [(int32_T)((int32_T)(3 * i) + 2)] - rtb_Merge2_1[(int32_T)((int32_T)(3 * - i) + 2)] * rtb_Switch1) + S[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_p0; - - // Assignment: '/Assignment6' - rtb_Assignment6[(int32_T)(3 + (int32_T)(6 * i))] = tmp_b[(int32_T)(3 * i)]; - rtb_Assignment6[(int32_T)(4 + (int32_T)(6 * i))] = tmp_b[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_Assignment6[(int32_T)(5 + (int32_T)(6 * i))] = tmp_b[(int32_T) - ((int32_T)(3 * i) + 2)]; - } - - // Assignment: '/Assignment1' - memcpy(&est_estimator_B->Assignment[0], &est_estimator_B->Switch1[0], - (uint32_T)(13689U * sizeof(real32_T))); - - // Product: '/Product1' incorporates: - // Math: '/Math Function1' - // Selector: '/Selector1' - - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 6; b_m++) { - Switch1[(int32_T)(i + (int32_T)(15 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 15; i_0++) { - Switch1[(int32_T)(i + (int32_T)(15 * b_m))] += - est_estimator_B->Switch1[(int32_T)((int32_T)(117 * i_0) + i)] * - rtb_Assignment6[(int32_T)((int32_T)(6 * i_0) + b_m)]; - } - } - } - - for (i = 0; i < 6; i++) { - // Assignment: '/Assignment1' incorporates: - // Product: '/Product1' - - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Assignment[(int32_T)((int32_T)(i + (int32_T)(117 * - (int32_T)(15 + b_m))) + 15)] = 0.0F; - for (i_0 = 0; i_0 < 15; i_0++) { - est_estimator_B->Assignment[(int32_T)((int32_T)(i + (int32_T)(117 * - (int32_T)(15 + b_m))) + 15)] = est_estimator_B->Assignment[(int32_T) - ((int32_T)((int32_T)((int32_T)(15 + b_m) * 117) + i) + 15)] + - rtb_Assignment6[(int32_T)((int32_T)(6 * i_0) + i)] * Switch1 - [(int32_T)((int32_T)(15 * b_m) + i_0)]; - } - } - - // Product: '/Product2' incorporates: - // Selector: '/Selector2' - - for (b_m = 0; b_m < 15; b_m++) { - rtb_Product2_b[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 15; i_0++) { - rtb_Product2_b[(int32_T)(i + (int32_T)(6 * b_m))] += rtb_Assignment6 - [(int32_T)((int32_T)(6 * i_0) + i)] * est_estimator_B->Switch1 - [(int32_T)((int32_T)(117 * b_m) + i_0)]; - } - } - - // End of Product: '/Product2' - } - - // Assignment: '/Assignment2' - for (i = 0; i < 15; i++) { - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Assignment[(int32_T)((int32_T)(b_m + (int32_T)(117 * i)) - + 15)] = rtb_Product2_b[(int32_T)((int32_T)(6 * i) + b_m)]; - } - } - - // End of Assignment: '/Assignment2' - for (i = 0; i < 6; i++) { - // Assignment: '/Assignment3' incorporates: - // Math: '/Math Function2' - - for (b_m = 0; b_m < 15; b_m++) { - est_estimator_B->Assignment[(int32_T)(b_m + (int32_T)(117 * (int32_T)(15 - + i)))] = rtb_Product2_b[(int32_T)((int32_T)(6 * b_m) + i)]; - } - - // End of Assignment: '/Assignment3' - - // Product: '/Product3' incorporates: - // Selector: '/Selector4' - - for (b_m = 0; b_m < 96; b_m++) { - rtb_Product3[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 15; i_0++) { - rtb_Product3[(int32_T)(i + (int32_T)(6 * b_m))] += - est_estimator_B->Switch1[(int32_T)((int32_T)((int32_T)(21 + b_m) * - 117) + i_0)] * rtb_Assignment6[(int32_T)((int32_T)(6 * i_0) + i)]; - } - } - - // End of Product: '/Product3' - } - - // Assignment: '/Assignment4' - for (i = 0; i < 96; i++) { - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Assignment[(int32_T)((int32_T)(b_m + (int32_T)(117 * - (int32_T)(21 + i))) + 15)] = rtb_Product3[(int32_T)((int32_T)(6 * i) + - b_m)]; - } - } - - // End of Assignment: '/Assignment4' - - // Assignment: '/Assignment5' incorporates: - // Math: '/Math Function3' - - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 96; b_m++) { - est_estimator_B->Assignment[(int32_T)((int32_T)(b_m + (int32_T)(117 * - (int32_T)(15 + i))) + 21)] = rtb_Product3[(int32_T)((int32_T)(6 * b_m) - + i)]; - } - } - - // End of Assignment: '/Assignment5' - - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_jy[i]; - } - - // End of DataTypeConversion: '/Conversion' - - // Assignment: '/Assignment' - rtb_Assignment_b[0] = rtb_Product1[3]; - rtb_Assignment_b[4] = rtb_Product1[3]; - rtb_Assignment_b[8] = rtb_Product1[3]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_c[0] = (real32_T)est_estimator_P->Constant3_Value_pi; - tmp_c[1] = rtb_Product1[2]; - tmp_c[2] = est_estimator_P->Gain_Gain_j * rtb_Product1[1]; - tmp_c[3] = est_estimator_P->Gain1_Gain_g * rtb_Product1[2]; - tmp_c[4] = (real32_T)est_estimator_P->Constant3_Value_pi; - tmp_c[5] = rtb_Product1[0]; - tmp_c[6] = rtb_Product1[1]; - tmp_c[7] = est_estimator_P->Gain2_Gain_eo * rtb_Product1[0]; - tmp_c[8] = (real32_T)est_estimator_P->Constant3_Value_pi; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i = 0; i < 3; i++) { - rtb_VectorConcatenate_c[(int32_T)(i << 2)] = rtb_Assignment_b[(int32_T)(3 * - i)] + tmp_c[(int32_T)(3 * i)]; - rtb_VectorConcatenate_c[(int32_T)(1 + (int32_T)(i << 2))] = - rtb_Assignment_b[(int32_T)((int32_T)(3 * i) + 1)] + tmp_c[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_VectorConcatenate_c[(int32_T)(2 + (int32_T)(i << 2))] = - rtb_Assignment_b[(int32_T)((int32_T)(3 * i) + 2)] + tmp_c[(int32_T) - ((int32_T)(3 * i) + 2)]; - } - - rtb_VectorConcatenate_c[3] = est_estimator_P->Gain1_Gain_ei * rtb_Product1[0]; - rtb_VectorConcatenate_c[7] = est_estimator_P->Gain1_Gain_ei * rtb_Product1[1]; - rtb_VectorConcatenate_c[11] = est_estimator_P->Gain1_Gain_ei * rtb_Product1 - [2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Reshape: '/Reshape1' - rtb_VectorConcatenate_c[12] = rtb_Product1[0]; - rtb_VectorConcatenate_c[13] = rtb_Product1[1]; - rtb_VectorConcatenate_c[14] = rtb_Product1[2]; - rtb_VectorConcatenate_c[15] = rtb_Product1[3]; - - // BusAssignment: '/Bus Assignment' incorporates: - // Gain: '/Gain' - // Product: '/Product' - // S-Function (sfix_bitop): '/Bitwise Operator2' - // Sum: '/Add' - // Sum: '/Add1' - - rtb_BusAssignment_a = rtb_Merge2; - rtb_BusAssignment_a.aug_state_enum = (uint32_T)(rtb_Merge2.aug_state_enum | - est_estimator_P->BitwiseOperator2_BitMask); - rtb_BusAssignment_a.ml_P_cam_ISS_ISS[0] = (rtb_Merge2.P_EST_ISS_ISS[0] - - est_estimator_P->tun_ase_ml_forward_projection_time * - rtb_Merge2.V_B_ISS_ISS[0]) + rtb_ImpAsg_InsertedFor_Out1_a_d[0]; - rtb_BusAssignment_a.ml_P_cam_ISS_ISS[1] = (rtb_Merge2.P_EST_ISS_ISS[1] - - est_estimator_P->tun_ase_ml_forward_projection_time * - rtb_Merge2.V_B_ISS_ISS[1]) + rtb_ImpAsg_InsertedFor_Out1_a_d[1]; - rtb_BusAssignment_a.ml_P_cam_ISS_ISS[2] = (rtb_Merge2.P_EST_ISS_ISS[2] - - est_estimator_P->tun_ase_ml_forward_projection_time * - rtb_Merge2.V_B_ISS_ISS[2]) + rtb_ImpAsg_InsertedFor_Out1_a_d[2]; - for (i = 0; i < 4; i++) { - rtb_BusAssignment_a.ml_quat_ISS2cam[i] = 0.0F; - rtb_BusAssignment_a.ml_quat_ISS2cam[i] += rtb_VectorConcatenate_c[i] * - rtb_UnitDelay25[0]; - rtb_BusAssignment_a.ml_quat_ISS2cam[i] += rtb_VectorConcatenate_c[(int32_T) - (i + 4)] * rtb_UnitDelay25[1]; - rtb_BusAssignment_a.ml_quat_ISS2cam[i] += rtb_VectorConcatenate_c[(int32_T) - (i + 8)] * rtb_UnitDelay25[2]; - rtb_BusAssignment_a.ml_quat_ISS2cam[i] += rtb_VectorConcatenate_c[(int32_T) - (i + 12)] * rtb_UnitDelay25[3]; - } - - // End of BusAssignment: '/Bus Assignment' - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - ml_vel_aug[0] = (real_T)rtb_Merge2.V_B_ISS_ISS[0]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion2' - - ml_omega_aug[0] = (real_T)rtb_Merge2.omega_B_ISS_B[0]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - ml_vel_aug[1] = (real_T)rtb_Merge2.V_B_ISS_ISS[1]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion2' - - ml_omega_aug[1] = (real_T)rtb_Merge2.omega_B_ISS_B[1]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - - ml_vel_aug[2] = (real_T)rtb_Merge2.V_B_ISS_ISS[2]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // DataTypeConversion: '/Data Type Conversion2' - - ml_omega_aug[2] = (real_T)rtb_Merge2.omega_B_ISS_B[2]; - - // SignalConversion: '/Signal Conversion1' - memcpy(&of_vel_aug[0], &rtb_of_vel_aug[0], (uint32_T)(48U * sizeof(real_T))); - - // SignalConversion: '/Signal Conversion1' - memcpy(&of_omega_aug[0], &rtb_of_omega_aug[0], (uint32_T)(48U * sizeof - (real_T))); - - // End of Outputs for SubSystem: '/If Action Subsystem2' - } else if ((int32_T)((int32_T)rtb_Merge2.kfl_status & (int32_T) - est_estimator_P->BitwiseOperator1_BitMask) != (int32_T) - est_estimator_P->Constant_Value_o) { - // Outputs for IfAction SubSystem: '/If Action Subsystem4' incorporates: - // ActionPort: '/Action Port' - - memcpy(&est_estimator_B->Assignment[0], &est_estimator_B->Switch1[0], - (uint32_T)(13689U * sizeof(real32_T))); - - // SignalConversion: '/Signal Conversion' incorporates: - // Inport: '/P_in' - - rtb_BusAssignment_a = rtb_Merge2; - - // SignalConversion: '/Signal Conversion2' - ml_vel_aug[0] = rtb_ml_vel_aug[0]; - - // SignalConversion: '/Signal Conversion2' - ml_omega_aug[0] = rtb_ml_omega_aug[0]; - - // SignalConversion: '/Signal Conversion2' - ml_vel_aug[1] = rtb_ml_vel_aug[1]; - - // SignalConversion: '/Signal Conversion2' - ml_omega_aug[1] = rtb_ml_omega_aug[1]; - - // SignalConversion: '/Signal Conversion2' - ml_vel_aug[2] = rtb_ml_vel_aug[2]; - - // SignalConversion: '/Signal Conversion2' - ml_omega_aug[2] = rtb_ml_omega_aug[2]; - - // SignalConversion: '/Signal Conversion2' - memcpy(&of_vel_aug[0], &rtb_of_vel_aug[0], (uint32_T)(48U * sizeof(real_T))); - - // SignalConversion: '/Signal Conversion2' - memcpy(&of_omega_aug[0], &rtb_of_omega_aug[0], (uint32_T)(48U * sizeof - (real_T))); - - // End of Outputs for SubSystem: '/If Action Subsystem4' - } else { - // Outputs for IfAction SubSystem: '/If Action Subsystem1' incorporates: - // ActionPort: '/Action Port' - - est_estimato_IfActionSubsystem1(&rtb_Merge2, est_estimator_B->Switch1, - rtb_ml_vel_aug, rtb_ml_omega_aug, rtb_of_vel_aug, rtb_of_omega_aug, - &rtb_BusAssignment_a, est_estimator_B->Assignment, ml_vel_aug, - ml_omega_aug, of_vel_aug, of_omega_aug); - - // End of Outputs for SubSystem: '/If Action Subsystem1' - } - - // End of If: '/If' - - // If: '/If' incorporates: - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - // Inport: '/Vision Registration' - // Logic: '/Logical Operator' - // RelationalOperator: '/Compare' - - if ((est_estimator_U_VisionRegistration->cvs_optical_flow_pulse > - est_estimator_P->CompareToConstant_const) && ((int32_T) - est_estimator_P->tun_ase_enable_of != 0)) { - // Outputs for IfAction SubSystem: '/If Action Subsystem2' incorporates: - // ActionPort: '/Action Port' - - rtb_VectorConcatenate_m[0] = est_estimator_P->Constant3_Value_cc; - - // Gain: '/Gain' incorporates: - // Constant: '/Constant' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[1] = est_estimator_P->Gain_Gain_h * (real32_T) - est_estimator_P->Constant_Value_p[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[2] = (real32_T)est_estimator_P->Constant_Value_p[1]; - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[3] = est_estimator_P->Gain1_Gain_b * (real32_T) - est_estimator_P->Constant_Value_p[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[4] = (real32_T)est_estimator_P->Constant_Value_p[2]; - rtb_VectorConcatenate_m[5] = est_estimator_P->Constant2_Value_pd; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant' - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[6] = est_estimator_P->Gain2_Gain_h * (real32_T) - est_estimator_P->Constant_Value_p[0]; - - // Gain: '/Gain3' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[7] = est_estimator_P->Gain3_Gain * (real32_T) - est_estimator_P->Constant_Value_p[1]; - - // Gain: '/Gain4' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[8] = est_estimator_P->Gain4_Gain * (real32_T) - est_estimator_P->Constant_Value_p[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[9] = (real32_T)est_estimator_P->Constant_Value_p[0]; - rtb_VectorConcatenate_m[10] = est_estimator_P->Constant1_Value_j; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[11] = est_estimator_P->Gain5_Gain * (real32_T) - est_estimator_P->Constant_Value_p[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[12] = (real32_T)est_estimator_P->Constant_Value_p[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[13] = (real32_T)est_estimator_P->Constant_Value_p[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' incorporates: - // Constant: '/Constant' - // DataTypeConversion: '/Conversion' - - rtb_VectorConcatenate_m[14] = (real32_T)est_estimator_P->Constant_Value_p[2]; - rtb_VectorConcatenate_m[15] = est_estimator_P->Constant_Value_f; - rtb_VectorConcatenate_i[0] = est_estimator_P->Constant3_Value_iq; - - // Gain: '/Gain1' incorporates: - // Constant: '/Constant' - // Constant: '/Constant3' - - rtb_Add_e[0] = est_estimator_P->Gain1_Gain_j * - rtb_BusAssignment_a.omega_B_ISS_B[0]; - rtb_Add_e[1] = est_estimator_P->Gain1_Gain_j * - rtb_BusAssignment_a.omega_B_ISS_B[1]; - rtb_Add_e[2] = est_estimator_P->Gain1_Gain_j * - rtb_BusAssignment_a.omega_B_ISS_B[2]; - - // Gain: '/Gain' - rtb_VectorConcatenate_i[1] = est_estimator_P->Gain_Gain_n * rtb_Add_e[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn3' - rtb_VectorConcatenate_i[2] = rtb_Add_e[1]; - - // Gain: '/Gain1' - rtb_VectorConcatenate_i[3] = est_estimator_P->Gain1_Gain_f * rtb_Add_e[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn5' - rtb_VectorConcatenate_i[4] = rtb_Add_e[2]; - rtb_VectorConcatenate_i[5] = est_estimator_P->Constant2_Value_n; - - // Gain: '/Gain2' incorporates: - // Constant: '/Constant2' - - rtb_VectorConcatenate_i[6] = est_estimator_P->Gain2_Gain_a * rtb_Add_e[0]; - - // Gain: '/Gain3' - rtb_VectorConcatenate_i[7] = est_estimator_P->Gain3_Gain_l * rtb_Add_e[1]; - - // Gain: '/Gain4' - rtb_VectorConcatenate_i[8] = est_estimator_P->Gain4_Gain_o * rtb_Add_e[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn10' - rtb_VectorConcatenate_i[9] = rtb_Add_e[0]; - rtb_VectorConcatenate_i[10] = est_estimator_P->Constant1_Value_nk; - - // Gain: '/Gain5' incorporates: - // Constant: '/Constant1' - - rtb_VectorConcatenate_i[11] = est_estimator_P->Gain5_Gain_j * rtb_Add_e[2]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn13' - rtb_VectorConcatenate_i[12] = rtb_Add_e[0]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn14' - rtb_VectorConcatenate_i[13] = rtb_Add_e[1]; - - // SignalConversion: '/ConcatBufferAtVector ConcatenateIn15' - rtb_VectorConcatenate_i[14] = rtb_Add_e[2]; - rtb_VectorConcatenate_i[15] = est_estimator_P->Constant_Value_j; - - // Product: '/Product2' incorporates: - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant3' - // Constant: '/Constant' - // Product: '/Product' - // Sum: '/Add' - - for (i = 0; i < 16; i++) { - rtb_VectorConcatenate_c[i] = (est_estimator_P->Constant3_Value_f * - rtb_VectorConcatenate_m[i] * - est_estimator_P->tun_ase_ml_forward_projection_time + - rtb_VectorConcatenate_i[i]) * est_estimator_P->Constant1_Value_cn * - est_estimator_P->tun_ase_ml_forward_projection_time; - } - - // End of Product: '/Product2' - - // MATLAB Function: '/MATLAB Function' - est_estimator_MATLABFunction(rtb_VectorConcatenate_c, - &est_estimator_B->sf_MATLABFunction_o); - - // Product: '/Product5' incorporates: - // Constant: '/Constant' - - hr_quat_ISS2hr_idx_1 = est_estimator_P->tun_ase_ml_forward_projection_time * - est_estimator_P->tun_ase_ml_forward_projection_time * - est_estimator_P->tun_ase_ml_forward_projection_time; - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m < 4; b_m++) { - // Product: '/Product4' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_c[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - - // Product: '/Product3' incorporates: - // Sum: '/Add2' - - rtb_VectorConcatenate_p[(int32_T)(b_m + (int32_T)(i << 2))] = 0.0F; - rtb_VectorConcatenate_c[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_i[(int32_T)(i << 2)] * - rtb_VectorConcatenate_m[b_m]; - rtb_VectorConcatenate_p[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)(i << 2)] * - rtb_VectorConcatenate_i[b_m]; - rtb_VectorConcatenate_c[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_i[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate_m[(int32_T)(b_m + 4)]; - rtb_VectorConcatenate_p[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)((int32_T)(i << 2) + 1)] * - rtb_VectorConcatenate_i[(int32_T)(b_m + 4)]; - rtb_VectorConcatenate_c[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_i[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate_m[(int32_T)(b_m + 8)]; - rtb_VectorConcatenate_p[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)((int32_T)(i << 2) + 2)] * - rtb_VectorConcatenate_i[(int32_T)(b_m + 8)]; - rtb_VectorConcatenate_c[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_i[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate_m[(int32_T)(b_m + 12)]; - rtb_VectorConcatenate_p[(int32_T)(b_m + (int32_T)(i << 2))] += - rtb_VectorConcatenate_m[(int32_T)((int32_T)(i << 2) + 3)] * - rtb_VectorConcatenate_i[(int32_T)(b_m + 12)]; - } - } - - // Sum: '/Add1' incorporates: - // Constant: '/Constant2' - // Product: '/Product1' - // Product: '/Product3' - // Product: '/Product4' - // Product: '/Product5' - // Sum: '/Add2' - - for (i = 0; i < 4; i++) { - rtb_VectorConcatenate[(int32_T)(i << 2)] = (rtb_VectorConcatenate_c - [(int32_T)(i << 2)] - rtb_VectorConcatenate_p[(int32_T)(i << 2)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_p3 + - est_estimator_B->sf_MATLABFunction_o.y[(int32_T)(i << 2)]; - rtb_VectorConcatenate[(int32_T)(1 + (int32_T)(i << 2))] = - (rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 1)] - - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 1)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_p3 + - est_estimator_B->sf_MATLABFunction_o.y[(int32_T)((int32_T)(i << 2) + 1)]; - rtb_VectorConcatenate[(int32_T)(2 + (int32_T)(i << 2))] = - (rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 2)] - - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 2)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_p3 + - est_estimator_B->sf_MATLABFunction_o.y[(int32_T)((int32_T)(i << 2) + 2)]; - rtb_VectorConcatenate[(int32_T)(3 + (int32_T)(i << 2))] = - (rtb_VectorConcatenate_c[(int32_T)((int32_T)(i << 2) + 3)] - - rtb_VectorConcatenate_p[(int32_T)((int32_T)(i << 2) + 3)]) * - hr_quat_ISS2hr_idx_1 / est_estimator_P->Constant2_Value_p3 + - est_estimator_B->sf_MATLABFunction_o.y[(int32_T)((int32_T)(i << 2) + 3)]; - } - - // End of Sum: '/Add1' - - // Product: '/Product1' - for (i = 0; i < 4; i++) { - rtb_Sum_k1 = rtb_VectorConcatenate[(int32_T)(i + 12)] * - rtb_BusAssignment_a.quat_ISS2B[3] + (rtb_VectorConcatenate[(int32_T)(i + - 8)] * rtb_BusAssignment_a.quat_ISS2B[2] + (rtb_VectorConcatenate - [(int32_T)(i + 4)] * rtb_BusAssignment_a.quat_ISS2B[1] + - rtb_VectorConcatenate[i] * rtb_BusAssignment_a.quat_ISS2B[0])); - rtb_Product1[i] = rtb_Sum_k1; - } - - // If: '/If' incorporates: - // Inport: '/In1' - - if (rtb_Product1[3] < 0.0F) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize(rtb_Product1, rtb_UnitDelay25, - (P_Normalize_est_estimator_T *)&est_estimator_P->Normalize_h); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_UnitDelay25[0] = rtb_Product1[0]; - rtb_UnitDelay25[1] = rtb_Product1[1]; - rtb_UnitDelay25[2] = rtb_Product1[2]; - rtb_UnitDelay25[3] = rtb_Product1[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // DotProduct: '/Dot Product' - - rtb_Switch1 = (real32_T)sqrt((real_T)(((rtb_UnitDelay25[0] * - rtb_UnitDelay25[0] + rtb_UnitDelay25[1] * rtb_UnitDelay25[1]) + - rtb_UnitDelay25[2] * rtb_UnitDelay25[2]) + rtb_UnitDelay25[3] * - rtb_UnitDelay25[3])); - - // If: '/If' incorporates: - // DataTypeConversion: '/Data Type Conversion' - // Inport: '/In1' - - if ((real_T)rtb_Switch1 > 1.0E-7) { - // Outputs for IfAction SubSystem: '/Normalize' incorporates: - // ActionPort: '/Action Port' - - est_estimator_Normalize_p(rtb_UnitDelay25, rtb_Switch1, rtb_Product1); - - // End of Outputs for SubSystem: '/Normalize' - } else { - // Outputs for IfAction SubSystem: '/No-op' incorporates: - // ActionPort: '/Action Port' - - rtb_Product1[0] = rtb_UnitDelay25[0]; - rtb_Product1[1] = rtb_UnitDelay25[1]; - rtb_Product1[2] = rtb_UnitDelay25[2]; - rtb_Product1[3] = rtb_UnitDelay25[3]; - - // End of Outputs for SubSystem: '/No-op' - } - - // End of If: '/If' - - // Sum: '/Sum' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Switch1 = rtb_Product1[3] * rtb_Product1[3] * - est_estimator_P->Gain_Gain_a - (real32_T)est_estimator_P->Constant1_Value; - for (i = 0; i < 9; i++) { - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_j[i]; - - // DataTypeConversion: '/Conversion' incorporates: - // Constant: '/Constant2' - - rtb_Assignment_n[i] = (real32_T)est_estimator_P->Constant2_Value_l[i]; - } - - // Assignment: '/Assignment' - rtb_Assignment_b[0] = rtb_Switch1; - rtb_Assignment_b[4] = rtb_Switch1; - rtb_Assignment_b[8] = rtb_Switch1; - - // Gain: '/Gain1' - rtb_Switch1 = est_estimator_P->Gain1_Gain_e * rtb_Product1[3]; - - // Assignment: '/Assignment' - rtb_Assignment_n[0] = rtb_Product1[3]; - rtb_Assignment_n[4] = rtb_Product1[3]; - rtb_Assignment_n[8] = rtb_Product1[3]; - - // Sum: '/Sum2' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_d[0] = (real32_T)est_estimator_P->Constant3_Value_p; - tmp_d[1] = rtb_Product1[2]; - tmp_d[2] = est_estimator_P->Gain_Gain_g * rtb_Product1[1]; - tmp_d[3] = est_estimator_P->Gain1_Gain_jc * rtb_Product1[2]; - tmp_d[4] = (real32_T)est_estimator_P->Constant3_Value_p; - tmp_d[5] = rtb_Product1[0]; - tmp_d[6] = rtb_Product1[1]; - tmp_d[7] = est_estimator_P->Gain2_Gain_o * rtb_Product1[0]; - tmp_d[8] = (real32_T)est_estimator_P->Constant3_Value_p; - - // Concatenate: '/Matrix Concatenate' incorporates: - // Gain: '/Gain1' - // Sum: '/Sum2' - - for (i = 0; i < 3; i++) { - rtb_VectorConcatenate_m[(int32_T)(i << 2)] = rtb_Assignment_n[(int32_T)(3 * - i)] + tmp_d[(int32_T)(3 * i)]; - rtb_VectorConcatenate_m[(int32_T)(1 + (int32_T)(i << 2))] = - rtb_Assignment_n[(int32_T)((int32_T)(3 * i) + 1)] + tmp_d[(int32_T) - ((int32_T)(3 * i) + 1)]; - rtb_VectorConcatenate_m[(int32_T)(2 + (int32_T)(i << 2))] = - rtb_Assignment_n[(int32_T)((int32_T)(3 * i) + 2)] + tmp_d[(int32_T) - ((int32_T)(3 * i) + 2)]; - } - - rtb_VectorConcatenate_m[3] = est_estimator_P->Gain1_Gain_i * rtb_Product1[0]; - rtb_VectorConcatenate_m[7] = est_estimator_P->Gain1_Gain_i * rtb_Product1[1]; - rtb_VectorConcatenate_m[11] = est_estimator_P->Gain1_Gain_i * rtb_Product1[2]; - - // End of Concatenate: '/Matrix Concatenate' - - // Reshape: '/Reshape1' - rtb_VectorConcatenate_m[12] = rtb_Product1[0]; - rtb_VectorConcatenate_m[13] = rtb_Product1[1]; - rtb_VectorConcatenate_m[14] = rtb_Product1[2]; - rtb_VectorConcatenate_m[15] = rtb_Product1[3]; - - // MATLAB Function: '/MATLAB Function' - // MATLAB Function 'state_manager/Optical Flow Registration Manager/If Action Subsystem2/MATLAB Function': ':1' - // From Zack's matlab filter. This function add loads up the most recent - // camera attitude and position, as well as setting the aug_state_byte - // ':1:5' - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // This function add loads up the most recent - // camera attitude and position, as well as setting the aug_state_byte - // Cast this back to double to not cause issues with other integers - // retain the values between function calls - if ((!est_estimator_DW->aug_velocity_not_empty) || - (!est_estimator_DW->aug_velocity_mag_not_empty) || - (!est_estimator_DW->aug_omega_not_empty) || - (!est_estimator_DW->aug_omega_mag_not_empty)) { - // Initialize values to zero and the same data type as state_in.V_B_ISS_ISS - memset(&est_estimator_DW->aug_velocity[0], 0, (uint32_T)(48U * sizeof - (real32_T))); - est_estimator_DW->aug_velocity_not_empty = true; - memset(&est_estimator_DW->aug_velocity_mag[0], 0, (uint32_T)(sizeof - (real32_T) << 4U)); - est_estimator_DW->aug_velocity_mag_not_empty = true; - memset(&est_estimator_DW->aug_omega[0], 0, (uint32_T)(48U * sizeof - (real32_T))); - est_estimator_DW->aug_omega_not_empty = true; - memset(&est_estimator_DW->aug_omega_mag[0], 0, (uint32_T)(sizeof(real32_T) - << 4U)); - est_estimator_DW->aug_omega_mag_not_empty = true; - } - - memcpy(&est_estimator_B->Switch1[0], &est_estimator_B->Assignment[0], - (uint32_T)(13689U * sizeof(real32_T))); - rtb_Merge2 = rtb_BusAssignment_a; - - // Constants/variables I'll need later - // Augment flag - // [MSB Oldest_OF_Aug ... Newest_OF_Aug Valid_ML_Augment(LSB)] - // Mask off just the OF bits - // Extract out the bits associated with OF - // Shift all the OF bits 1 bit more significant - // Set the newest OF bit as valid - // Mask off the unused bits, remove rollover - rtb_Merge2.aug_state_enum = (uint32_T)((uint32_T)((uint32_T)((uint32_T) - ((uint32_T)(rtb_BusAssignment_a.aug_state_enum & 131070U) << 1U) | 2U) & - 131070U) | (uint32_T)((int32_T)(uint32_T) - (rtb_BusAssignment_a.aug_state_enum & 1U) != 0)); - - // Restore the value of the ML aug bit - // new_jf = bitset(bitshift(bitand(jf, bin2dec('111110')), 1), 2) - // Augment the state vector - // Ive arranged the state vector so it is: - // [IMU location, ML Camera Location, Newest OF Camera Location .... Oldest OF Camera Loc] - UnitDelay_DSTATE_aug_state_enum = (uint32_T)(16U - (uint32_T) - est_estimator_U_VisionRegistration->cvs_optical_flow_pulse); - if (UnitDelay_DSTATE_aug_state_enum > 16U) { - UnitDelay_DSTATE_aug_state_enum = 0U; - } - - numFeatures = (uint8_T)(int32_T)((int32_T)UnitDelay_DSTATE_aug_state_enum - + 1); - - // lowest number replaces most recent - memset(&of_in_prange[0], 0, (uint32_T)(90U * sizeof(real_T))); - num_augs = 1.0; - for (num_original = 0; num_original < 15; num_original++) { - if (num_augs == (real_T)numFeatures) { - num_augs++; - } - - kept_augmentations[num_original] = num_augs; - br = (int32_T)((int32_T)(6 * num_original) + 1); - nx = (int32_T)((int32_T)(1 + num_original) * 6); - if (br > nx) { - br = 1; - nx = 0; - } - - handrail_knowledge_dims = (int32_T)((int32_T)(nx - br) + 1); - C_sizes_idx_1 = (int32_T)(nx - br); - for (i = 0; i <= C_sizes_idx_1; i++) { - f_data[i] = (int8_T)(int32_T)((int32_T)(int8_T)(int32_T)(br + i) - 1); - } - - tmp_9 = (num_augs - 1.0) * 6.0 + 21.0; - for (i = 0; i < 6; i++) { - tmp_g[i] = (int8_T)(int32_T)(1 + i); - } - - for (i = 0; i <= (int32_T)(handrail_knowledge_dims - 1); i++) { - of_in_prange[(int32_T)f_data[i]] = tmp_9 + (real_T)tmp_g[i]; - } - - num_augs++; - } - - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_Merge2_0[(int32_T)(b_m + (int32_T)(15 * i))] = - rtb_Merge2.of_quat_ISS2cam[(int32_T)((int32_T)((int32_T)(i << 4) + - (int32_T)kept_augmentations[b_m]) - 1)]; - } - } - - for (i = 0; i < 4; i++) { - for (b_m = 0; b_m < 15; b_m++) { - rtb_Merge2.of_quat_ISS2cam[(int32_T)((int32_T)(b_m + (int32_T)(i << 4)) - + 1)] = rtb_Merge2_0[(int32_T)((int32_T)(15 * i) + b_m)]; - } - - // DataTypeConversion: '/Data Type Conversion1' incorporates: - // Constant: '/Constant2' - // Product: '/Product' - - rtb_VectorConcatenate_o[i] = (real_T)(((rtb_VectorConcatenate_m[(int32_T) - (i + 4)] * est_estimator_P->tun_abp_q_body2navcam[1] + - rtb_VectorConcatenate_m[i] * est_estimator_P->tun_abp_q_body2navcam[0]) - + rtb_VectorConcatenate_m[(int32_T)(i + 8)] * - est_estimator_P->tun_abp_q_body2navcam[2]) + rtb_VectorConcatenate_m - [(int32_T)(i + 12)] * est_estimator_P->tun_abp_q_body2navcam[3]); - rtb_Merge2.of_quat_ISS2cam[(int32_T)(i << 4)] = (real32_T) - rtb_VectorConcatenate_o[i]; - } - - // Product: '/Product' incorporates: - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_e[0] = (real32_T)est_estimator_P->Constant3_Value_l; - tmp_e[1] = rtb_Product1[2]; - tmp_e[2] = est_estimator_P->Gain_Gain_f * rtb_Product1[1]; - tmp_e[3] = est_estimator_P->Gain1_Gain_k * rtb_Product1[2]; - tmp_e[4] = (real32_T)est_estimator_P->Constant3_Value_l; - tmp_e[5] = rtb_Product1[0]; - tmp_e[6] = rtb_Product1[1]; - tmp_e[7] = est_estimator_P->Gain2_Gain_i * rtb_Product1[0]; - tmp_e[8] = (real32_T)est_estimator_P->Constant3_Value_l; - - // Math: '/Math Function' incorporates: - // Gain: '/Gain2' - // Math: '/Math Function1' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Product1_0[i] = rtb_Product1[0] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 3)] = rtb_Product1[1] * rtb_Product1[i]; - rtb_Product1_0[(int32_T)(i + 6)] = rtb_Product1[2] * rtb_Product1[i]; - } - - // End of Math: '/Math Function' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Assignment_n[(int32_T)(3 * i)] = (rtb_Assignment_b[i] - rtb_Switch1 * - tmp_e[i]) + rtb_Product1_0[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_p; - rtb_Assignment_n[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 3)] - tmp_e[(int32_T)(i + 3)] * rtb_Switch1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 1)] * - est_estimator_P->Gain2_Gain_p; - rtb_Assignment_n[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 6)] - tmp_e[(int32_T)(i + 6)] * rtb_Switch1) + - rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->Gain2_Gain_p; - } - - // End of Sum: '/Sum1' - for (i = 0; i < 3; i++) { - // MATLAB Function: '/MATLAB Function' incorporates: - // Constant: '/Constant1' - // DataTypeConversion: '/Data Type Conversion2' - // Gain: '/Gain' - // Product: '/Product' - // Sum: '/Add' - // Sum: '/Add1' - - UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)(i << 4)] = ((rtb_Assignment_n - [(int32_T)(i + 3)] * est_estimator_P->tun_abp_p_navcam_imu_est[1] + - rtb_Assignment_n[i] * est_estimator_P->tun_abp_p_navcam_imu_est[0]) + - rtb_Assignment_n[(int32_T)(i + 6)] * - est_estimator_P->tun_abp_p_navcam_imu_est[2]) + - (rtb_BusAssignment_a.P_EST_ISS_ISS[i] - - est_estimator_P->tun_ase_ml_forward_projection_time * - rtb_BusAssignment_a.V_B_ISS_ISS[i]); - for (b_m = 0; b_m < 15; b_m++) { - UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)((int32_T)(b_m + (int32_T)(i << - 4)) + 1)] = rtb_Merge2.of_P_cam_ISS_ISS[(int32_T)((int32_T)((int32_T) - (i << 4) + (int32_T)kept_augmentations[b_m]) - 1)]; - } - } - - // MATLAB Function: '/MATLAB Function' - // Augment velocities - for (i = 0; i < 3; i++) { - memcpy(&rtb_Merge2.of_P_cam_ISS_ISS[(int32_T)(i << 4)], - &UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)(i << 4)], (uint32_T) - (sizeof(real32_T) << 4U)); - rtb_BusAssignment_h[(int32_T)(i << 4)] = rtb_BusAssignment_a.V_B_ISS_ISS[i]; - for (b_m = 0; b_m < 15; b_m++) { - rtb_BusAssignment_h[(int32_T)((int32_T)(b_m + (int32_T)(i << 4)) + 1)] = - est_estimator_DW->aug_velocity[(int32_T)((int32_T)((int32_T)(i << 4) + - (int32_T)kept_augmentations[b_m]) - 1)]; - } - } - - for (i = 0; i < 3; i++) { - memcpy(&est_estimator_DW->aug_velocity[(int32_T)(i << 4)], - &rtb_BusAssignment_h[(int32_T)(i << 4)], (uint32_T)(sizeof(real32_T) - << 4U)); - } - - rtb_VectorConcatenate[0] = aimgbiekknohhdjm_norm - (rtb_BusAssignment_a.V_B_ISS_ISS); - for (i = 0; i < 15; i++) { - rtb_VectorConcatenate[(int32_T)(i + 1)] = - est_estimator_DW->aug_velocity_mag[(int32_T)((int32_T) - kept_augmentations[i] - 1)]; - } - - memcpy(&est_estimator_DW->aug_velocity_mag[0], &rtb_VectorConcatenate[0], - (uint32_T)(sizeof(real32_T) << 4U)); - for (i = 0; i < 3; i++) { - UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)(i << 4)] = - rtb_BusAssignment_a.omega_B_ISS_B[i]; - for (b_m = 0; b_m < 15; b_m++) { - UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)((int32_T)(b_m + (int32_T)(i << - 4)) + 1)] = est_estimator_DW->aug_omega[(int32_T)((int32_T)((int32_T) - (i << 4) + (int32_T)kept_augmentations[b_m]) - 1)]; - } - } - - for (i = 0; i < 3; i++) { - memcpy(&est_estimator_DW->aug_omega[(int32_T)(i << 4)], - &UnitDelay_DSTATE_n_of_P_cam_ISS[(int32_T)(i << 4)], (uint32_T) - (sizeof(real32_T) << 4U)); - } - - rtb_VectorConcatenate[0] = aimgbiekknohhdjm_norm - (rtb_BusAssignment_a.omega_B_ISS_B); - for (i = 0; i < 15; i++) { - rtb_VectorConcatenate[(int32_T)(i + 1)] = est_estimator_DW->aug_omega_mag - [(int32_T)((int32_T)kept_augmentations[i] - 1)]; - } - - memcpy(&est_estimator_DW->aug_omega_mag[0], &rtb_VectorConcatenate[0], - (uint32_T)(sizeof(real32_T) << 4U)); - - // Augmenting the covariance matrix - // Move covariances down the stack: - // P = [A B C] then P+1 = [A 0 B] - // [D E F] [0 0 0] - // [G H J] [D 0 E] - // Section D - for (i = 0; i < 21; i++) { - for (b_m = 0; b_m < 90; b_m++) { - est_estimator_B->Switch1[(int32_T)((int32_T)(b_m + (int32_T)(117 * i)) + - 27)] = est_estimator_B->Assignment[(int32_T)((int32_T)((int32_T)(117 * - i) + (int32_T)of_in_prange[b_m]) - 1)]; - } - } - - // Section B - for (i = 0; i < 90; i++) { - for (b_m = 0; b_m < 21; b_m++) { - est_estimator_B->Switch1_c[(int32_T)(b_m + (int32_T)(21 * i))] = - est_estimator_B->Switch1[(int32_T)((int32_T)((int32_T)((int32_T) - of_in_prange[i] - 1) * 117) + b_m)]; - } - } - - for (i = 0; i < 90; i++) { - memcpy(&est_estimator_B->Switch1[(int32_T)((int32_T)(i * 117) + 3159)], - &est_estimator_B->Switch1_c[(int32_T)(i * 21)], (uint32_T)(21U * - sizeof(real32_T))); - } - - // Section E - for (i = 0; i < 90; i++) { - for (b_m = 0; b_m < 90; b_m++) { - est_estimator_B->Switch1_k[(int32_T)(b_m + (int32_T)(90 * i))] = - est_estimator_B->Switch1[(int32_T)((int32_T)((int32_T)((int32_T) - ((int32_T)of_in_prange[i] - 1) * 117) + (int32_T)of_in_prange[b_m]) - - 1)]; - } - } - - for (i = 0; i < 90; i++) { - memcpy(&est_estimator_B->Switch1[(int32_T)((int32_T)(i * 117) + 3186)], - &est_estimator_B->Switch1_k[(int32_T)(i * 90)], (uint32_T)(90U * - sizeof(real32_T))); - } - - // Now actually place an augmentation of IMU into OF - // M here is actually just the left part of J as defined in equation 24 in the - // Visinav paper by Mourikis '09 + space for the ML augmentation. - fkfcbaiengdjgdje_quaternion_to_rotation(rtb_BusAssignment_a.quat_ISS2B, - rtb_Product1_0); - - // Copyright (c) 2017, United States Government, as represented by the - // Administrator of the National Aeronautics and Space Administration. - // - // All rights reserved. - // - // The Astrobee platform is licensed under the Apache License, Version 2.0 - // (the "License"); you may not use this file except in compliance with the - // License. You may obtain a copy of the License at - // - // http://www.apache.org/licenses/LICENSE-2.0 - // - // Unless required by applicable law or agreed to in writing, software - // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - // License for the specific language governing permissions and limitations - // under the License. - // construct swew matrix from a vector - // From Zack and Brian's ekf.m Used as a nested function in the optical - // flow update - fkfcbaiengdjgdje_quaternion_to_rotation - (est_estimator_P->tun_abp_q_body2navcam, rtb_Assignment_o); - for (i = 0; i < 3; i++) { - M[(int32_T)(6 * i)] = rtb_Assignment_o[(int32_T)(3 * i)]; - M[(int32_T)(1 + (int32_T)(6 * i))] = rtb_Assignment_o[(int32_T)((int32_T) - (3 * i) + 1)]; - M[(int32_T)(2 + (int32_T)(6 * i))] = rtb_Assignment_o[(int32_T)((int32_T) - (3 * i) + 2)]; - rtb_Add_e[i] = rtb_Product1_0[(int32_T)((int32_T)(3 * i) + 2)] * - est_estimator_P->tun_abp_p_navcam_imu_est[2] + (rtb_Product1_0[(int32_T) - ((int32_T)(3 * i) + 1)] * est_estimator_P->tun_abp_p_navcam_imu_est[1] + - rtb_Product1_0[(int32_T)(3 * i)] * - est_estimator_P->tun_abp_p_navcam_imu_est[0]); - } - - for (i = 0; i < 12; i++) { - M[(int32_T)(6 * (int32_T)(i + 3))] = 0.0F; - M[(int32_T)(1 + (int32_T)(6 * (int32_T)(i + 3)))] = 0.0F; - M[(int32_T)(2 + (int32_T)(6 * (int32_T)(i + 3)))] = 0.0F; - } - - for (i = 0; i < 6; i++) { - M[(int32_T)(6 * (int32_T)(i + 15))] = 0.0F; - M[(int32_T)(1 + (int32_T)(6 * (int32_T)(i + 15)))] = 0.0F; - M[(int32_T)(2 + (int32_T)(6 * (int32_T)(i + 15)))] = 0.0F; - } - - M[3] = 0.0F; - M[9] = -rtb_Add_e[2]; - M[15] = rtb_Add_e[1]; - M[4] = rtb_Add_e[2]; - M[10] = 0.0F; - M[16] = -rtb_Add_e[0]; - M[5] = -rtb_Add_e[1]; - M[11] = rtb_Add_e[0]; - M[17] = 0.0F; - for (i = 0; i < 9; i++) { - M[(int32_T)(3 + (int32_T)(6 * (int32_T)(i + 3)))] = 0.0F; - M[(int32_T)(4 + (int32_T)(6 * (int32_T)(i + 3)))] = 0.0F; - M[(int32_T)(5 + (int32_T)(6 * (int32_T)(i + 3)))] = 0.0F; - } - - for (i = 0; i < 3; i++) { - M[(int32_T)(3 + (int32_T)(6 * (int32_T)(i + 12)))] = (real32_T)g[(int32_T) - (3 * i)]; - M[(int32_T)(4 + (int32_T)(6 * (int32_T)(i + 12)))] = (real32_T)g[(int32_T) - ((int32_T)(3 * i) + 1)]; - M[(int32_T)(5 + (int32_T)(6 * (int32_T)(i + 12)))] = (real32_T)g[(int32_T) - ((int32_T)(3 * i) + 2)]; - } - - for (i = 0; i < 6; i++) { - M[(int32_T)(3 + (int32_T)(6 * (int32_T)(i + 15)))] = 0.0F; - M[(int32_T)(4 + (int32_T)(6 * (int32_T)(i + 15)))] = 0.0F; - M[(int32_T)(5 + (int32_T)(6 * (int32_T)(i + 15)))] = 0.0F; - } - - // J = [eye(21, size(P, 2)); M zeros(6, size(P, 2) - size(M, 2)); zeros(size(P, 1) - 27, 27) eye(size(P, 1) - 27, size(P, 2) - 27)]; - // P = J * P * J'; - // P = [A 0 C] then P+1 = [A AMt C ] - // [0 0 0] [MA MAMt MC] - // [G 0 J] [G GMt J ] - // Center section - // Top left sections - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 21; b_m++) { - M_0[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 21; i_0++) { - M_0[(int32_T)(i + (int32_T)(6 * b_m))] += M[(int32_T)((int32_T)(6 * - i_0) + i)] * est_estimator_B->Switch1[(int32_T)((int32_T)(117 * b_m) - + i_0)]; - } - } - - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Switch1[(int32_T)((int32_T)(i + (int32_T)(117 * - (int32_T)(21 + b_m))) + 21)] = 0.0F; - for (i_0 = 0; i_0 < 21; i_0++) { - est_estimator_B->Switch1[(int32_T)((int32_T)(i + (int32_T)(117 * - (int32_T)(21 + b_m))) + 21)] = est_estimator_B->Switch1[(int32_T) - ((int32_T)((int32_T)((int32_T)(21 + b_m) * 117) + i) + 21)] + M_0 - [(int32_T)((int32_T)(6 * i_0) + i)] * M[(int32_T)((int32_T)(6 * i_0) - + b_m)]; - } - } - - for (b_m = 0; b_m < 21; b_m++) { - M_1[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 21; i_0++) { - M_1[(int32_T)(i + (int32_T)(6 * b_m))] += M[(int32_T)((int32_T)(6 * - i_0) + i)] * est_estimator_B->Switch1[(int32_T)((int32_T)(117 * b_m) - + i_0)]; - } - } - } - - for (i = 0; i < 21; i++) { - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Switch1[(int32_T)((int32_T)(b_m + (int32_T)(117 * i)) + - 21)] = M_1[(int32_T)((int32_T)(6 * i) + b_m)]; - } - } - - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 21; b_m++) { - M_0[(int32_T)(b_m + (int32_T)(21 * i))] = est_estimator_B->Switch1 - [(int32_T)((int32_T)((int32_T)(117 * b_m) + i) + 21)]; - } - } - - for (i = 0; i < 6; i++) { - memcpy(&est_estimator_B->Switch1[(int32_T)((int32_T)(i * 117) + 2457)], - &M_0[(int32_T)(i * 21)], (uint32_T)(21U * sizeof(real32_T))); - } - - // Bottom right sections - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 90; b_m++) { - M_2[(int32_T)(i + (int32_T)(6 * b_m))] = 0.0F; - for (i_0 = 0; i_0 < 21; i_0++) { - M_2[(int32_T)(i + (int32_T)(6 * b_m))] += est_estimator_B->Switch1 - [(int32_T)((int32_T)((int32_T)(27 + b_m) * 117) + i_0)] * M[(int32_T) - ((int32_T)(6 * i_0) + i)]; - } - } - } - - for (i = 0; i < 90; i++) { - for (b_m = 0; b_m < 6; b_m++) { - est_estimator_B->Switch1[(int32_T)((int32_T)(b_m + (int32_T)(117 * - (int32_T)(27 + i))) + 21)] = M_2[(int32_T)((int32_T)(6 * i) + b_m)]; - } - } - - for (i = 0; i < 6; i++) { - for (b_m = 0; b_m < 90; b_m++) { - M_2[(int32_T)(b_m + (int32_T)(90 * i))] = est_estimator_B->Switch1 - [(int32_T)((int32_T)((int32_T)((int32_T)(27 + b_m) * 117) + i) + 21)]; - } - } - - for (i = 0; i < 6; i++) { - memcpy(&est_estimator_B->Switch1[(int32_T)((int32_T)(i * 117) + 2484)], - &M_2[(int32_T)(i * 90)], (uint32_T)(90U * sizeof(real32_T))); - } - - // SignalConversion: '/Signal Conversion1' - // ':1:5' - rtb_ml_vel_aug[0] = ml_vel_aug[0]; - - // SignalConversion: '/Signal Conversion1' - rtb_ml_omega_aug[0] = ml_omega_aug[0]; - - // SignalConversion: '/Signal Conversion1' - rtb_ml_vel_aug[1] = ml_vel_aug[1]; - - // SignalConversion: '/Signal Conversion1' - rtb_ml_omega_aug[1] = ml_omega_aug[1]; - - // SignalConversion: '/Signal Conversion1' - rtb_ml_vel_aug[2] = ml_vel_aug[2]; - - // SignalConversion: '/Signal Conversion1' - rtb_ml_omega_aug[2] = ml_omega_aug[2]; - for (i = 0; i < 48; i++) { - // SignalConversion: '/Signal Conversion1' incorporates: - // MATLAB Function: '/MATLAB Function' - - rtb_of_vel_aug[i] = (real_T)est_estimator_DW->aug_velocity[i]; - - // SignalConversion: '/Signal Conversion1' incorporates: - // MATLAB Function: '/MATLAB Function' - - rtb_of_omega_aug[i] = (real_T)est_estimator_DW->aug_omega[i]; - } - - // End of Outputs for SubSystem: '/If Action Subsystem2' - } else { - // Outputs for IfAction SubSystem: '/If Action Subsystem1' incorporates: - // ActionPort: '/Action Port' - - est_estimato_IfActionSubsystem1(&rtb_BusAssignment_a, - est_estimator_B->Assignment, ml_vel_aug, ml_omega_aug, of_vel_aug, - of_omega_aug, &rtb_Merge2, est_estimator_B->Switch1, rtb_ml_vel_aug, - rtb_ml_omega_aug, rtb_of_vel_aug, rtb_of_omega_aug); - - // End of Outputs for SubSystem: '/If Action Subsystem1' - } - - // End of If: '/If' - - // Sqrt: '/Sqrt' incorporates: - // Math: '/Math Function' - // SignalConversion: '/TmpSignal ConversionAtMath FunctionInport1' - // Sum: '/Sum of Elements' - - rtb_Switch1 = (real32_T)sqrt((real_T)((est_estimator_B->Switch1[1416] * - est_estimator_B->Switch1[1416] + est_estimator_B->Switch1[1534] * - est_estimator_B->Switch1[1534]) + est_estimator_B->Switch1[1652] * - est_estimator_B->Switch1[1652])); - - // Switch: '/Switch2' incorporates: - // Constant: '/Constant2' - // Constant: '/Constant3' - // S-Function (sfix_bitop): '/Bitwise Operator' - // Sum: '/Sum' - // UnitDelay: '/Unit Delay' - - if ((int32_T)((int32_T)rtb_Merge2.kfl_status & (int32_T) - est_estimator_P->BitwiseOperator_BitMask_l) != 0) { - num_augs = est_estimator_P->Constant2_Value_h; - } else { - num_augs = est_estimator_DW->UnitDelay_DSTATE_j + - est_estimator_P->Constant3_Value_p3; - } - - // End of Switch: '/Switch2' - - // Saturate: '/Saturation' - if (num_augs > (real_T)est_estimator_P->tun_ase_acquired_ticks) { - num_augs = (real_T)est_estimator_P->tun_ase_acquired_ticks; - } else { - if (num_augs < est_estimator_P->Saturation_LowerSat) { - num_augs = est_estimator_P->Saturation_LowerSat; - } - } - - // End of Saturate: '/Saturation' - - // RelationalOperator: '/Compare' incorporates: - // Constant: '/Constant' - - rtb_Compare = (num_augs >= (real_T)est_estimator_P->tun_ase_acquired_ticks); - - // Switch: '/Switch1' incorporates: - // Constant: '/Constant1' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Constant: '/Constant' - // Logic: '/Logical Operator' - // Logic: '/Logical Operator1' - // Logic: '/Logical Operator2' - // Logic: '/Logical Operator3' - // Logic: '/Logical Operator4' - // Logic: '/Logical Operator5' - // Logic: '/Logical Operator6' - // Logic: '/Logical Operator7' - // Logic: '/Logical Operator8' - // RelationalOperator: '/Relational Operator' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // RelationalOperator: '/Compare' - // Switch: '/Switch' - // Switch: '/Switch3' - - if ((rtb_Switch1 < est_estimator_P->tun_ase_converged_thresh) && - ((rtb_Merge2.confidence == est_estimator_P->ase_status_diverged) || - ((rtb_Merge2.confidence == est_estimator_P->ase_status_acquiring) && - (!rtb_Compare)))) { - rtb_Switch_m = est_estimator_P->ase_status_converged; - } else if (((rtb_Switch1 > est_estimator_P->tun_ase_diverged_thresh) || - (!((!rtIsNaNF(rtb_Switch1)) && (!rtIsInfF(rtb_Switch1))))) && - ((rtb_Merge2.confidence == est_estimator_P->ase_status_converged) || - (rtb_Merge2.confidence == est_estimator_P->ase_status_acquiring))) - { - // Switch: '/Switch' incorporates: - // Constant: '/Constant' - - rtb_Switch_m = est_estimator_P->ase_status_diverged; - } else if ((rtb_Merge2.confidence == est_estimator_P->ase_status_converged) && - rtb_Compare) { - // Switch: '/Switch3' incorporates: - // Constant: '/Constant4' - // Switch: '/Switch' - - rtb_Switch_m = est_estimator_P->ase_status_acquiring; - } else { - // Switch: '/Switch' incorporates: - // Switch: '/Switch3' - - rtb_Switch_m = rtb_Merge2.confidence; - } - - // End of Switch: '/Switch1' - - // BusAssignment: '/Bus Assignment' - UnitDelay_DSTATE_n_cov_diag[0] = est_estimator_B->Switch1[0]; - UnitDelay_DSTATE_n_cov_diag[1] = est_estimator_B->Switch1[118]; - UnitDelay_DSTATE_n_cov_diag[2] = est_estimator_B->Switch1[236]; - UnitDelay_DSTATE_n_cov_diag[3] = est_estimator_B->Switch1[354]; - UnitDelay_DSTATE_n_cov_diag[4] = est_estimator_B->Switch1[472]; - UnitDelay_DSTATE_n_cov_diag[5] = est_estimator_B->Switch1[590]; - UnitDelay_DSTATE_n_cov_diag[6] = est_estimator_B->Switch1[708]; - UnitDelay_DSTATE_n_cov_diag[7] = est_estimator_B->Switch1[826]; - UnitDelay_DSTATE_n_cov_diag[8] = est_estimator_B->Switch1[944]; - UnitDelay_DSTATE_n_cov_diag[9] = est_estimator_B->Switch1[1062]; - UnitDelay_DSTATE_n_cov_diag[10] = est_estimator_B->Switch1[1180]; - UnitDelay_DSTATE_n_cov_diag[11] = est_estimator_B->Switch1[1298]; - UnitDelay_DSTATE_n_cov_diag[12] = est_estimator_B->Switch1[1416]; - UnitDelay_DSTATE_n_cov_diag[13] = est_estimator_B->Switch1[1534]; - UnitDelay_DSTATE_n_cov_diag[14] = est_estimator_B->Switch1[1652]; - UnitDelay_DSTATE_n_cov_diag[15] = est_estimator_B->Switch1[1770]; - UnitDelay_DSTATE_n_cov_diag[16] = est_estimator_B->Switch1[1888]; - UnitDelay_DSTATE_n_cov_diag[17] = est_estimator_B->Switch1[2006]; - UnitDelay_DSTATE_n_cov_diag[18] = est_estimator_B->Switch1[2124]; - UnitDelay_DSTATE_n_cov_diag[19] = est_estimator_B->Switch1[2242]; - UnitDelay_DSTATE_n_cov_diag[20] = est_estimator_B->Switch1[2360]; - UnitDelay_DSTATE_n_cov_diag[21] = est_estimator_B->Switch1[2478]; - UnitDelay_DSTATE_n_cov_diag[22] = est_estimator_B->Switch1[2596]; - UnitDelay_DSTATE_n_cov_diag[23] = est_estimator_B->Switch1[2714]; - UnitDelay_DSTATE_n_cov_diag[24] = est_estimator_B->Switch1[2832]; - UnitDelay_DSTATE_n_cov_diag[25] = est_estimator_B->Switch1[2950]; - UnitDelay_DSTATE_n_cov_diag[26] = est_estimator_B->Switch1[3068]; - UnitDelay_DSTATE_n_cov_diag[27] = est_estimator_B->Switch1[3186]; - UnitDelay_DSTATE_n_cov_diag[28] = est_estimator_B->Switch1[3304]; - UnitDelay_DSTATE_n_cov_diag[29] = est_estimator_B->Switch1[3422]; - UnitDelay_DSTATE_n_cov_diag[30] = est_estimator_B->Switch1[3540]; - UnitDelay_DSTATE_n_cov_diag[31] = est_estimator_B->Switch1[3658]; - UnitDelay_DSTATE_n_cov_diag[32] = est_estimator_B->Switch1[3776]; - UnitDelay_DSTATE_n_cov_diag[33] = est_estimator_B->Switch1[3894]; - UnitDelay_DSTATE_n_cov_diag[34] = est_estimator_B->Switch1[4012]; - UnitDelay_DSTATE_n_cov_diag[35] = est_estimator_B->Switch1[4130]; - UnitDelay_DSTATE_n_cov_diag[36] = est_estimator_B->Switch1[4248]; - UnitDelay_DSTATE_n_cov_diag[37] = est_estimator_B->Switch1[4366]; - UnitDelay_DSTATE_n_cov_diag[38] = est_estimator_B->Switch1[4484]; - UnitDelay_DSTATE_n_cov_diag[39] = est_estimator_B->Switch1[4602]; - UnitDelay_DSTATE_n_cov_diag[40] = est_estimator_B->Switch1[4720]; - UnitDelay_DSTATE_n_cov_diag[41] = est_estimator_B->Switch1[4838]; - UnitDelay_DSTATE_n_cov_diag[42] = est_estimator_B->Switch1[4956]; - UnitDelay_DSTATE_n_cov_diag[43] = est_estimator_B->Switch1[5074]; - UnitDelay_DSTATE_n_cov_diag[44] = est_estimator_B->Switch1[5192]; - UnitDelay_DSTATE_n_cov_diag[45] = est_estimator_B->Switch1[5310]; - UnitDelay_DSTATE_n_cov_diag[46] = est_estimator_B->Switch1[5428]; - UnitDelay_DSTATE_n_cov_diag[47] = est_estimator_B->Switch1[5546]; - UnitDelay_DSTATE_n_cov_diag[48] = est_estimator_B->Switch1[5664]; - UnitDelay_DSTATE_n_cov_diag[49] = est_estimator_B->Switch1[5782]; - UnitDelay_DSTATE_n_cov_diag[50] = est_estimator_B->Switch1[5900]; - UnitDelay_DSTATE_n_cov_diag[51] = est_estimator_B->Switch1[6018]; - UnitDelay_DSTATE_n_cov_diag[52] = est_estimator_B->Switch1[6136]; - UnitDelay_DSTATE_n_cov_diag[53] = est_estimator_B->Switch1[6254]; - UnitDelay_DSTATE_n_cov_diag[54] = est_estimator_B->Switch1[6372]; - UnitDelay_DSTATE_n_cov_diag[55] = est_estimator_B->Switch1[6490]; - UnitDelay_DSTATE_n_cov_diag[56] = est_estimator_B->Switch1[6608]; - UnitDelay_DSTATE_n_cov_diag[57] = est_estimator_B->Switch1[6726]; - UnitDelay_DSTATE_n_cov_diag[58] = est_estimator_B->Switch1[6844]; - UnitDelay_DSTATE_n_cov_diag[59] = est_estimator_B->Switch1[6962]; - UnitDelay_DSTATE_n_cov_diag[60] = est_estimator_B->Switch1[7080]; - UnitDelay_DSTATE_n_cov_diag[61] = est_estimator_B->Switch1[7198]; - UnitDelay_DSTATE_n_cov_diag[62] = est_estimator_B->Switch1[7316]; - UnitDelay_DSTATE_n_cov_diag[63] = est_estimator_B->Switch1[7434]; - UnitDelay_DSTATE_n_cov_diag[64] = est_estimator_B->Switch1[7552]; - UnitDelay_DSTATE_n_cov_diag[65] = est_estimator_B->Switch1[7670]; - UnitDelay_DSTATE_n_cov_diag[66] = est_estimator_B->Switch1[7788]; - UnitDelay_DSTATE_n_cov_diag[67] = est_estimator_B->Switch1[7906]; - UnitDelay_DSTATE_n_cov_diag[68] = est_estimator_B->Switch1[8024]; - UnitDelay_DSTATE_n_cov_diag[69] = est_estimator_B->Switch1[8142]; - UnitDelay_DSTATE_n_cov_diag[70] = est_estimator_B->Switch1[8260]; - UnitDelay_DSTATE_n_cov_diag[71] = est_estimator_B->Switch1[8378]; - UnitDelay_DSTATE_n_cov_diag[72] = est_estimator_B->Switch1[8496]; - UnitDelay_DSTATE_n_cov_diag[73] = est_estimator_B->Switch1[8614]; - UnitDelay_DSTATE_n_cov_diag[74] = est_estimator_B->Switch1[8732]; - UnitDelay_DSTATE_n_cov_diag[75] = est_estimator_B->Switch1[8850]; - UnitDelay_DSTATE_n_cov_diag[76] = est_estimator_B->Switch1[8968]; - UnitDelay_DSTATE_n_cov_diag[77] = est_estimator_B->Switch1[9086]; - UnitDelay_DSTATE_n_cov_diag[78] = est_estimator_B->Switch1[9204]; - UnitDelay_DSTATE_n_cov_diag[79] = est_estimator_B->Switch1[9322]; - UnitDelay_DSTATE_n_cov_diag[80] = est_estimator_B->Switch1[9440]; - UnitDelay_DSTATE_n_cov_diag[81] = est_estimator_B->Switch1[9558]; - UnitDelay_DSTATE_n_cov_diag[82] = est_estimator_B->Switch1[9676]; - UnitDelay_DSTATE_n_cov_diag[83] = est_estimator_B->Switch1[9794]; - UnitDelay_DSTATE_n_cov_diag[84] = est_estimator_B->Switch1[9912]; - UnitDelay_DSTATE_n_cov_diag[85] = est_estimator_B->Switch1[10030]; - UnitDelay_DSTATE_n_cov_diag[86] = est_estimator_B->Switch1[10148]; - UnitDelay_DSTATE_n_cov_diag[87] = est_estimator_B->Switch1[10266]; - UnitDelay_DSTATE_n_cov_diag[88] = est_estimator_B->Switch1[10384]; - UnitDelay_DSTATE_n_cov_diag[89] = est_estimator_B->Switch1[10502]; - UnitDelay_DSTATE_n_cov_diag[90] = est_estimator_B->Switch1[10620]; - UnitDelay_DSTATE_n_cov_diag[91] = est_estimator_B->Switch1[10738]; - UnitDelay_DSTATE_n_cov_diag[92] = est_estimator_B->Switch1[10856]; - UnitDelay_DSTATE_n_cov_diag[93] = est_estimator_B->Switch1[10974]; - UnitDelay_DSTATE_n_cov_diag[94] = est_estimator_B->Switch1[11092]; - UnitDelay_DSTATE_n_cov_diag[95] = est_estimator_B->Switch1[11210]; - UnitDelay_DSTATE_n_cov_diag[96] = est_estimator_B->Switch1[11328]; - UnitDelay_DSTATE_n_cov_diag[97] = est_estimator_B->Switch1[11446]; - UnitDelay_DSTATE_n_cov_diag[98] = est_estimator_B->Switch1[11564]; - UnitDelay_DSTATE_n_cov_diag[99] = est_estimator_B->Switch1[11682]; - UnitDelay_DSTATE_n_cov_diag[100] = est_estimator_B->Switch1[11800]; - UnitDelay_DSTATE_n_cov_diag[101] = est_estimator_B->Switch1[11918]; - UnitDelay_DSTATE_n_cov_diag[102] = est_estimator_B->Switch1[12036]; - UnitDelay_DSTATE_n_cov_diag[103] = est_estimator_B->Switch1[12154]; - UnitDelay_DSTATE_n_cov_diag[104] = est_estimator_B->Switch1[12272]; - UnitDelay_DSTATE_n_cov_diag[105] = est_estimator_B->Switch1[12390]; - UnitDelay_DSTATE_n_cov_diag[106] = est_estimator_B->Switch1[12508]; - UnitDelay_DSTATE_n_cov_diag[107] = est_estimator_B->Switch1[12626]; - UnitDelay_DSTATE_n_cov_diag[108] = est_estimator_B->Switch1[12744]; - UnitDelay_DSTATE_n_cov_diag[109] = est_estimator_B->Switch1[12862]; - UnitDelay_DSTATE_n_cov_diag[110] = est_estimator_B->Switch1[12980]; - UnitDelay_DSTATE_n_cov_diag[111] = est_estimator_B->Switch1[13098]; - UnitDelay_DSTATE_n_cov_diag[112] = est_estimator_B->Switch1[13216]; - UnitDelay_DSTATE_n_cov_diag[113] = est_estimator_B->Switch1[13334]; - UnitDelay_DSTATE_n_cov_diag[114] = est_estimator_B->Switch1[13452]; - UnitDelay_DSTATE_n_cov_diag[115] = est_estimator_B->Switch1[13570]; - UnitDelay_DSTATE_n_cov_diag[116] = est_estimator_B->Switch1[13688]; - - // Sum: '/Sum' incorporates: - // BusAssignment: '/Bus Assignment' - // Constant: '/Constant1' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Math: '/Math Function' - - rtb_Switch1 = rtb_Merge2.quat_ISS2B[3] * rtb_Merge2.quat_ISS2B[3] * - est_estimator_P->Gain_Gain_h3 - (real32_T) - est_estimator_P->Constant1_Value_ec; - - // Assignment: '/Assignment' incorporates: - // Constant: '/Constant2' - // DataTypeConversion: '/Conversion' - - for (i = 0; i < 9; i++) { - rtb_Assignment_b[i] = (real32_T)est_estimator_P->Constant2_Value_d[i]; - } - - rtb_Assignment_b[0] = rtb_Switch1; - rtb_Assignment_b[4] = rtb_Switch1; - rtb_Assignment_b[8] = rtb_Switch1; - - // End of Assignment: '/Assignment' - - // Gain: '/Gain1' incorporates: - // BusAssignment: '/Bus Assignment' - - rtb_Sum_k1 = est_estimator_P->Gain1_Gain_jb * rtb_Merge2.quat_ISS2B[3]; - - // Product: '/Product' incorporates: - // BusAssignment: '/Bus Assignment' - // Constant: '/Constant3' - // DataTypeConversion: '/Conversion' - // Gain: '/Gain' - // Gain: '/Gain1' - // Gain: '/Gain2' - - tmp_f[0] = (real32_T)est_estimator_P->Constant3_Value_ol; - tmp_f[1] = rtb_Merge2.quat_ISS2B[2]; - tmp_f[2] = est_estimator_P->Gain_Gain_j2 * rtb_Merge2.quat_ISS2B[1]; - tmp_f[3] = est_estimator_P->Gain1_Gain_jp * rtb_Merge2.quat_ISS2B[2]; - tmp_f[4] = (real32_T)est_estimator_P->Constant3_Value_ol; - tmp_f[5] = rtb_Merge2.quat_ISS2B[0]; - tmp_f[6] = rtb_Merge2.quat_ISS2B[1]; - tmp_f[7] = est_estimator_P->Gain2_Gain_ko * rtb_Merge2.quat_ISS2B[0]; - tmp_f[8] = (real32_T)est_estimator_P->Constant3_Value_ol; - - // Math: '/Math Function' incorporates: - // BusAssignment: '/Bus Assignment' - // Gain: '/Gain2' - // Product: '/Product1' - - for (i = 0; i < 3; i++) { - rtb_Merge2_1[i] = rtb_Merge2.quat_ISS2B[0] * rtb_Merge2.quat_ISS2B[i]; - rtb_Merge2_1[(int32_T)(i + 3)] = rtb_Merge2.quat_ISS2B[1] * - rtb_Merge2.quat_ISS2B[i]; - rtb_Merge2_1[(int32_T)(i + 6)] = rtb_Merge2.quat_ISS2B[2] * - rtb_Merge2.quat_ISS2B[i]; - } - - // End of Math: '/Math Function' - - // Sum: '/Sum1' incorporates: - // Gain: '/Gain2' - // Product: '/Product' - // Product: '/Product' - - for (i = 0; i < 3; i++) { - rtb_Assignment_n[(int32_T)(3 * i)] = (rtb_Assignment_b[i] - rtb_Sum_k1 * - tmp_f[i]) + rtb_Merge2_1[(int32_T)(3 * i)] * - est_estimator_P->Gain2_Gain_a2; - rtb_Assignment_n[(int32_T)(1 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 3)] - tmp_f[(int32_T)(i + 3)] * rtb_Sum_k1) + rtb_Merge2_1 - [(int32_T)((int32_T)(3 * i) + 1)] * est_estimator_P->Gain2_Gain_a2; - rtb_Assignment_n[(int32_T)(2 + (int32_T)(3 * i))] = (rtb_Assignment_b - [(int32_T)(i + 6)] - tmp_f[(int32_T)(i + 6)] * rtb_Sum_k1) + rtb_Merge2_1 - [(int32_T)((int32_T)(3 * i) + 2)] * est_estimator_P->Gain2_Gain_a2; - } - - // End of Sum: '/Sum1' - for (i = 0; i < 3; i++) { - // Sum: '/Sum1' incorporates: - // BusAssignment: '/Bus Assignment' - // Constant: '/Constant5' - // Product: '/Product' - - rtb_Sum1_k3[i] = rtb_Merge2.P_EST_ISS_ISS[i] - ((rtb_Assignment_n[(int32_T) - (i + 3)] * est_estimator_P->tun_abp_p_imu_body_body[1] + - rtb_Assignment_n[i] * est_estimator_P->tun_abp_p_imu_body_body[0]) + - rtb_Assignment_n[(int32_T)(i + 6)] * - est_estimator_P->tun_abp_p_imu_body_body[2]); - - // Update for UnitDelay: '/Unit Delay20' - est_estimator_DW->UnitDelay20_DSTATE[i] = rtb_ml_vel_aug[i]; - - // Update for UnitDelay: '/Unit Delay21' - est_estimator_DW->UnitDelay21_DSTATE[i] = rtb_ml_omega_aug[i]; - } - - // Update for UnitDelay: '/Unit Delay22' - memcpy(&est_estimator_DW->UnitDelay22_DSTATE[0], &rtb_of_vel_aug[0], (uint32_T) - (48U * sizeof(real_T))); - - // Update for UnitDelay: '/Unit Delay23' - memcpy(&est_estimator_DW->UnitDelay23_DSTATE[0], &rtb_of_omega_aug[0], - (uint32_T)(48U * sizeof(real_T))); - - // Update for Delay: '/Delay' - est_estimator_DW->icLoad = 0U; - memcpy(&est_estimator_DW->Delay_DSTATE[0], &est_estimator_B->Switch1[0], - (uint32_T)(13689U * sizeof(ase_cov_datatype))); - - // Update for UnitDelay: '/Unit Delay1' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay1_DSTATE[0] = rtb_Merge2.quat_ISS2B[0]; - est_estimator_DW->UnitDelay1_DSTATE[1] = rtb_Merge2.quat_ISS2B[1]; - est_estimator_DW->UnitDelay1_DSTATE[2] = rtb_Merge2.quat_ISS2B[2]; - est_estimator_DW->UnitDelay1_DSTATE[3] = rtb_Merge2.quat_ISS2B[3]; - - // Update for UnitDelay: '/Unit Delay3' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay3_DSTATE[0] = rtb_Merge2.gyro_bias[0]; - - // Update for UnitDelay: '/Unit Delay4' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay4_DSTATE[0] = rtb_Merge2.V_B_ISS_ISS[0]; - - // Update for UnitDelay: '/Unit Delay6' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay6_DSTATE[0] = rtb_Merge2.accel_bias[0]; - - // Update for UnitDelay: '/Unit Delay7' incorporates: - // BusAssignment: '/Bus Assignment1' - - est_estimator_DW->UnitDelay7_DSTATE[0] = rtb_Sum1_k3[0]; - - // Update for UnitDelay: '/Unit Delay3' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay3_DSTATE[1] = rtb_Merge2.gyro_bias[1]; - - // Update for UnitDelay: '/Unit Delay4' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay4_DSTATE[1] = rtb_Merge2.V_B_ISS_ISS[1]; - - // Update for UnitDelay: '/Unit Delay6' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay6_DSTATE[1] = rtb_Merge2.accel_bias[1]; - - // Update for UnitDelay: '/Unit Delay7' incorporates: - // BusAssignment: '/Bus Assignment1' - - est_estimator_DW->UnitDelay7_DSTATE[1] = rtb_Sum1_k3[1]; - - // Update for UnitDelay: '/Unit Delay3' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay3_DSTATE[2] = rtb_Merge2.gyro_bias[2]; - - // Update for UnitDelay: '/Unit Delay4' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay4_DSTATE[2] = rtb_Merge2.V_B_ISS_ISS[2]; - - // Update for UnitDelay: '/Unit Delay6' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay6_DSTATE[2] = rtb_Merge2.accel_bias[2]; - - // Update for UnitDelay: '/Unit Delay7' incorporates: - // BusAssignment: '/Bus Assignment1' - - est_estimator_DW->UnitDelay7_DSTATE[2] = rtb_Sum1_k3[2]; - - // Update for UnitDelay: '/Unit Delay8' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay8_DSTATE = rtb_Switch_m; - - // Update for UnitDelay: '/Unit Delay9' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay9_DSTATE = rtb_Merge2.aug_state_enum; - - // Update for UnitDelay: '/Unit Delay10' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay10_DSTATE[0] = rtb_Merge2.ml_quat_ISS2cam[0]; - est_estimator_DW->UnitDelay10_DSTATE[1] = rtb_Merge2.ml_quat_ISS2cam[1]; - est_estimator_DW->UnitDelay10_DSTATE[2] = rtb_Merge2.ml_quat_ISS2cam[2]; - est_estimator_DW->UnitDelay10_DSTATE[3] = rtb_Merge2.ml_quat_ISS2cam[3]; - - // Update for UnitDelay: '/Unit Delay11' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay11_DSTATE[0] = rtb_Merge2.ml_P_cam_ISS_ISS[0]; - est_estimator_DW->UnitDelay11_DSTATE[1] = rtb_Merge2.ml_P_cam_ISS_ISS[1]; - est_estimator_DW->UnitDelay11_DSTATE[2] = rtb_Merge2.ml_P_cam_ISS_ISS[2]; - - // Update for UnitDelay: '/Unit Delay12' incorporates: - // BusAssignment: '/Bus Assignment' - - memcpy(&est_estimator_DW->UnitDelay12_DSTATE[0], &rtb_Merge2.of_quat_ISS2cam[0], - (uint32_T)(sizeof(real32_T) << 6U)); - - // Update for UnitDelay: '/Unit Delay13' incorporates: - // BusAssignment: '/Bus Assignment' - - memcpy(&est_estimator_DW->UnitDelay13_DSTATE[0], &rtb_Merge2.of_P_cam_ISS_ISS - [0], (uint32_T)(48U * sizeof(real32_T))); - - // Update for UnitDelay: '/Unit Delay14' incorporates: - // BusAssignment: '/Bus Assignment1' - - memcpy(&est_estimator_DW->UnitDelay14_DSTATE[0], &UnitDelay_DSTATE_n_cov_diag - [0], (uint32_T)(117U * sizeof(ase_cov_datatype))); - - // Update for UnitDelay: '/Unit Delay15' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay15_DSTATE = rtb_Merge2.kfl_status; - - // Update for UnitDelay: '/Unit Delay16' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay16_DSTATE = rtb_Merge2.update_OF_tracks_cnt; - - // Update for UnitDelay: '/Unit Delay17' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay17_DSTATE = rtb_Merge2.update_ML_features_cnt; - - // Update for UnitDelay: '/Unit Delay18' incorporates: - // BusAssignment: '/Bus Assignment' - - memcpy(&est_estimator_DW->UnitDelay18_DSTATE[0], - &rtb_Merge2.of_mahal_distance[0], (uint32_T)(50U * sizeof(real_T))); - - // Update for UnitDelay: '/Unit Delay19' incorporates: - // BusAssignment: '/Bus Assignment' - - memcpy(&est_estimator_DW->UnitDelay19_DSTATE[0], - &rtb_Merge2.ml_mahal_distance[0], (uint32_T)(50U * sizeof(real_T))); - - // Update for UnitDelay: '/Unit Delay24' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay24_DSTATE[0] = rtb_Merge2.hr_P_hr_ISS_ISS[0]; - est_estimator_DW->UnitDelay24_DSTATE[1] = rtb_Merge2.hr_P_hr_ISS_ISS[1]; - est_estimator_DW->UnitDelay24_DSTATE[2] = rtb_Merge2.hr_P_hr_ISS_ISS[2]; - - // Update for UnitDelay: '/Unit Delay25' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay25_DSTATE[0] = rtb_Merge2.hr_quat_ISS2hr[0]; - est_estimator_DW->UnitDelay25_DSTATE[1] = rtb_Merge2.hr_quat_ISS2hr[1]; - est_estimator_DW->UnitDelay25_DSTATE[2] = rtb_Merge2.hr_quat_ISS2hr[2]; - est_estimator_DW->UnitDelay25_DSTATE[3] = rtb_Merge2.hr_quat_ISS2hr[3]; - - // Update for UnitDelay: '/Unit Delay26' incorporates: - // BusAssignment: '/Bus Assignment' - - est_estimator_DW->UnitDelay26_DSTATE[0] = rtb_Merge2.P_EST_ISS_ISS[0]; - est_estimator_DW->UnitDelay26_DSTATE[1] = rtb_Merge2.P_EST_ISS_ISS[1]; - est_estimator_DW->UnitDelay26_DSTATE[2] = rtb_Merge2.P_EST_ISS_ISS[2]; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/handrail_msg' - - est_estimator_DW->DelayInput1_DSTATE = - est_estimator_U_handrail_msg->cvs_timestamp_sec; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/handrail_msg' - - est_estimator_DW->DelayInput1_DSTATE_h = - est_estimator_U_handrail_msg->cvs_timestamp_nsec; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Inport: '/cmc_msg' - // SignalConversion: '/Signal Conversion' - - est_estimator_DW->DelayInput1_DSTATE_k = - est_estimator_U_cmc_msg_o->localization_mode_cmd; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/landmark_msg' - - est_estimator_DW->DelayInput1_DSTATE_o = - est_estimator_U_landmark_msg->cvs_timestamp_sec; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/landmark_msg' - - est_estimator_DW->DelayInput1_DSTATE_n = - est_estimator_U_landmark_msg->cvs_timestamp_nsec; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/optical_flow_msg' - - est_estimator_DW->DelayInput1_DSTATE_b = - est_estimator_U_cvs_optical_flow_msg_n->cvs_timestamp_sec; - - // Update for UnitDelay: '/Delay Input1' incorporates: - // Update for Inport: '/optical_flow_msg' - - est_estimator_DW->DelayInput1_DSTATE_d = - est_estimator_U_cvs_optical_flow_msg_n->cvs_timestamp_nsec; - - // Update for UnitDelay: '/Unit Delay' - est_estimator_DW->UnitDelay_DSTATE_j = num_augs; - - // Outport: '/kfl_msg' incorporates: - // BusAssignment: '/Bus Assignment' - // BusAssignment: '/Bus Assignment1' - - est_estimator_Y_kfl_msg_h->quat_ISS2B[0] = rtb_Merge2.quat_ISS2B[0]; - est_estimator_Y_kfl_msg_h->quat_ISS2B[1] = rtb_Merge2.quat_ISS2B[1]; - est_estimator_Y_kfl_msg_h->quat_ISS2B[2] = rtb_Merge2.quat_ISS2B[2]; - est_estimator_Y_kfl_msg_h->quat_ISS2B[3] = rtb_Merge2.quat_ISS2B[3]; - est_estimator_Y_kfl_msg_h->omega_B_ISS_B[0] = rtb_Merge2.omega_B_ISS_B[0]; - est_estimator_Y_kfl_msg_h->gyro_bias[0] = rtb_Merge2.gyro_bias[0]; - est_estimator_Y_kfl_msg_h->V_B_ISS_ISS[0] = rtb_Merge2.V_B_ISS_ISS[0]; - est_estimator_Y_kfl_msg_h->A_B_ISS_ISS[0] = rtb_Merge2.A_B_ISS_ISS[0]; - est_estimator_Y_kfl_msg_h->accel_bias[0] = rtb_Merge2.accel_bias[0]; - est_estimator_Y_kfl_msg_h->P_B_ISS_ISS[0] = rtb_Sum1_k3[0]; - est_estimator_Y_kfl_msg_h->omega_B_ISS_B[1] = rtb_Merge2.omega_B_ISS_B[1]; - est_estimator_Y_kfl_msg_h->gyro_bias[1] = rtb_Merge2.gyro_bias[1]; - est_estimator_Y_kfl_msg_h->V_B_ISS_ISS[1] = rtb_Merge2.V_B_ISS_ISS[1]; - est_estimator_Y_kfl_msg_h->A_B_ISS_ISS[1] = rtb_Merge2.A_B_ISS_ISS[1]; - est_estimator_Y_kfl_msg_h->accel_bias[1] = rtb_Merge2.accel_bias[1]; - est_estimator_Y_kfl_msg_h->P_B_ISS_ISS[1] = rtb_Sum1_k3[1]; - est_estimator_Y_kfl_msg_h->omega_B_ISS_B[2] = rtb_Merge2.omega_B_ISS_B[2]; - est_estimator_Y_kfl_msg_h->gyro_bias[2] = rtb_Merge2.gyro_bias[2]; - est_estimator_Y_kfl_msg_h->V_B_ISS_ISS[2] = rtb_Merge2.V_B_ISS_ISS[2]; - est_estimator_Y_kfl_msg_h->A_B_ISS_ISS[2] = rtb_Merge2.A_B_ISS_ISS[2]; - est_estimator_Y_kfl_msg_h->accel_bias[2] = rtb_Merge2.accel_bias[2]; - est_estimator_Y_kfl_msg_h->P_B_ISS_ISS[2] = rtb_Sum1_k3[2]; - est_estimator_Y_kfl_msg_h->confidence = rtb_Switch_m; - est_estimator_Y_kfl_msg_h->aug_state_enum = rtb_Merge2.aug_state_enum; - est_estimator_Y_kfl_msg_h->ml_quat_ISS2cam[0] = rtb_Merge2.ml_quat_ISS2cam[0]; - est_estimator_Y_kfl_msg_h->ml_quat_ISS2cam[1] = rtb_Merge2.ml_quat_ISS2cam[1]; - est_estimator_Y_kfl_msg_h->ml_quat_ISS2cam[2] = rtb_Merge2.ml_quat_ISS2cam[2]; - est_estimator_Y_kfl_msg_h->ml_quat_ISS2cam[3] = rtb_Merge2.ml_quat_ISS2cam[3]; - est_estimator_Y_kfl_msg_h->ml_P_cam_ISS_ISS[0] = rtb_Merge2.ml_P_cam_ISS_ISS[0]; - est_estimator_Y_kfl_msg_h->ml_P_cam_ISS_ISS[1] = rtb_Merge2.ml_P_cam_ISS_ISS[1]; - est_estimator_Y_kfl_msg_h->ml_P_cam_ISS_ISS[2] = rtb_Merge2.ml_P_cam_ISS_ISS[2]; - memcpy(&est_estimator_Y_kfl_msg_h->of_quat_ISS2cam[0], - &rtb_Merge2.of_quat_ISS2cam[0], (uint32_T)(sizeof(real32_T) << 6U)); - memcpy(&est_estimator_Y_kfl_msg_h->of_P_cam_ISS_ISS[0], - &rtb_Merge2.of_P_cam_ISS_ISS[0], (uint32_T)(48U * sizeof(real32_T))); - memcpy(&est_estimator_Y_kfl_msg_h->cov_diag[0], &UnitDelay_DSTATE_n_cov_diag[0], - (uint32_T)(117U * sizeof(ase_cov_datatype))); - est_estimator_Y_kfl_msg_h->kfl_status = rtb_Merge2.kfl_status; - est_estimator_Y_kfl_msg_h->update_OF_tracks_cnt = - rtb_Merge2.update_OF_tracks_cnt; - est_estimator_Y_kfl_msg_h->update_ML_features_cnt = - rtb_Merge2.update_ML_features_cnt; - memcpy(&est_estimator_Y_kfl_msg_h->of_mahal_distance[0], - &rtb_Merge2.of_mahal_distance[0], (uint32_T)(50U * sizeof(real_T))); - memcpy(&est_estimator_Y_kfl_msg_h->ml_mahal_distance[0], - &rtb_Merge2.ml_mahal_distance[0], (uint32_T)(50U * sizeof(real_T))); - est_estimator_Y_kfl_msg_h->hr_P_hr_ISS_ISS[0] = rtb_Merge2.hr_P_hr_ISS_ISS[0]; - est_estimator_Y_kfl_msg_h->hr_P_hr_ISS_ISS[1] = rtb_Merge2.hr_P_hr_ISS_ISS[1]; - est_estimator_Y_kfl_msg_h->hr_P_hr_ISS_ISS[2] = rtb_Merge2.hr_P_hr_ISS_ISS[2]; - est_estimator_Y_kfl_msg_h->hr_quat_ISS2hr[0] = rtb_Merge2.hr_quat_ISS2hr[0]; - est_estimator_Y_kfl_msg_h->hr_quat_ISS2hr[1] = rtb_Merge2.hr_quat_ISS2hr[1]; - est_estimator_Y_kfl_msg_h->hr_quat_ISS2hr[2] = rtb_Merge2.hr_quat_ISS2hr[2]; - est_estimator_Y_kfl_msg_h->hr_quat_ISS2hr[3] = rtb_Merge2.hr_quat_ISS2hr[3]; - est_estimator_Y_kfl_msg_h->P_EST_ISS_ISS[0] = rtb_Merge2.P_EST_ISS_ISS[0]; - est_estimator_Y_kfl_msg_h->P_EST_ISS_ISS[1] = rtb_Merge2.P_EST_ISS_ISS[1]; - est_estimator_Y_kfl_msg_h->P_EST_ISS_ISS[2] = rtb_Merge2.P_EST_ISS_ISS[2]; - - // End of Outputs for SubSystem: '/est_estimator' - - // Outport: '/P_out' - memcpy(&est_estimator_Y_P_out[0], &est_estimator_B->Switch1[0], (uint32_T) - (13689U * sizeof(ase_cov_datatype))); -} - -// Model initialize function -void est_estimator_initialize(RT_MODEL_est_estimator_T *const est_estimator_M, - cvs_landmark_msg *est_estimator_U_landmark_msg, cvs_registration_pulse - *est_estimator_U_VisionRegistration, cvs_optical_flow_msg - *est_estimator_U_cvs_optical_flow_msg_n, cvs_handrail_msg - *est_estimator_U_handrail_msg, imu_msg *est_estimator_U_imu_msg_c, cmc_msg - *est_estimator_U_cmc_msg_o, real32_T est_estimator_U_Q_ISS2B[4], kfl_msg - *est_estimator_Y_kfl_msg_h, ase_cov_datatype est_estimator_Y_P_out[13689]) -{ - P_est_estimator_T *est_estimator_P = ((P_est_estimator_T *) - est_estimator_M->defaultParam); - B_est_estimator_T *est_estimator_B = ((B_est_estimator_T *) - est_estimator_M->blockIO); - DW_est_estimator_T *est_estimator_DW = ((DW_est_estimator_T *) - est_estimator_M->dwork); - - { - // local scratch DWork variables - int32_T ForEach_itr; - int32_T ForEach_itr_g; - int32_T i; - - // Start for Atomic SubSystem: '/est_estimator' - // Start for Iterator SubSystem: '/filter' - for (ForEach_itr_g = 0; ForEach_itr_g < 3; ForEach_itr_g++) { - est_estimator_DW->CoreSubsys[ForEach_itr_g].uHzLowPass_states = 0.0F; - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0] = - 0.0F; - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[1] = - 0.0F; - - // Start for MATLAB Function: '/MATLAB Function1' - est_estim_MATLABFunction1_Start(&est_estimator_B->CoreSubsys[ForEach_itr_g] - .sf_MATLABFunction1); - } - - // End of Start for SubSystem: '/filter' - - // Start for Iterator SubSystem: '/filter_with_HP_filter' - for (ForEach_itr = 0; ForEach_itr < 3; ForEach_itr++) { - est_estimator_DW->CoreSubsys_l[ForEach_itr].uHzLowPass_states = 0.0F; - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[0] = - 0.0F; - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[1] = - 0.0F; - est_estimator_DW->CoreSubsys_l[ForEach_itr].HighPassFilter_states = 0.0F; - - // Start for MATLAB Function: '/MATLAB Function1' - est_estim_MATLABFunction1_Start(&est_estimator_B->CoreSubsys_l[ForEach_itr] - .sf_MATLABFunction1); - } - - // End of Start for SubSystem: '/filter_with_HP_filter' - // End of Start for SubSystem: '/est_estimator' - - // SystemInitialize for Atomic SubSystem: '/est_estimator' - // InitializeConditions for UnitDelay: '/Unit Delay20' - est_estimator_DW->UnitDelay20_DSTATE[0] = - est_estimator_P->UnitDelay20_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay21' - est_estimator_DW->UnitDelay21_DSTATE[0] = - est_estimator_P->UnitDelay21_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay20' - est_estimator_DW->UnitDelay20_DSTATE[1] = - est_estimator_P->UnitDelay20_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay21' - est_estimator_DW->UnitDelay21_DSTATE[1] = - est_estimator_P->UnitDelay21_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay20' - est_estimator_DW->UnitDelay20_DSTATE[2] = - est_estimator_P->UnitDelay20_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay21' - est_estimator_DW->UnitDelay21_DSTATE[2] = - est_estimator_P->UnitDelay21_InitialCondition; - for (i = 0; i < 48; i++) { - // InitializeConditions for UnitDelay: '/Unit Delay22' - est_estimator_DW->UnitDelay22_DSTATE[i] = - est_estimator_P->UnitDelay22_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay23' - est_estimator_DW->UnitDelay23_DSTATE[i] = - est_estimator_P->UnitDelay23_InitialCondition; - } - - // InitializeConditions for Delay: '/Delay' - est_estimator_DW->icLoad = 1U; - - // InitializeConditions for UnitDelay: '/Unit Delay1' - est_estimator_DW->UnitDelay1_DSTATE[0] = - est_estimator_P->tun_ase_state_ic_quat_ISS2B[0]; - est_estimator_DW->UnitDelay1_DSTATE[1] = - est_estimator_P->tun_ase_state_ic_quat_ISS2B[1]; - est_estimator_DW->UnitDelay1_DSTATE[2] = - est_estimator_P->tun_ase_state_ic_quat_ISS2B[2]; - est_estimator_DW->UnitDelay1_DSTATE[3] = - est_estimator_P->tun_ase_state_ic_quat_ISS2B[3]; - - // InitializeConditions for UnitDelay: '/Unit Delay3' - est_estimator_DW->UnitDelay3_DSTATE[0] = - est_estimator_P->ase_state_ic_gyro_bias[0]; - - // InitializeConditions for UnitDelay: '/Unit Delay4' - est_estimator_DW->UnitDelay4_DSTATE[0] = - est_estimator_P->tun_ase_state_ic_V_B_ISS_ISS[0]; - - // InitializeConditions for UnitDelay: '/Unit Delay6' - est_estimator_DW->UnitDelay6_DSTATE[0] = - est_estimator_P->ase_state_ic_accel_bias[0]; - - // InitializeConditions for UnitDelay: '/Unit Delay7' - est_estimator_DW->UnitDelay7_DSTATE[0] = - est_estimator_P->tun_ase_state_ic_P_B_ISS_ISS[0]; - - // InitializeConditions for UnitDelay: '/Unit Delay3' - est_estimator_DW->UnitDelay3_DSTATE[1] = - est_estimator_P->ase_state_ic_gyro_bias[1]; - - // InitializeConditions for UnitDelay: '/Unit Delay4' - est_estimator_DW->UnitDelay4_DSTATE[1] = - est_estimator_P->tun_ase_state_ic_V_B_ISS_ISS[1]; - - // InitializeConditions for UnitDelay: '/Unit Delay6' - est_estimator_DW->UnitDelay6_DSTATE[1] = - est_estimator_P->ase_state_ic_accel_bias[1]; - - // InitializeConditions for UnitDelay: '/Unit Delay7' - est_estimator_DW->UnitDelay7_DSTATE[1] = - est_estimator_P->tun_ase_state_ic_P_B_ISS_ISS[1]; - - // InitializeConditions for UnitDelay: '/Unit Delay3' - est_estimator_DW->UnitDelay3_DSTATE[2] = - est_estimator_P->ase_state_ic_gyro_bias[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay4' - est_estimator_DW->UnitDelay4_DSTATE[2] = - est_estimator_P->tun_ase_state_ic_V_B_ISS_ISS[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay6' - est_estimator_DW->UnitDelay6_DSTATE[2] = - est_estimator_P->ase_state_ic_accel_bias[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay7' - est_estimator_DW->UnitDelay7_DSTATE[2] = - est_estimator_P->tun_ase_state_ic_P_B_ISS_ISS[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay8' - est_estimator_DW->UnitDelay8_DSTATE = - est_estimator_P->ase_state_ic_confidence; - - // InitializeConditions for UnitDelay: '/Unit Delay9' - est_estimator_DW->UnitDelay9_DSTATE = (uint32_T) - est_estimator_P->ase_state_ic_aug_state_enum; - - // InitializeConditions for UnitDelay: '/Unit Delay10' - est_estimator_DW->UnitDelay10_DSTATE[0] = - est_estimator_P->ase_state_ic_ml_quat_ISS2cam[0]; - est_estimator_DW->UnitDelay10_DSTATE[1] = - est_estimator_P->ase_state_ic_ml_quat_ISS2cam[1]; - est_estimator_DW->UnitDelay10_DSTATE[2] = - est_estimator_P->ase_state_ic_ml_quat_ISS2cam[2]; - est_estimator_DW->UnitDelay10_DSTATE[3] = - est_estimator_P->ase_state_ic_ml_quat_ISS2cam[3]; - - // InitializeConditions for UnitDelay: '/Unit Delay11' - est_estimator_DW->UnitDelay11_DSTATE[0] = - est_estimator_P->ase_state_ic_ml_P_cam_ISS_ISS[0]; - est_estimator_DW->UnitDelay11_DSTATE[1] = - est_estimator_P->ase_state_ic_ml_P_cam_ISS_ISS[1]; - est_estimator_DW->UnitDelay11_DSTATE[2] = - est_estimator_P->ase_state_ic_ml_P_cam_ISS_ISS[2]; - - // InitializeConditions for UnitDelay: '/Unit Delay12' - memcpy(&est_estimator_DW->UnitDelay12_DSTATE[0], - &est_estimator_P->ase_state_ic_of_quat_ISS2cam[0], (uint32_T)(sizeof - (real32_T) << 6U)); - - // InitializeConditions for UnitDelay: '/Unit Delay13' - memcpy(&est_estimator_DW->UnitDelay13_DSTATE[0], - &est_estimator_P->ase_state_ic_of_P_cam_ISS_ISS[0], (uint32_T)(48U * - sizeof(real32_T))); - - // InitializeConditions for UnitDelay: '/Unit Delay14' - memcpy(&est_estimator_DW->UnitDelay14_DSTATE[0], - &est_estimator_P->ase_state_ic_cov_diag[0], (uint32_T)(117U * sizeof - (ase_cov_datatype))); - - // InitializeConditions for UnitDelay: '/Unit Delay15' - est_estimator_DW->UnitDelay15_DSTATE = (uint16_T) - est_estimator_P->ase_state_ic_status; - - // InitializeConditions for UnitDelay: '/Unit Delay16' - est_estimator_DW->UnitDelay16_DSTATE = - est_estimator_P->UnitDelay16_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay17' - est_estimator_DW->UnitDelay17_DSTATE = - est_estimator_P->UnitDelay17_InitialCondition; - for (i = 0; i < 50; i++) { - // InitializeConditions for UnitDelay: '/Unit Delay18' - est_estimator_DW->UnitDelay18_DSTATE[i] = - est_estimator_P->UnitDelay18_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay19' - est_estimator_DW->UnitDelay19_DSTATE[i] = - est_estimator_P->UnitDelay19_InitialCondition; - } - - // InitializeConditions for UnitDelay: '/Unit Delay24' - est_estimator_DW->UnitDelay24_DSTATE[0] = - est_estimator_P->UnitDelay24_InitialCondition; - est_estimator_DW->UnitDelay24_DSTATE[1] = - est_estimator_P->UnitDelay24_InitialCondition; - est_estimator_DW->UnitDelay24_DSTATE[2] = - est_estimator_P->UnitDelay24_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay25' - est_estimator_DW->UnitDelay25_DSTATE[0] = - est_estimator_P->UnitDelay25_InitialCondition; - est_estimator_DW->UnitDelay25_DSTATE[1] = - est_estimator_P->UnitDelay25_InitialCondition; - est_estimator_DW->UnitDelay25_DSTATE[2] = - est_estimator_P->UnitDelay25_InitialCondition; - est_estimator_DW->UnitDelay25_DSTATE[3] = - est_estimator_P->UnitDelay25_InitialCondition; - - // InitializeConditions for UnitDelay: '/Unit Delay26' - est_estimator_DW->UnitDelay26_DSTATE[0] = - est_estimator_P->tun_ase_state_ic_P_EST_ISS_ISS[0]; - est_estimator_DW->UnitDelay26_DSTATE[1] = - est_estimator_P->tun_ase_state_ic_P_EST_ISS_ISS[1]; - est_estimator_DW->UnitDelay26_DSTATE[2] = - est_estimator_P->tun_ase_state_ic_P_EST_ISS_ISS[2]; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE = est_estimator_P->DetectChange6_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_h = - est_estimator_P->DetectChange7_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_k = - est_estimator_P->DetectChange_vinit_i; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_o = est_estimator_P->DetectChange_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_n = - est_estimator_P->DetectChange1_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_b = - est_estimator_P->DetectChange4_vinit; - - // InitializeConditions for UnitDelay: '/Delay Input1' - est_estimator_DW->DelayInput1_DSTATE_d = - est_estimator_P->DetectChange5_vinit; - - // InitializeConditions for UnitDelay: '/Unit Delay' - est_estimator_DW->UnitDelay_DSTATE_j = - est_estimator_P->UnitDelay_InitialCondition_h; - - // SystemInitialize for Iterator SubSystem: '/filter' - for (ForEach_itr_g = 0; ForEach_itr_g < 3; ForEach_itr_g++) { - // InitializeConditions for DiscreteTransferFcn: '/3 Hz Low Pass' - est_estimator_DW->CoreSubsys[ForEach_itr_g].uHzLowPass_states = - est_estimator_P->CoreSubsys.uHzLowPass_InitialStates; - - // InitializeConditions for DiscreteTransferFcn: '/Discrete Transfer Fcn' - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[0] = - est_estimator_P->CoreSubsys.DiscreteTransferFcn_InitialStat; - est_estimator_DW->CoreSubsys[ForEach_itr_g].DiscreteTransferFcn_states[1] = - est_estimator_P->CoreSubsys.DiscreteTransferFcn_InitialStat; - - // SystemInitialize for MATLAB Function: '/MATLAB Function1' - est_estima_MATLABFunction1_Init(&est_estimator_DW-> - CoreSubsys[ForEach_itr_g].sf_MATLABFunction1); - } - - // End of SystemInitialize for SubSystem: '/filter' - - // SystemInitialize for Iterator SubSystem: '/filter_with_HP_filter' - for (ForEach_itr = 0; ForEach_itr < 3; ForEach_itr++) { - // InitializeConditions for DiscreteTransferFcn: '/3 Hz Low Pass' - est_estimator_DW->CoreSubsys_l[ForEach_itr].uHzLowPass_states = - est_estimator_P->CoreSubsys_l.uHzLowPass_InitialStates; - - // InitializeConditions for DiscreteTransferFcn: '/Discrete Transfer Fcn' - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[0] = - est_estimator_P->CoreSubsys_l.DiscreteTransferFcn_InitialStat; - est_estimator_DW->CoreSubsys_l[ForEach_itr].DiscreteTransferFcn_states[1] = - est_estimator_P->CoreSubsys_l.DiscreteTransferFcn_InitialStat; - - // InitializeConditions for DiscreteTransferFcn: '/High Pass Filter' - est_estimator_DW->CoreSubsys_l[ForEach_itr].HighPassFilter_states = - est_estimator_P->CoreSubsys_l.HighPassFilter_InitialStates; - - // SystemInitialize for MATLAB Function: '/MATLAB Function1' - est_estima_MATLABFunction1_Init(&est_estimator_DW-> - CoreSubsys_l[ForEach_itr].sf_MATLABFunction1); - } - - // End of SystemInitialize for SubSystem: '/filter_with_HP_filter' - - // SystemInitialize for Enabled SubSystem: '/Enabled Row-Wise SUM' - // SystemInitialize for Outport: '/Out1' - for (i = 0; i < 50; i++) { - est_estimator_B->SumofElements3[i] = est_estimator_P->Out1_Y0; - } - - // End of SystemInitialize for Outport: '/Out1' - // End of SystemInitialize for SubSystem: '/Enabled Row-Wise SUM' - - // SystemInitialize for IfAction SubSystem: '/Absolute_Update' - // SystemInitialize for Iterator SubSystem: '/ML Update' - // SystemInitialize for IfAction SubSystem: '/HR_Compute_Residual_and_H1' - // SystemInitialize for MATLAB Function: '/Compute Global positions of Handrail Features' - est_estimator_DW->hr_P_hr_ISS_ISS_pers_not_empty = false; - est_estimator_DW->hr_quat_ISS2hr_pers_not_empty = false; - - // End of SystemInitialize for SubSystem: '/HR_Compute_Residual_and_H1' - - // SystemInitialize for IfAction SubSystem: '/ML_Compute_Residual_and_H' - // SystemInitialize for MATLAB Function: '/Compute Residual and H' - est_estimator_DW->num_consecutive_rejected_meas = 0.0; - - // End of SystemInitialize for SubSystem: '/ML_Compute_Residual_and_H' - - // SystemInitialize for Merge: '/Merge' - for (i = 0; i < 6; i++) { - est_estimator_B->r_out[i] = est_estimator_P->Merge_1_InitialOutput; - } - - // End of SystemInitialize for Merge: '/Merge' - - // SystemInitialize for Merge: '/Merge' - est_estimator_B->error_out = est_estimator_P->Merge_2_InitialOutput; - - // SystemInitialize for Merge: '/Merge' - for (i = 0; i < 702; i++) { - est_estimator_B->H_out[i] = est_estimator_P->Merge_3_InitialOutput; - } - - // End of SystemInitialize for Merge: '/Merge' - - // SystemInitialize for Merge: '/Merge' - for (i = 0; i < 36; i++) { - est_estimator_B->R_mat[i] = est_estimator_P->Merge_4_InitialOutput; - } - - // End of SystemInitialize for Merge: '/Merge' - // End of SystemInitialize for SubSystem: '/ML Update' - // End of SystemInitialize for SubSystem: '/Absolute_Update' - - // SystemInitialize for IfAction SubSystem: '/If Action Subsystem2' - // SystemInitialize for MATLAB Function: '/MATLAB Function' - est_estimator_DW->aug_velocity_not_empty = false; - est_estimator_DW->aug_velocity_mag_not_empty = false; - est_estimator_DW->aug_omega_not_empty = false; - est_estimator_DW->aug_omega_mag_not_empty = false; - - // End of SystemInitialize for SubSystem: '/If Action Subsystem2' - // End of SystemInitialize for SubSystem: '/est_estimator' - } -} - -// Model terminate function -void est_estimator_terminate(RT_MODEL_est_estimator_T * est_estimator_M) -{ - // model code - rt_FREE(est_estimator_M->blockIO); - if (est_estimator_M->paramIsMalloced) { - rt_FREE(est_estimator_M->defaultParam); - } - - rt_FREE(est_estimator_M->dwork); - rt_FREE(est_estimator_M); -} - -// Model data allocation function -RT_MODEL_est_estimator_T *est_estimator(cvs_landmark_msg - *est_estimator_U_landmark_msg, cvs_registration_pulse - *est_estimator_U_VisionRegistration, cvs_optical_flow_msg - *est_estimator_U_cvs_optical_flow_msg_n, cvs_handrail_msg - *est_estimator_U_handrail_msg, imu_msg *est_estimator_U_imu_msg_c, cmc_msg - *est_estimator_U_cmc_msg_o, real32_T est_estimator_U_Q_ISS2B[4], kfl_msg - *est_estimator_Y_kfl_msg_h, ase_cov_datatype est_estimator_Y_P_out[13689]) -{ - RT_MODEL_est_estimator_T *est_estimator_M; - est_estimator_M = (RT_MODEL_est_estimator_T *) malloc(sizeof - (RT_MODEL_est_estimator_T)); - if (est_estimator_M == NULL) { - return NULL; - } - - (void) memset((char *)est_estimator_M, 0, - sizeof(RT_MODEL_est_estimator_T)); - - // block I/O - { - B_est_estimator_T *b = (B_est_estimator_T *) malloc(sizeof(B_est_estimator_T)); - rt_VALIDATE_MEMORY(est_estimator_M,b); - est_estimator_M->blockIO = (b); - } - - // parameters - { - P_est_estimator_T *p; - static int_T pSeen = 0; - - // only malloc on multiple model instantiation - if (pSeen == 1 ) { - p = (P_est_estimator_T *) malloc(sizeof(P_est_estimator_T)); - rt_VALIDATE_MEMORY(est_estimator_M,p); - (void) memcpy(p, &est_estimator_P, - sizeof(P_est_estimator_T)); - est_estimator_M->paramIsMalloced = (true); - } else { - p = &est_estimator_P; - est_estimator_M->paramIsMalloced = (false); - pSeen = 1; - } - - est_estimator_M->defaultParam = (p); - } - - // states (dwork) - { - DW_est_estimator_T *dwork = (DW_est_estimator_T *) malloc(sizeof - (DW_est_estimator_T)); - rt_VALIDATE_MEMORY(est_estimator_M,dwork); - est_estimator_M->dwork = (dwork); - } - - { - P_est_estimator_T *est_estimator_P = ((P_est_estimator_T *) - est_estimator_M->defaultParam); - B_est_estimator_T *est_estimator_B = ((B_est_estimator_T *) - est_estimator_M->blockIO); - DW_est_estimator_T *est_estimator_DW = ((DW_est_estimator_T *) - est_estimator_M->dwork); - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); - - // non-finite (run-time) assignments - est_estimator_P->UnitDelay2_InitialCondition = rtInfF; - est_estimator_P->UnitDelay2_InitialCondition_f = rtInfF; - - // block I/O - (void) memset(((void *) est_estimator_B), 0, - sizeof(B_est_estimator_T)); - - // states (dwork) - (void) memset((void *)est_estimator_DW, 0, - sizeof(DW_est_estimator_T)); - - // external inputs - (void)memset(&est_estimator_U_Q_ISS2B[0], 0, sizeof(real32_T) << 2U); - *est_estimator_U_landmark_msg = est_estimator_rtZcvs_landmark_m; - *est_estimator_U_VisionRegistration = est_estimator_rtZcvs_registrati; - *est_estimator_U_cvs_optical_flow_msg_n = est_estimator_rtZcvs_optical_fl; - *est_estimator_U_handrail_msg = est_estimator_rtZcvs_handrail_m; - *est_estimator_U_imu_msg_c = est_estimator_rtZimu_msg; - *est_estimator_U_cmc_msg_o = est_estimator_rtZcmc_msg; - - // external outputs - (*est_estimator_Y_kfl_msg_h) = est_estimator_rtZkfl_msg; - (void) memset(&est_estimator_Y_P_out[0], 0, - 13689U*sizeof(real32_T)); - } - - return est_estimator_M; -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.h b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.h deleted file mode 100644 index 72e5704cb0..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator.h +++ /dev/null @@ -1,1604 +0,0 @@ -// -// File: est_estimator.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_est_estimator_h_ -#define RTW_HEADER_est_estimator_h_ -#include -#include -#include -#ifndef est_estimator_COMMON_INCLUDES_ -# define est_estimator_COMMON_INCLUDES_ -#include -#include "rtwtypes.h" -#include "compute_delta_state_and_cov.h" -#include "apply_delta_state.h" -#include "of_residual_and_h.h" -#include "matrix_multiply.h" -#endif // est_estimator_COMMON_INCLUDES_ - -#include "est_estimator_types.h" -#include "rtGetNaN.h" -#include "rt_nonfinite.h" -#include "aaiepphlecjelnoh_permute.h" -#include "aimgbiekknohhdjm_norm.h" -#include "baaanophjmgddbie_sum.h" -#include "baieimopcbaiaaai_eulers_to_quat.h" -#include "biecdbieglngohlf_pinv.h" -#include "biekcjmgdbaadbim_eye.h" -#include "div_nzp_s32_floor.h" -#include "djmgjecbcbiengln_power.h" -#include "ecjedbaiaiekohln_quaternion_to_rotation.h" -#include "fkfcbaiengdjgdje_quaternion_to_rotation.h" -#include "fkngdjekgdjepphl_sum.h" -#include "gdjmmglnmgdjlfkf_quat_rotation_vec.h" -#include "glfcngdjgdjmmglf_pinv.h" -#include "hdbaohdbkngdbimo_PadeApproximantOfDegree.h" -#include "hdbihlngknopkfcj_repmat.h" -#include "iecjbieccbailfcb_abs.h" -#include "iecjopppiecjmgln_quatmult.h" -#include "iekfiecjknopophd_pinv.h" -#include "imohcjmoimopimoh_nullAssignment.h" -#include "imohknglphlfpphd_repmat.h" -#include "jekfopppngdbhlng_diag.h" -#include "jmglopphppphkfkf_qr.h" -#include "jmohiecblfcjnohl_qr.h" -#include "kngldbimhdbaimgd_quat_propagate_step.h" -#include "mgdbbiekfknonglf_nullAssignment.h" -#include "mglfbimobiechdbi_bitget.h" -#include "mglnkfkfmglfjekn_PadeApproximantOfDegree.h" -#include "moppbaaafkfkimgd_diag.h" -#include "ngdjjecbgdbaglfc_eye.h" -#include "nohdcbaibiecnohl_power.h" -#include "nohlcjekmohddjmg_abs.h" -#include "ophlcjmgkfcbmohl_nullAssignment.h" -#include "rt_powf_snf.h" -#include "rt_roundd_snf.h" -#include "rtGetInf.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -# define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) -#endif - -#ifndef rtmSetErrorStatus -# define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -// Exported data define - -// Definition for custom storage class: Define -#define ASE_ML_NUM_FEATURES 50 -#define ASE_OF_NUM_AUG 16 -#define ASE_OF_NUM_FEATURES 50 - -// Block signals for system '/MATLAB Function1' -typedef struct { - real32_T num_out[3]; // '/MATLAB Function1' - real32_T den_out[3]; // '/MATLAB Function1' -} B_MATLABFunction1_est_estimat_T; - -// Block states (auto storage) for system '/MATLAB Function1' -typedef struct { - real32_T y; // '/MATLAB Function1' - real32_T num[3]; // '/MATLAB Function1' - real32_T den[3]; // '/MATLAB Function1' - real32_T prev_impeller_speed; // '/MATLAB Function1' -} DW_MATLABFunction1_est_estima_T; - -// Block signals for system '/CoreSubsys' -typedef struct { - B_MATLABFunction1_est_estimat_T sf_MATLABFunction1;// '/MATLAB Function1' -} B_CoreSubsys_est_estimator_T; - -// Block states (auto storage) for system '/CoreSubsys' -typedef struct { - real32_T uHzLowPass_states; // '/3 Hz Low Pass' - real32_T DiscreteTransferFcn_states[2];// '/Discrete Transfer Fcn' - DW_MATLABFunction1_est_estima_T sf_MATLABFunction1;// '/MATLAB Function1' -} DW_CoreSubsys_est_estimator_T; - -// Block signals for system '/CoreSubsys' -typedef struct { - B_MATLABFunction1_est_estimat_T sf_MATLABFunction1;// '/MATLAB Function1' -} B_CoreSubsys_est_estimator_k_T; - -// Block states (auto storage) for system '/CoreSubsys' -typedef struct { - real32_T uHzLowPass_states; // '/3 Hz Low Pass' - real32_T DiscreteTransferFcn_states[2];// '/Discrete Transfer Fcn' - real32_T HighPassFilter_states; // '/High Pass Filter' - DW_MATLABFunction1_est_estima_T sf_MATLABFunction1;// '/MATLAB Function1' -} DW_CoreSubsys_est_estimator_a_T; - -// Block signals for system '/MATLAB Function' -typedef struct { - real32_T y[16]; // '/MATLAB Function' -} B_MATLABFunction_est_estimato_T; - -// Block signals (auto storage) -typedef struct { - real_T tmp_data[22500]; - real32_T q1_data[22500]; - real_T tmp_data_m[10000]; - real32_T ex_compute_delta_state_an_e[13689];// '/ex_compute_delta_state_and_cov' - real32_T Assignment[13689]; // '/Assignment' - real32_T Switch1[13689]; - real32_T MatrixConcatenate2[11934]; // '/Matrix Concatenate2' - real32_T ex_of_residual_and_h_o3[11232];// '/ex_of_residual_and_h' - real32_T q1_data_c[10000]; - real32_T ex_of_residual_and_h_o7[9216];// '/ex_of_residual_and_h' - ase_cov_datatype Switch1_k[8100]; - real32_T x_data[2500]; - ase_cov_datatype Switch1_c[1890]; - real32_T MatrixConcatenate[1755]; // '/Matrix Concatenate' - real32_T of_measured_in[1600]; - real32_T of_measured[1600]; // '/compute_of_global_points' - real32_T b_x_data[1600]; - real32_T of_measured_in_data[1600]; - ase_cov_datatype Selector1[1530]; // '/Selector1' - ase_cov_datatype P_IC[1530]; // '/MATLAB Function2' - real32_T r_out[6]; // '/Merge' - real32_T H_out[702]; // '/Merge' - real32_T R_mat[36]; // '/Merge' - int32_T error_out; // '/Merge' - uint8_T SumofElements3[50]; // '/Sum of Elements3' - B_MATLABFunction_est_estimato_T sf_MATLABFunction_l0;// '/MATLAB Function' - B_MATLABFunction_est_estimato_T sf_MATLABFunction_o;// '/MATLAB Function' - B_MATLABFunction_est_estimato_T sf_MATLABFunction;// '/MATLAB Function' - B_CoreSubsys_est_estimator_k_T CoreSubsys_l[3];// '/CoreSubsys' - B_CoreSubsys_est_estimator_T CoreSubsys[3];// '/CoreSubsys' -} B_est_estimator_T; - -// Block states (auto storage) for system '' -typedef struct { - real_T UnitDelay20_DSTATE[3]; // '/Unit Delay20' - real_T UnitDelay21_DSTATE[3]; // '/Unit Delay21' - real_T UnitDelay22_DSTATE[48]; // '/Unit Delay22' - real_T UnitDelay23_DSTATE[48]; // '/Unit Delay23' - real_T UnitDelay18_DSTATE[50]; // '/Unit Delay18' - real_T UnitDelay19_DSTATE[50]; // '/Unit Delay19' - real_T UnitDelay_DSTATE_j; // '/Unit Delay' - real_T num_consecutive_rejected_meas;// '/Compute Residual and H' - ase_cov_datatype Delay_DSTATE[13689];// '/Delay' - real32_T UnitDelay1_DSTATE[4]; // '/Unit Delay1' - real32_T UnitDelay3_DSTATE[3]; // '/Unit Delay3' - real32_T UnitDelay4_DSTATE[3]; // '/Unit Delay4' - real32_T UnitDelay6_DSTATE[3]; // '/Unit Delay6' - real32_T UnitDelay7_DSTATE[3]; // '/Unit Delay7' - real32_T UnitDelay10_DSTATE[4]; // '/Unit Delay10' - real32_T UnitDelay11_DSTATE[3]; // '/Unit Delay11' - real32_T UnitDelay12_DSTATE[64]; // '/Unit Delay12' - real32_T UnitDelay13_DSTATE[48]; // '/Unit Delay13' - ase_cov_datatype UnitDelay14_DSTATE[117];// '/Unit Delay14' - real32_T UnitDelay24_DSTATE[3]; // '/Unit Delay24' - real32_T UnitDelay25_DSTATE[4]; // '/Unit Delay25' - real32_T UnitDelay26_DSTATE[3]; // '/Unit Delay26' - uint32_T UnitDelay9_DSTATE; // '/Unit Delay9' - uint32_T DelayInput1_DSTATE; // '/Delay Input1' - uint32_T DelayInput1_DSTATE_h; // '/Delay Input1' - uint32_T DelayInput1_DSTATE_o; // '/Delay Input1' - uint32_T DelayInput1_DSTATE_n; // '/Delay Input1' - uint32_T DelayInput1_DSTATE_b; // '/Delay Input1' - uint32_T DelayInput1_DSTATE_d; // '/Delay Input1' - real32_T aug_velocity[48]; // '/MATLAB Function' - real32_T aug_velocity_mag[16]; // '/MATLAB Function' - real32_T aug_omega[48]; // '/MATLAB Function' - real32_T aug_omega_mag[16]; // '/MATLAB Function' - real32_T hr_P_hr_ISS_ISS_pers[3]; // '/Compute Global positions of Handrail Features' - real32_T hr_quat_ISS2hr_pers[4]; // '/Compute Global positions of Handrail Features' - uint16_T UnitDelay15_DSTATE; // '/Unit Delay15' - uint8_T UnitDelay8_DSTATE; // '/Unit Delay8' - uint8_T UnitDelay16_DSTATE; // '/Unit Delay16' - uint8_T UnitDelay17_DSTATE; // '/Unit Delay17' - uint8_T DelayInput1_DSTATE_k; // '/Delay Input1' - uint8_T icLoad; // '/Delay' - boolean_T aug_velocity_not_empty; // '/MATLAB Function' - boolean_T aug_velocity_mag_not_empty;// '/MATLAB Function' - boolean_T aug_omega_not_empty; // '/MATLAB Function' - boolean_T aug_omega_mag_not_empty; // '/MATLAB Function' - boolean_T hr_P_hr_ISS_ISS_pers_not_empty;// '/Compute Global positions of Handrail Features' - boolean_T hr_quat_ISS2hr_pers_not_empty;// '/Compute Global positions of Handrail Features' - DW_CoreSubsys_est_estimator_a_T CoreSubsys_l[3];// '/CoreSubsys' - DW_CoreSubsys_est_estimator_T CoreSubsys[3];// '/CoreSubsys' -} DW_est_estimator_T; - -// Parameters for system: '/CoreSubsys' -struct P_CoreSubsys_est_estimator_T_ { - real32_T uHzLowPass_NumCoef; // Expression: single([0.2604]) - // Referenced by: '/3 Hz Low Pass' - - real32_T uHzLowPass_DenCoef[2]; // Expression: single([1 -0.7396]) - // Referenced by: '/3 Hz Low Pass' - - real32_T uHzLowPass_InitialStates; // Computed Parameter: uHzLowPass_InitialStates - // Referenced by: '/3 Hz Low Pass' - - real32_T DiscreteTransferFcn_InitialStat;// Computed Parameter: DiscreteTransferFcn_InitialStat - // Referenced by: '/Discrete Transfer Fcn' - -}; - -// Parameters for system: '/CoreSubsys' -struct P_CoreSubsys_est_estimator_g_T_ { - real32_T uHzLowPass_NumCoef; // Expression: single([0.2604]) - // Referenced by: '/3 Hz Low Pass' - - real32_T uHzLowPass_DenCoef[2]; // Expression: single([1 -0.7396]) - // Referenced by: '/3 Hz Low Pass' - - real32_T uHzLowPass_InitialStates; // Computed Parameter: uHzLowPass_InitialStates - // Referenced by: '/3 Hz Low Pass' - - real32_T DiscreteTransferFcn_InitialStat;// Computed Parameter: DiscreteTransferFcn_InitialStat - // Referenced by: '/Discrete Transfer Fcn' - - real32_T HighPassFilter_InitialStates;// Computed Parameter: HighPassFilter_InitialStates - // Referenced by: '/High Pass Filter' - -}; - -// Parameters for system: '/Normalize' -struct P_Normalize_est_estimator_T_ { - real_T Constant1_Value; // Expression: -1 - // Referenced by: '/Constant1' - -}; - -// Parameters (auto storage) -struct P_est_estimator_T_ { - real_T ase_minumum_resid_thresh; // Variable: ase_minumum_resid_thresh - // Referenced by: - // '/Constant1' - // '/Constant1' - - real_T ase_ts; // Variable: ase_ts - // Referenced by: - // '/Constant' - // '/Gain' - // '/Gain1' - // '/MATLAB Function' - // '/Gain3' - - real_T astrobee_fsw_step_size; // Variable: astrobee_fsw_step_size - // Referenced by: - // '/Constant2' - // '/Constant2' - - real32_T ase_accel_fixed_bias[3]; // Variable: ase_accel_fixed_bias - // Referenced by: '/Constant5' - - real32_T ase_gyro_fixed_bias[3]; // Variable: ase_gyro_fixed_bias - // Referenced by: '/Constant1' - - real32_T ase_state_ic_A_B_ISS_ISS[3];// Variable: ase_state_ic_A_B_ISS_ISS - // Referenced by: '/Unit Delay5' - - real32_T ase_state_ic_accel_bias[3]; // Variable: ase_state_ic_accel_bias - // Referenced by: '/Unit Delay6' - - real32_T ase_state_ic_cov_diag[117]; // Variable: ase_state_ic_cov_diag - // Referenced by: '/Unit Delay14' - - real32_T ase_state_ic_gyro_bias[3]; // Variable: ase_state_ic_gyro_bias - // Referenced by: '/Unit Delay3' - - real32_T ase_state_ic_ml_P_cam_ISS_ISS[3];// Variable: ase_state_ic_ml_P_cam_ISS_ISS - // Referenced by: '/Unit Delay11' - - real32_T ase_state_ic_ml_quat_ISS2cam[4];// Variable: ase_state_ic_ml_quat_ISS2cam - // Referenced by: '/Unit Delay10' - - real32_T ase_state_ic_of_P_cam_ISS_ISS[48];// Variable: ase_state_ic_of_P_cam_ISS_ISS - // Referenced by: '/Unit Delay13' - - real32_T ase_state_ic_of_quat_ISS2cam[64];// Variable: ase_state_ic_of_quat_ISS2cam - // Referenced by: '/Unit Delay12' - - real32_T fam_impeller_speeds[3]; // Variable: fam_impeller_speeds - // Referenced by: - // '/Constant' - // '/Constant' - - real32_T tun_abp_p_dockcam_imu_est[3];// Variable: tun_abp_p_dockcam_imu_est - // Referenced by: '/Constant2' - - real32_T tun_abp_p_imu_body_body[3]; // Variable: tun_abp_p_imu_body_body - // Referenced by: '/Constant5' - - real32_T tun_abp_p_navcam_imu_est[3];// Variable: tun_abp_p_navcam_imu_est - // Referenced by: - // '/MATLAB Function' - // '/Constant1' - // '/Constant1' - - real32_T tun_abp_p_perchcam_imu_est[3];// Variable: tun_abp_p_perchcam_imu_est - // Referenced by: '/Constant4' - - real32_T tun_abp_q_body2dockcam[4]; // Variable: tun_abp_q_body2dockcam - // Referenced by: - // '/Constant7' - // '/Constant1' - - real32_T tun_abp_q_body2navcam[4]; // Variable: tun_abp_q_body2navcam - // Referenced by: - // '/MATLAB Function' - // '/Constant2' - // '/Constant6' - // '/Constant' - - real32_T tun_abp_q_body2perchcam[4]; // Variable: tun_abp_q_body2perchcam - // Referenced by: - // '/Constant8' - // '/Compute Residual and H' - - real32_T tun_abp_quat_body2imu[4]; // Variable: tun_abp_quat_body2imu - // Referenced by: - // '/Constant' - // '/Constant2' - - real32_T tun_ase_Q_imu[12]; // Variable: tun_ase_Q_imu - // Referenced by: '/Constant9' - - real32_T tun_ase_acquired_ticks; // Variable: tun_ase_acquired_ticks - // Referenced by: - // '/Saturation' - // '/Constant' - - real32_T tun_ase_converged_thresh; // Variable: tun_ase_converged_thresh - // Referenced by: '/Constant' - - real32_T tun_ase_diverged_thresh; // Variable: tun_ase_diverged_thresh - // Referenced by: '/Constant' - - real32_T tun_ase_dock_r_mag; // Variable: tun_ase_dock_r_mag - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_dockcam_distortion; // Variable: tun_ase_dockcam_distortion - // Referenced by: '/Constant5' - - real32_T tun_ase_dockcam_inv_focal_length;// Variable: tun_ase_dockcam_inv_focal_length - // Referenced by: '/Constant3' - - real32_T tun_ase_gravity_accel[3]; // Variable: tun_ase_gravity_accel - // Referenced by: '/Constant3' - - real32_T tun_ase_hr_distance_r; // Variable: tun_ase_hr_distance_r - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_hr_r_mag; // Variable: tun_ase_hr_r_mag - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_mahal_distance_max; // Variable: tun_ase_mahal_distance_max - // Referenced by: - // '/ex_of_residual_and_h' - // '/Compute Residual and H' - - real32_T tun_ase_map_error; // Variable: tun_ase_map_error - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_max_accel; // Variable: tun_ase_max_accel - // Referenced by: '/diag' - - real32_T tun_ase_max_gyro; // Variable: tun_ase_max_gyro - // Referenced by: '/diag' - - real32_T tun_ase_min_ar_meas; // Variable: tun_ase_min_ar_meas - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_min_ml_meas; // Variable: tun_ase_min_ml_meas - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_ml_forward_projection_time;// Variable: tun_ase_ml_forward_projection_time - // Referenced by: - // '/Constant' - // '/Gain' - // '/Constant' - // '/Gain' - // '/Compute Residual and H' - // '/Compute Residual and H' - - real32_T tun_ase_navcam_distortion; // Variable: tun_ase_navcam_distortion - // Referenced by: - // '/ex_of_residual_and_h' - // '/Constant4' - - real32_T tun_ase_navcam_inv_focal_length;// Variable: tun_ase_navcam_inv_focal_length - // Referenced by: - // '/ex_of_residual_and_h' - // '/Constant2' - - real32_T tun_ase_of_r_mag; // Variable: tun_ase_of_r_mag - // Referenced by: '/ex_of_residual_and_h' - - real32_T tun_ase_q_saturated_accel; // Variable: tun_ase_q_saturated_accel - // Referenced by: '/diag' - - real32_T tun_ase_q_saturated_gyro; // Variable: tun_ase_q_saturated_gyro - // Referenced by: '/diag' - - real32_T tun_ase_state_ic_P_B_ISS_ISS[3];// Variable: tun_ase_state_ic_P_B_ISS_ISS - // Referenced by: '/Unit Delay7' - - real32_T tun_ase_state_ic_P_EST_ISS_ISS[3];// Variable: tun_ase_state_ic_P_EST_ISS_ISS - // Referenced by: '/Unit Delay26' - - real32_T tun_ase_state_ic_V_B_ISS_ISS[3];// Variable: tun_ase_state_ic_V_B_ISS_ISS - // Referenced by: '/Unit Delay4' - - real32_T tun_ase_state_ic_cov_diag[15];// Variable: tun_ase_state_ic_cov_diag - // Referenced by: '/Constant4' - - real32_T tun_ase_state_ic_omega_B_ISS_B[3];// Variable: tun_ase_state_ic_omega_B_ISS_B - // Referenced by: '/Unit Delay2' - - real32_T tun_ase_state_ic_quat_ISS2B[4];// Variable: tun_ase_state_ic_quat_ISS2B - // Referenced by: '/Unit Delay1' - - real32_T tun_ase_vis_r_mag; // Variable: tun_ase_vis_r_mag - // Referenced by: '/Compute Residual and H' - - real32_T tun_ase_vocam_inv_focal_length;// Variable: tun_ase_vocam_inv_focal_length - // Referenced by: '/compute_of_global_points' - - real32_T tun_aug_ic_cov; // Variable: tun_aug_ic_cov - // Referenced by: '/Gain' - - real32_T tun_grav_hp_den[2]; // Variable: tun_grav_hp_den - // Referenced by: '/High Pass Filter' - - real32_T tun_grav_hp_num[2]; // Variable: tun_grav_hp_num - // Referenced by: '/High Pass Filter' - - real32_T tun_ic_cov_pos; // Variable: tun_ic_cov_pos - // Referenced by: '/Constant3' - - real32_T tun_ic_cov_quat; // Variable: tun_ic_cov_quat - // Referenced by: '/Constant1' - - real32_T tun_max_mahal_reject_frames;// Variable: tun_max_mahal_reject_frames - // Referenced by: '/Compute Residual and H' - - uint32_T ase_aug_state_bitmask; // Variable: ase_aug_state_bitmask - // Referenced by: '/Constant' - - uint8_T ase_local_mode_docking; // Variable: ase_local_mode_docking - // Referenced by: - // '/Constant' - // '/Constant' - - uint8_T ase_local_mode_map; // Variable: ase_local_mode_map - // Referenced by: - // '/Constant' - // '/Constant' - // '/Constant' - - uint8_T ase_local_mode_perching; // Variable: ase_local_mode_perching - // Referenced by: - // '/Constant' - // '/Constant' - - uint8_T ase_state_ic_aug_state_enum; // Variable: ase_state_ic_aug_state_enum - // Referenced by: '/Unit Delay9' - - uint8_T ase_state_ic_confidence; // Variable: ase_state_ic_confidence - // Referenced by: '/Unit Delay8' - - uint8_T ase_state_ic_status; // Variable: ase_state_ic_status - // Referenced by: '/Unit Delay15' - - uint8_T ase_status_acquiring; // Variable: ase_status_acquiring - // Referenced by: - // '/Constant4' - // '/Constant' - // '/Constant' - - uint8_T ase_status_converged; // Variable: ase_status_converged - // Referenced by: - // '/Constant1' - // '/Constant' - // '/Constant' - // '/Constant' - - uint8_T ase_status_diverged; // Variable: ase_status_diverged - // Referenced by: - // '/Constant' - // '/Constant' - - uint8_T fam_impeller_speeds_cnt; // Variable: fam_impeller_speeds_cnt - // Referenced by: '/Saturation' - - uint8_T tun_ase_enable_of; // Variable: tun_ase_enable_of - // Referenced by: - // '/Constant' - // '/Constant' - - uint8_T tun_ase_gravity_removal; // Variable: tun_ase_gravity_removal - // Referenced by: '/Constant4' - - uint8_T tun_grav_hp_enable_f; // Variable: tun_grav_hp_enable_f - // Referenced by: '/Constant1' - - uint32_T FixPtBitwiseOperator3_BitMask;// Mask Parameter: FixPtBitwiseOperator3_BitMask - // Referenced by: '/FixPt Bitwise Operator3' - - uint32_T BitwiseOperator2_BitMask; // Mask Parameter: BitwiseOperator2_BitMask - // Referenced by: '/Bitwise Operator2' - - uint32_T BitwiseOperator_BitMask; // Mask Parameter: BitwiseOperator_BitMask - // Referenced by: '/Bitwise Operator' - - uint32_T DetectChange6_vinit; // Mask Parameter: DetectChange6_vinit - // Referenced by: '/Delay Input1' - - uint32_T DetectChange7_vinit; // Mask Parameter: DetectChange7_vinit - // Referenced by: '/Delay Input1' - - uint32_T DetectChange_vinit; // Mask Parameter: DetectChange_vinit - // Referenced by: '/Delay Input1' - - uint32_T DetectChange1_vinit; // Mask Parameter: DetectChange1_vinit - // Referenced by: '/Delay Input1' - - uint32_T DetectChange4_vinit; // Mask Parameter: DetectChange4_vinit - // Referenced by: '/Delay Input1' - - uint32_T DetectChange5_vinit; // Mask Parameter: DetectChange5_vinit - // Referenced by: '/Delay Input1' - - uint16_T BitwiseOperator_BitMask_c; // Mask Parameter: BitwiseOperator_BitMask_c - // Referenced by: '/Bitwise Operator' - - uint16_T BitwiseOperator1_BitMask; // Mask Parameter: BitwiseOperator1_BitMask - // Referenced by: '/Bitwise Operator1' - - uint16_T BitwiseOperator_BitMask_l; // Mask Parameter: BitwiseOperator_BitMask_l - // Referenced by: '/Bitwise Operator' - - uint8_T CompareToConstant7_const; // Mask Parameter: CompareToConstant7_const - // Referenced by: '/Constant' - - uint8_T CompareToConstant_const; // Mask Parameter: CompareToConstant_const - // Referenced by: '/Constant' - - uint8_T DetectChange_vinit_i; // Mask Parameter: DetectChange_vinit_i - // Referenced by: '/Delay Input1' - - kfl_msg State_out_Y0; // Computed Parameter: State_out_Y0 - // Referenced by: '/State_out' - - kfl_msg UnitDelay_InitialCondition; // Computed Parameter: UnitDelay_InitialCondition - // Referenced by: '/Unit Delay' - - kfl_msg State_out_Y0_h; // Computed Parameter: State_out_Y0_h - // Referenced by: '/State_out' - - kfl_msg UnitDelay_InitialCondition_p;// Computed Parameter: UnitDelay_InitialCondition_p - // Referenced by: '/Unit Delay' - - real_T Constant_Value[50]; // Expression: zeros(ase_ml_num_features, 1) - // Referenced by: '/Constant' - - real_T Merge_6_InitialOutput; // Computed Parameter: Merge_6_InitialOutput - // Referenced by: '/Merge' - - real_T Constant3_Value[50]; // Expression: zeros(ase_of_num_features,1) - // Referenced by: '/Constant3' - - real_T Constant2_Value[50]; // Expression: zeros(ase_ml_num_features,1) - // Referenced by: '/Constant2' - - real_T Constant2_Value_j[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant_Value_p[3]; // Expression: [0 0 0] - // Referenced by: '/Constant' - - real_T Constant1_Value; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_l; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_l[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_p; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_m[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_m; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_m; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_b[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_n; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_d; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant3_Value_pv; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_jy[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant3_Value_pi; // Expression: 0 - // Referenced by: '/Constant3' - - real_T UnitDelay20_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay20' - - real_T UnitDelay21_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay21' - - real_T UnitDelay22_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay22' - - real_T UnitDelay23_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay23' - - real_T UnitDelay18_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay18' - - real_T UnitDelay19_InitialCondition; // Expression: 0 - // Referenced by: '/Unit Delay19' - - real_T Constant_Value_l[3]; // Expression: [0 0 0] - // Referenced by: '/Constant' - - real_T Constant2_Value_a[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_mb; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_k; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_ja[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_c; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_c; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_bl[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_l; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_o; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_p[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_k; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_i; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Constant2_Value_bd[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_e; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_kc; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Merge2_1_InitialOutput; // Computed Parameter: Merge2_1_InitialOutput - // Referenced by: '/Merge2' - - real_T Merge2_2_InitialOutput; // Computed Parameter: Merge2_2_InitialOutput - // Referenced by: '/Merge2' - - real_T Merge2_3_InitialOutput; // Computed Parameter: Merge2_3_InitialOutput - // Referenced by: '/Merge2' - - real_T Merge2_4_InitialOutput; // Computed Parameter: Merge2_4_InitialOutput - // Referenced by: '/Merge2' - - real_T Constant2_Value_h; // Expression: 0 - // Referenced by: '/Constant2' - - real_T UnitDelay_InitialCondition_h; // Expression: 0 - // Referenced by: '/Unit Delay' - - real_T Constant3_Value_p3; // Expression: 1 - // Referenced by: '/Constant3' - - real_T Saturation_LowerSat; // Expression: 0 - // Referenced by: '/Saturation' - - real_T Constant2_Value_d[9]; // Expression: zeros(9,1) - // Referenced by: '/Constant2' - - real_T Constant1_Value_ec; // Expression: 1 - // Referenced by: '/Constant1' - - real_T Constant3_Value_ol; // Expression: 0 - // Referenced by: '/Constant3' - - real_T Merge2_1_InitialOutput_c; // Computed Parameter: Merge2_1_InitialOutput_c - // Referenced by: '/Merge2' - - real_T Merge2_2_InitialOutput_c; // Computed Parameter: Merge2_2_InitialOutput_c - // Referenced by: '/Merge2' - - real_T Merge2_3_InitialOutput_c; // Computed Parameter: Merge2_3_InitialOutput_c - // Referenced by: '/Merge2' - - real_T Merge2_4_InitialOutput_c; // Computed Parameter: Merge2_4_InitialOutput_c - // Referenced by: '/Merge2' - - real32_T P_out_Y0; // Computed Parameter: P_out_Y0 - // Referenced by: '/P_out' - - real32_T UnitDelay1_InitialCondition;// Computed Parameter: UnitDelay1_InitialCondition - // Referenced by: '/Unit Delay1' - - real32_T Merge_1_InitialOutput; // Computed Parameter: Merge_1_InitialOutput - // Referenced by: '/Merge' - - real32_T Merge_3_InitialOutput; // Computed Parameter: Merge_3_InitialOutput - // Referenced by: '/Merge' - - real32_T Merge_4_InitialOutput; // Computed Parameter: Merge_4_InitialOutput - // Referenced by: '/Merge' - - real32_T Merge_7_InitialOutput; // Computed Parameter: Merge_7_InitialOutput - // Referenced by: '/Merge' - - real32_T Merge_8_InitialOutput; // Computed Parameter: Merge_8_InitialOutput - // Referenced by: '/Merge' - - real32_T UnitDelay2_InitialCondition;// Computed Parameter: UnitDelay2_InitialCondition - // Referenced by: '/Unit Delay2' - - real32_T P_out_Y0_e; // Computed Parameter: P_out_Y0_e - // Referenced by: '/P_out' - - real32_T UnitDelay2_InitialCondition_f;// Computed Parameter: UnitDelay2_InitialCondition_f - // Referenced by: '/Unit Delay2' - - real32_T UnitDelay1_InitialCondition_m;// Computed Parameter: UnitDelay1_InitialCondition_m - // Referenced by: '/Unit Delay1' - - real32_T Gain2_Gain; // Computed Parameter: Gain2_Gain - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_e; // Computed Parameter: Gain2_Gain_e - // Referenced by: '/Gain2' - - real32_T Gain1_Gain; // Computed Parameter: Gain1_Gain - // Referenced by: '/Gain1' - - real32_T Gain_Gain; // Computed Parameter: Gain_Gain - // Referenced by: '/Gain' - - real32_T Gain1_Gain_d; // Computed Parameter: Gain1_Gain_d - // Referenced by: '/Gain1' - - real32_T Gain_Gain_l; // Computed Parameter: Gain_Gain_l - // Referenced by: '/Gain' - - real32_T Constant1_Value_cn; // Computed Parameter: Constant1_Value_cn - // Referenced by: '/Constant1' - - real32_T Constant3_Value_f; // Computed Parameter: Constant3_Value_f - // Referenced by: '/Constant3' - - real32_T Constant3_Value_cc; // Computed Parameter: Constant3_Value_cc - // Referenced by: '/Constant3' - - real32_T Gain_Gain_h; // Computed Parameter: Gain_Gain_h - // Referenced by: '/Gain' - - real32_T Gain1_Gain_b; // Computed Parameter: Gain1_Gain_b - // Referenced by: '/Gain1' - - real32_T Constant2_Value_pd; // Computed Parameter: Constant2_Value_pd - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_h; // Computed Parameter: Gain2_Gain_h - // Referenced by: '/Gain2' - - real32_T Gain3_Gain; // Computed Parameter: Gain3_Gain - // Referenced by: '/Gain3' - - real32_T Gain4_Gain; // Computed Parameter: Gain4_Gain - // Referenced by: '/Gain4' - - real32_T Constant1_Value_j; // Computed Parameter: Constant1_Value_j - // Referenced by: '/Constant1' - - real32_T Gain5_Gain; // Computed Parameter: Gain5_Gain - // Referenced by: '/Gain5' - - real32_T Constant_Value_f; // Computed Parameter: Constant_Value_f - // Referenced by: '/Constant' - - real32_T Constant3_Value_iq; // Computed Parameter: Constant3_Value_iq - // Referenced by: '/Constant3' - - real32_T Gain1_Gain_j; // Computed Parameter: Gain1_Gain_j - // Referenced by: '/Gain1' - - real32_T Gain_Gain_n; // Computed Parameter: Gain_Gain_n - // Referenced by: '/Gain' - - real32_T Gain1_Gain_f; // Computed Parameter: Gain1_Gain_f - // Referenced by: '/Gain1' - - real32_T Constant2_Value_n; // Computed Parameter: Constant2_Value_n - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_a; // Computed Parameter: Gain2_Gain_a - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_l; // Computed Parameter: Gain3_Gain_l - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_o; // Computed Parameter: Gain4_Gain_o - // Referenced by: '/Gain4' - - real32_T Constant1_Value_nk; // Computed Parameter: Constant1_Value_nk - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_j; // Computed Parameter: Gain5_Gain_j - // Referenced by: '/Gain5' - - real32_T Constant_Value_j; // Computed Parameter: Constant_Value_j - // Referenced by: '/Constant' - - real32_T Constant2_Value_p3; // Computed Parameter: Constant2_Value_p3 - // Referenced by: '/Constant2' - - real32_T Gain_Gain_a; // Computed Parameter: Gain_Gain_a - // Referenced by: '/Gain' - - real32_T Gain1_Gain_e; // Computed Parameter: Gain1_Gain_e - // Referenced by: '/Gain1' - - real32_T Gain_Gain_f; // Computed Parameter: Gain_Gain_f - // Referenced by: '/Gain' - - real32_T Gain1_Gain_k; // Computed Parameter: Gain1_Gain_k - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_i; // Computed Parameter: Gain2_Gain_i - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_p; // Computed Parameter: Gain2_Gain_p - // Referenced by: '/Gain2' - - real32_T Gain_Gain_g; // Computed Parameter: Gain_Gain_g - // Referenced by: '/Gain' - - real32_T Gain1_Gain_jc; // Computed Parameter: Gain1_Gain_jc - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_o; // Computed Parameter: Gain2_Gain_o - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_i; // Computed Parameter: Gain1_Gain_i - // Referenced by: '/Gain1' - - real32_T Constant1_Value_a; // Computed Parameter: Constant1_Value_a - // Referenced by: '/Constant1' - - real32_T Constant3_Value_da; // Computed Parameter: Constant3_Value_da - // Referenced by: '/Constant3' - - real32_T Constant3_Value_oe; // Computed Parameter: Constant3_Value_oe - // Referenced by: '/Constant3' - - real32_T Constant5_Value[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant5' - - real32_T Gain_Gain_d; // Computed Parameter: Gain_Gain_d - // Referenced by: '/Gain' - - real32_T Gain1_Gain_n; // Computed Parameter: Gain1_Gain_n - // Referenced by: '/Gain1' - - real32_T Constant2_Value_m2; // Computed Parameter: Constant2_Value_m2 - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_k; // Computed Parameter: Gain2_Gain_k - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_f; // Computed Parameter: Gain3_Gain_f - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_b; // Computed Parameter: Gain4_Gain_b - // Referenced by: '/Gain4' - - real32_T Constant1_Value_i; // Computed Parameter: Constant1_Value_i - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_f; // Computed Parameter: Gain5_Gain_f - // Referenced by: '/Gain5' - - real32_T Constant_Value_m; // Computed Parameter: Constant_Value_m - // Referenced by: '/Constant' - - real32_T Constant3_Value_op; // Computed Parameter: Constant3_Value_op - // Referenced by: '/Constant3' - - real32_T Gain1_Gain_o; // Computed Parameter: Gain1_Gain_o - // Referenced by: '/Gain1' - - real32_T Gain_Gain_ng; // Computed Parameter: Gain_Gain_ng - // Referenced by: '/Gain' - - real32_T Gain1_Gain_a; // Computed Parameter: Gain1_Gain_a - // Referenced by: '/Gain1' - - real32_T Constant2_Value_o; // Computed Parameter: Constant2_Value_o - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_b; // Computed Parameter: Gain2_Gain_b - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_o; // Computed Parameter: Gain3_Gain_o - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_i; // Computed Parameter: Gain4_Gain_i - // Referenced by: '/Gain4' - - real32_T Constant1_Value_b; // Computed Parameter: Constant1_Value_b - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_c; // Computed Parameter: Gain5_Gain_c - // Referenced by: '/Gain5' - - real32_T Constant_Value_px; // Computed Parameter: Constant_Value_px - // Referenced by: '/Constant' - - real32_T Constant2_Value_mp; // Computed Parameter: Constant2_Value_mp - // Referenced by: '/Constant2' - - real32_T Gain_Gain_c; // Computed Parameter: Gain_Gain_c - // Referenced by: '/Gain' - - real32_T Gain1_Gain_nq; // Computed Parameter: Gain1_Gain_nq - // Referenced by: '/Gain1' - - real32_T Gain_Gain_fr; // Computed Parameter: Gain_Gain_fr - // Referenced by: '/Gain' - - real32_T Gain1_Gain_c; // Computed Parameter: Gain1_Gain_c - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_hc; // Computed Parameter: Gain2_Gain_hc - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_g; // Computed Parameter: Gain2_Gain_g - // Referenced by: '/Gain2' - - ase_cov_datatype Constant3_Value_l2[90];// Computed Parameter: Constant3_Value_l2 - // Referenced by: '/Constant3' - - real32_T Gain_Gain_cu; // Computed Parameter: Gain_Gain_cu - // Referenced by: '/Gain' - - real32_T Gain1_Gain_l; // Computed Parameter: Gain1_Gain_l - // Referenced by: '/Gain1' - - real32_T Gain_Gain_ne; // Computed Parameter: Gain_Gain_ne - // Referenced by: '/Gain' - - real32_T Gain1_Gain_j4; // Computed Parameter: Gain1_Gain_j4 - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_n; // Computed Parameter: Gain2_Gain_n - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_p0; // Computed Parameter: Gain2_Gain_p0 - // Referenced by: '/Gain2' - - real32_T Gain_Gain_fx; // Computed Parameter: Gain_Gain_fx - // Referenced by: '/Gain' - - real32_T Gain1_Gain_ik; // Computed Parameter: Gain1_Gain_ik - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_m; // Computed Parameter: Gain2_Gain_m - // Referenced by: '/Gain2' - - real32_T Gain_Gain_j; // Computed Parameter: Gain_Gain_j - // Referenced by: '/Gain' - - real32_T Gain1_Gain_g; // Computed Parameter: Gain1_Gain_g - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_eo; // Computed Parameter: Gain2_Gain_eo - // Referenced by: '/Gain2' - - real32_T Gain1_Gain_ei; // Computed Parameter: Gain1_Gain_ei - // Referenced by: '/Gain1' - - real32_T Constant2_Value_ad[13689]; // Expression: zeros(ase_total_num_states^2,1, 'single') - // Referenced by: '/Constant2' - - real32_T Constant5_Value_a[102]; // Expression: ones(1, ase_total_num_states - size(tun_ase_state_ic_cov_diag, 2), 'single') - // Referenced by: '/Constant5' - - real32_T UnitDelay24_InitialCondition;// Computed Parameter: UnitDelay24_InitialCondition - // Referenced by: '/Unit Delay24' - - real32_T UnitDelay25_InitialCondition;// Computed Parameter: UnitDelay25_InitialCondition - // Referenced by: '/Unit Delay25' - - real32_T Constant1_Value_i3; // Computed Parameter: Constant1_Value_i3 - // Referenced by: '/Constant1' - - real32_T Constant3_Value_dc; // Computed Parameter: Constant3_Value_dc - // Referenced by: '/Constant3' - - real32_T Constant3_Value_c4; // Computed Parameter: Constant3_Value_c4 - // Referenced by: '/Constant3' - - real32_T Gain_Gain_n1; // Computed Parameter: Gain_Gain_n1 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_au; // Computed Parameter: Gain1_Gain_au - // Referenced by: '/Gain1' - - real32_T Constant2_Value_am; // Computed Parameter: Constant2_Value_am - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_a5; // Computed Parameter: Gain2_Gain_a5 - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_m; // Computed Parameter: Gain3_Gain_m - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_h; // Computed Parameter: Gain4_Gain_h - // Referenced by: '/Gain4' - - real32_T Constant1_Value_p; // Computed Parameter: Constant1_Value_p - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_a; // Computed Parameter: Gain5_Gain_a - // Referenced by: '/Gain5' - - real32_T Constant_Value_d; // Computed Parameter: Constant_Value_d - // Referenced by: '/Constant' - - real32_T Constant3_Value_dg; // Computed Parameter: Constant3_Value_dg - // Referenced by: '/Constant3' - - real32_T Gain_Gain_g0; // Computed Parameter: Gain_Gain_g0 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_jx; // Computed Parameter: Gain1_Gain_jx - // Referenced by: '/Gain1' - - real32_T Gain_Gain_e; // Computed Parameter: Gain_Gain_e - // Referenced by: '/Gain' - - real32_T Gain1_Gain_nf; // Computed Parameter: Gain1_Gain_nf - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_h5; // Computed Parameter: Gain2_Gain_h5 - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_gg; // Computed Parameter: Gain2_Gain_gg - // Referenced by: '/Gain2' - - real32_T Gain_Gain_co; // Computed Parameter: Gain_Gain_co - // Referenced by: '/Gain' - - real32_T Gain1_Gain_cz; // Computed Parameter: Gain1_Gain_cz - // Referenced by: '/Gain1' - - real32_T Constant2_Value_c; // Computed Parameter: Constant2_Value_c - // Referenced by: '/Constant2' - - real32_T Gain2_Gain_g4; // Computed Parameter: Gain2_Gain_g4 - // Referenced by: '/Gain2' - - real32_T Gain3_Gain_fu; // Computed Parameter: Gain3_Gain_fu - // Referenced by: '/Gain3' - - real32_T Gain4_Gain_g; // Computed Parameter: Gain4_Gain_g - // Referenced by: '/Gain4' - - real32_T Constant1_Value_o; // Computed Parameter: Constant1_Value_o - // Referenced by: '/Constant1' - - real32_T Gain5_Gain_jr; // Computed Parameter: Gain5_Gain_jr - // Referenced by: '/Gain5' - - real32_T Constant_Value_lw; // Computed Parameter: Constant_Value_lw - // Referenced by: '/Constant' - - real32_T Constant2_Value_j0; // Computed Parameter: Constant2_Value_j0 - // Referenced by: '/Constant2' - - real32_T Gain_Gain_c0; // Computed Parameter: Gain_Gain_c0 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_il; // Computed Parameter: Gain1_Gain_il - // Referenced by: '/Gain1' - - real32_T Gain_Gain_lj; // Computed Parameter: Gain_Gain_lj - // Referenced by: '/Gain' - - real32_T Gain1_Gain_lb; // Computed Parameter: Gain1_Gain_lb - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_m4; // Computed Parameter: Gain2_Gain_m4 - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_j; // Computed Parameter: Gain2_Gain_j - // Referenced by: '/Gain2' - - real32_T Gain_Gain_p; // Computed Parameter: Gain_Gain_p - // Referenced by: '/Gain' - - real32_T Gain1_Gain_fe; // Computed Parameter: Gain1_Gain_fe - // Referenced by: '/Gain1' - - real32_T Gain_Gain_az; // Computed Parameter: Gain_Gain_az - // Referenced by: '/Gain' - - real32_T Gain1_Gain_ln; // Computed Parameter: Gain1_Gain_ln - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_c; // Computed Parameter: Gain2_Gain_c - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_i5; // Computed Parameter: Gain2_Gain_i5 - // Referenced by: '/Gain2' - - real32_T Constant6_Value[3]; // Expression: single([0 0 0]) - // Referenced by: '/Constant6' - - ase_cov_datatype Constant5_Value_p[180];// Computed Parameter: Constant5_Value_p - // Referenced by: '/Constant5' - - real32_T Gain_Gain_fm; // Computed Parameter: Gain_Gain_fm - // Referenced by: '/Gain' - - real32_T Gain1_Gain_l4; // Computed Parameter: Gain1_Gain_l4 - // Referenced by: '/Gain1' - - real32_T Gain_Gain_k; // Computed Parameter: Gain_Gain_k - // Referenced by: '/Gain' - - real32_T Gain1_Gain_j5; // Computed Parameter: Gain1_Gain_j5 - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_i5w; // Computed Parameter: Gain2_Gain_i5w - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_kj; // Computed Parameter: Gain2_Gain_kj - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_cx; // Computed Parameter: Gain2_Gain_cx - // Referenced by: '/Gain2' - - real32_T Gain_Gain_h3; // Computed Parameter: Gain_Gain_h3 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_jb; // Computed Parameter: Gain1_Gain_jb - // Referenced by: '/Gain1' - - real32_T Gain_Gain_j2; // Computed Parameter: Gain_Gain_j2 - // Referenced by: '/Gain' - - real32_T Gain1_Gain_jp; // Computed Parameter: Gain1_Gain_jp - // Referenced by: '/Gain1' - - real32_T Gain2_Gain_ko; // Computed Parameter: Gain2_Gain_ko - // Referenced by: '/Gain2' - - real32_T Gain2_Gain_a2; // Computed Parameter: Gain2_Gain_a2 - // Referenced by: '/Gain2' - - int32_T Switch3_Threshold; // Computed Parameter: Switch3_Threshold - // Referenced by: '/Switch3' - - int32_T Switch2_Threshold; // Computed Parameter: Switch2_Threshold - // Referenced by: '/Switch2' - - int32_T Merge_2_InitialOutput; // Computed Parameter: Merge_2_InitialOutput - // Referenced by: '/Merge' - - int32_T Switch2_Threshold_b; // Computed Parameter: Switch2_Threshold_b - // Referenced by: '/Switch2' - - int32_T Switch3_Threshold_d; // Computed Parameter: Switch3_Threshold_d - // Referenced by: '/Switch3' - - uint32_T Constant_Value_h; // Expression: uint32(1) - // Referenced by: '/Constant' - - uint32_T Delay_DelayLength; // Computed Parameter: Delay_DelayLength - // Referenced by: '/Delay' - - uint32_T BitwiseOperator1_BitMask_l; // Expression: BitMask - // Referenced by: '/Bitwise Operator1' - - uint16_T Constant_Value_ln; // Expression: uint16(1) - // Referenced by: '/Constant' - - uint16_T Constant_Value_c; // Expression: uint16(2) - // Referenced by: '/Constant' - - uint16_T Constant_Value_o; // Computed Parameter: Constant_Value_o - // Referenced by: '/Constant' - - uint8_T Merge_5_InitialOutput; // Computed Parameter: Merge_5_InitialOutput - // Referenced by: '/Merge' - - uint8_T Constant2_Value_n4; // Expression: uint8(0) - // Referenced by: '/Constant2' - - uint8_T Constant_Value_dd; // Expression: uint8(0) - // Referenced by: '/Constant' - - uint8_T Constant_Value_oa; // Expression: uint8(0) - // Referenced by: '/Constant' - - uint8_T Constant1_Value_f; // Expression: uint8(0) - // Referenced by: '/Constant1' - - uint8_T Out1_Y0; // Computed Parameter: Out1_Y0 - // Referenced by: '/Out1' - - uint8_T UnitDelay16_InitialCondition;// Computed Parameter: UnitDelay16_InitialCondition - // Referenced by: '/Unit Delay16' - - uint8_T UnitDelay17_InitialCondition;// Computed Parameter: UnitDelay17_InitialCondition - // Referenced by: '/Unit Delay17' - - uint8_T Constant_Value_oq; // Expression: const - // Referenced by: '/Constant' - - uint8_T Saturation_LowerSat_n; // Computed Parameter: Saturation_LowerSat_n - // Referenced by: '/Saturation' - - uint8_T Switch_Threshold; // Computed Parameter: Switch_Threshold - // Referenced by: '/Switch' - - uint8_T Constant_Value_lf; // Expression: const - // Referenced by: '/Constant' - - uint8_T Constant_Value_js; // Expression: const - // Referenced by: '/Constant' - - uint8_T Constant_Value_e; // Expression: const - // Referenced by: '/Constant' - - P_Normalize_est_estimator_T Normalize_i;// '/Normalize' - P_Normalize_est_estimator_T Normalize_h;// '/Normalize' - P_Normalize_est_estimator_T Normalize;// '/Normalize' - P_CoreSubsys_est_estimator_g_T CoreSubsys_l;// '/CoreSubsys' - P_CoreSubsys_est_estimator_T CoreSubsys;// '/CoreSubsys' -}; - -// Real-time Model Data Structure -struct tag_RTM_est_estimator_T { - const char_T * volatile errorStatus; - B_est_estimator_T *blockIO; - P_est_estimator_T *defaultParam; - boolean_T paramIsMalloced; - DW_est_estimator_T *dwork; -}; - -#ifdef __cplusplus - -extern "C" { - -#endif - -#ifdef __cplusplus - -} -#endif - -// External data declarations for dependent source files -extern const cvs_landmark_msg est_estimator_rtZcvs_landmark_msg;// cvs_landmark_msg ground -extern const cvs_registration_pulse est_estimator_rtZcvs_registration_pulse;// cvs_registration_pulse ground -extern const cvs_optical_flow_msg est_estimator_rtZcvs_optical_flow_msg;// cvs_optical_flow_msg ground -extern const cvs_handrail_msg est_estimator_rtZcvs_handrail_msg;// cvs_handrail_msg ground -extern const imu_msg est_estimator_rtZimu_msg;// imu_msg ground -extern const cmc_msg est_estimator_rtZcmc_msg;// cmc_msg ground -extern const kfl_msg est_estimator_rtZkfl_msg;// kfl_msg ground - -#ifdef __cplusplus - -extern "C" { - -#endif - - extern const char *RT_MEMORY_ALLOCATION_ERROR; - -#ifdef __cplusplus - -} -#endif - -extern P_est_estimator_T est_estimator_P;// parameters - -#ifdef __cplusplus - -extern "C" { - -#endif - - // Model entry point functions - extern RT_MODEL_est_estimator_T *est_estimator(cvs_landmark_msg - *est_estimator_U_landmark_msg, cvs_registration_pulse - *est_estimator_U_VisionRegistration, cvs_optical_flow_msg - *est_estimator_U_cvs_optical_flow_msg_n, cvs_handrail_msg - *est_estimator_U_handrail_msg, imu_msg *est_estimator_U_imu_msg_c, cmc_msg - *est_estimator_U_cmc_msg_o, real32_T est_estimator_U_Q_ISS2B[4], kfl_msg - *est_estimator_Y_kfl_msg_h, ase_cov_datatype est_estimator_Y_P_out[13689]); - extern void est_estimator_initialize(RT_MODEL_est_estimator_T *const - est_estimator_M, cvs_landmark_msg *est_estimator_U_landmark_msg, - cvs_registration_pulse *est_estimator_U_VisionRegistration, - cvs_optical_flow_msg *est_estimator_U_cvs_optical_flow_msg_n, - cvs_handrail_msg *est_estimator_U_handrail_msg, imu_msg - *est_estimator_U_imu_msg_c, cmc_msg *est_estimator_U_cmc_msg_o, real32_T - est_estimator_U_Q_ISS2B[4], kfl_msg *est_estimator_Y_kfl_msg_h, - ase_cov_datatype est_estimator_Y_P_out[13689]); - extern void est_estimator_step(RT_MODEL_est_estimator_T *const est_estimator_M, - cvs_landmark_msg *est_estimator_U_landmark_msg, cvs_registration_pulse - *est_estimator_U_VisionRegistration, cvs_optical_flow_msg - *est_estimator_U_cvs_optical_flow_msg_n, cvs_handrail_msg - *est_estimator_U_handrail_msg, imu_msg *est_estimator_U_imu_msg_c, cmc_msg - *est_estimator_U_cmc_msg_o, real32_T est_estimator_U_Q_ISS2B[4], kfl_msg - *est_estimator_Y_kfl_msg_h, ase_cov_datatype est_estimator_Y_P_out[13689]); - extern void est_estimator_terminate(RT_MODEL_est_estimator_T * est_estimator_M); - -#ifdef __cplusplus - -} -#endif - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Note that this particular code originates from a subsystem build, -// and has its own system numbers different from the parent model. -// Refer to the system hierarchy for this subsystem below, and use the -// MATLAB hilite_system command to trace the generated code back -// to the parent model. For example, -// -// hilite_system('astrobee/fsw_lib/est_estimator') - opens subsystem astrobee/fsw_lib/est_estimator -// hilite_system('astrobee/fsw_lib/est_estimator/Kp') - opens and selects block Kp -// -// Here is the system hierarchy for this model -// -// '' : 'astrobee/fsw_lib' -// '' : 'astrobee/fsw_lib/est_estimator' -// '' : 'astrobee/fsw_lib/est_estimator/Delay - Initial Conditions' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep' -// '' : 'astrobee/fsw_lib/est_estimator/predictor' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/If Action Subsystem1' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/ML_correector' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/update_validity_check' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/HR_Compute_Residual_and_H1' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/ML_Compute_Residual_and_H' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/HR_Compute_Residual_and_H1/Compute Global positions of Handrail Features' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/HR_Compute_Residual_and_H1/Compute Residual and H' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/ML_Compute_Residual_and_H/Compare To Constant' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/ML_Compute_Residual_and_H/Compare To Constant2' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/Compute Residual and H matrix/ML_Compute_Residual_and_H/Compute Residual and H' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Absolute_Update/ML Update/ML_correector/apply_delta_state' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/Compute_DeltaState_and_Covariance' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/Simulink Compute H and R' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/apply_delta_state' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/compute_of_global_points' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/Simulink Compute H and R/compress_of_residual_and_h' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/Optical_Flow_Update/OF Update/Simulink Compute H and R/compute_of_residual_and_h' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant1' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant2' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant3' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant4' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant5' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant6' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant7' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Compare To Constant8' -// '' : 'astrobee/fsw_lib/est_estimator/camera_update/vision_preprocessing/Enabled Row-Wise SUM' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/kfl_system_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/Detect Change2' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/Detect Change3' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/filter' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/filter_with_HP_filter' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/filter/MATLAB Function1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/filter_with_HP_filter/MATLAB Function1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_A2B/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/imu_prep/rotate_vec_B2A1/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/kfl_system_prep/Bit Clear' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/kfl_system_prep/Detect Change' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/kfl_system_prep/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/kfl_system_prep/dummy_subsystem' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/handrail_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/landmark_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/optical_flow_prep' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/handrail_prep/Detect Change6' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/handrail_prep/Detect Change7' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/landmark_prep/Detect Change' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/landmark_prep/Detect Change1' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/optical_flow_prep/Detect Change4' -// '' : 'astrobee/fsw_lib/est_estimator/filter_prep/vision_system_prep/optical_flow_prep/Detect Change5' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant1' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant2' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant3' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant4' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant5' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant6' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/Compare To Constant7' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/output_prep/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Filter_10hz_1order' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/MATLAB Function2' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/diag' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/Covariance Propogation/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/create_omega_matrix' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/create_omega_matrix1' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/predictor/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/vector_magnitude' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/Compare To Constant' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/Quaternion_Multiplication' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/create_omega_matrix' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/create_omega_matrix1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/Optical Flow Registration Manager/If Action Subsystem2/single_step_zero_order_propogator/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/vector_magnitude' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/Compare To Constant1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/Compare To Constant3' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/Compare To Constant5' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/Compare To Zero' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem4' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/Quaternion_Multiplication' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/Quaternion_Multiplication/Quaternion Xi/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/MATLAB Function' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/create_omega_matrix' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/create_omega_matrix1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/Normalize/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/No-op' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/Normalize' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/first_order_quaternion_propogation/quat_normalize_and_enforce_positive_scalar/vector_normalize/vector_magnitude' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/Data Type Conversion Inherited1' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/rotate_vec_B2A/quaternion_to_DCM/skew_symetric_matrix_operator/Data Type Conversion Inherited' -// '' : 'astrobee/fsw_lib/est_estimator/state_manager/absolute_localizaton_registration_manager/If Action Subsystem2/skew_symetric_matrix_operator/Data Type Conversion Inherited' - -#endif // RTW_HEADER_est_estimator_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_data.cpp b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_data.cpp deleted file mode 100644 index 2a786a1bd7..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_data.cpp +++ /dev/null @@ -1,1943 +0,0 @@ -// -// File: est_estimator_data.cpp -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "est_estimator.h" -#include "est_estimator_private.h" - -// Block parameters (auto storage) -P_est_estimator_T est_estimator_P = { - 0.01, - 0.016, - 0.016, - - { -0.0F, -0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F }, - - { 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F - }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - - { 209.439514F, 261.799408F, 293.215332F }, - - { -0.1308F, -0.0723F, -0.0158F }, - - { 0.0247F, 0.0183F, 0.0094F }, - - { 0.093F, -0.0605F, -0.092F }, - - { -0.1578F, 0.0326F, -0.026F }, - - { 0.5F, -0.5F, -0.5F, 0.5F }, - - { 0.5F, 0.5F, 0.5F, 0.5F }, - - { 0.0F, -0.707107F, 0.0F, 0.707107F }, - - { 0.0F, 0.0F, 0.707107F, 0.707107F }, - - { 5.9705E-8F, 5.9705E-8F, 5.9705E-8F, 3.04617E-18F, 3.04617E-18F, 3.04617E-18F, - 1.0E-8F, 1.0E-8F, 1.0E-8F, 1.0E-10F, 1.0E-10F, 1.0E-10F }, - 6250.0F, - 0.01F, - 0.0225F, - 2.0F, - 0.998468F, - 0.00120481F, - - { 0.0F, 0.0F, 9.80665F }, - 0.01F, - 0.5F, - 20.0F, - 0.02F, - 29.43F, - 2.61799F, - 4.0F, - 15.0F, - 0.0333333F, - 0.998468F, - 0.00163652F, - 1.0F, - 1.0F, - 0.00121847F, - - { 0.0F, 0.0F, -0.7F }, - - { 0.0247F, 0.0183F, -0.6906F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0001F, 0.0001F, 0.0001F, 2.0E-11F, 2.0E-11F, 2.0E-11F, 0.0001F, 0.0001F, - 0.0001F, 1.0E-9F, 1.0E-9F, 1.0E-9F, 0.001F, 0.001F, 0.001F }, - - { 0.0F, 0.0F, 0.0F }, - - { 0.0F, 0.0F, 0.0F, 1.0F }, - 4.0F, - 0.00163652F, - 1.0E-5F, - - { 2.16F, -1.84F }, - - { 2.0F, -2.0F }, - 0.001F, - 0.0001F, - 200000.0F, - 131070U, - 1U, - 0U, - 2U, - 0U, - 0U, - 1U, - 1U, - 0U, - 2U, - 3U, - 1U, - 0U, - 0U, - 1U, - 1U, - 1U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 65532U, - 1U, - 1U, - 3U, - 0U, - 0U, - - { - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // quat_ISS2B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // gyro_bias - - { - 0.0F, 0.0F, 0.0F } - , // V_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // accel_bias - - { - 0.0F, 0.0F, 0.0F } - , // P_B_ISS_ISS - 0U, // confidence - 0U, // aug_state_enum - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // ml_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F } - , // ml_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F } - , // of_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cov_diag - 0U, // kfl_status - 0U, // update_OF_tracks_cnt - 0U, // update_ML_features_cnt - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // of_mahal_distance - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // ml_mahal_distance - - { - 0.0F, 0.0F, 0.0F } - , // hr_P_hr_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // hr_quat_ISS2hr - - { - 0.0F, 0.0F, 0.0F } - // P_EST_ISS_ISS - }, - - { - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // quat_ISS2B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // gyro_bias - - { - 0.0F, 0.0F, 0.0F } - , // V_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // accel_bias - - { - 0.0F, 0.0F, 0.0F } - , // P_B_ISS_ISS - 0U, // confidence - 0U, // aug_state_enum - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // ml_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F } - , // ml_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F } - , // of_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cov_diag - 0U, // kfl_status - 0U, // update_OF_tracks_cnt - 0U, // update_ML_features_cnt - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // of_mahal_distance - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // ml_mahal_distance - - { - 0.0F, 0.0F, 0.0F } - , // hr_P_hr_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // hr_quat_ISS2hr - - { - 0.0F, 0.0F, 0.0F } - // P_EST_ISS_ISS - }, - - { - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // quat_ISS2B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // gyro_bias - - { - 0.0F, 0.0F, 0.0F } - , // V_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // accel_bias - - { - 0.0F, 0.0F, 0.0F } - , // P_B_ISS_ISS - 0U, // confidence - 0U, // aug_state_enum - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // ml_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F } - , // ml_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F } - , // of_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cov_diag - 0U, // kfl_status - 0U, // update_OF_tracks_cnt - 0U, // update_ML_features_cnt - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // of_mahal_distance - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // ml_mahal_distance - - { - 0.0F, 0.0F, 0.0F } - , // hr_P_hr_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // hr_quat_ISS2hr - - { - 0.0F, 0.0F, 0.0F } - // P_EST_ISS_ISS - }, - - { - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // quat_ISS2B - - { - 0.0F, 0.0F, 0.0F } - , // omega_B_ISS_B - - { - 0.0F, 0.0F, 0.0F } - , // gyro_bias - - { - 0.0F, 0.0F, 0.0F } - , // V_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // A_B_ISS_ISS - - { - 0.0F, 0.0F, 0.0F } - , // accel_bias - - { - 0.0F, 0.0F, 0.0F } - , // P_B_ISS_ISS - 0U, // confidence - 0U, // aug_state_enum - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // ml_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F } - , // ml_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F } - , // of_quat_ISS2cam - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // of_P_cam_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F } - , // cov_diag - 0U, // kfl_status - 0U, // update_OF_tracks_cnt - 0U, // update_ML_features_cnt - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // of_mahal_distance - - { - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 } - , // ml_mahal_distance - - { - 0.0F, 0.0F, 0.0F } - , // hr_P_hr_ISS_ISS - - { - 0.0F, 0.0F, 0.0F, 0.0F } - , // hr_quat_ISS2hr - - { - 0.0F, 0.0F, 0.0F } - // P_EST_ISS_ISS - }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - - { 0.0, 0.0, 0.0 }, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 2.0F, - 0.5F, - 0.5F, - 0.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 48.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - 0.5F, - 0.5F, - 0.0F, - - { 0.0F, 0.0F, 0.0F }, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 48.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F }, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - -1.0F, - - { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F - }, - - { 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, - 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F }, - 0.0F, - 0.0F, - 0.5F, - 0.5F, - 0.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 0.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - -1.0F, - -1.0F, - 0.0F, - -1.0F, - 0.0F, - 48.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - - { 0.0F, 0.0F, 0.0F }, - - { -1.0F, -0.0F, -0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, -0.0F, -1.0F, -0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, -0.0F, -0.0F, -1.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, - 0.0F }, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - -1.0F, - 2.0F, - 2.0F, - -1.0F, - -1.0F, - -1.0F, - 2.0F, - 1, - 1, - 0, - 1, - 1, - 1U, - 1U, - 131070U, - 1U, - 2U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 0U, - 10U, - 1U, - 0U, - 15U, - 4U, - 10U, - - // Start of '/Normalize' - { - -1.0 - } - // End of '/Normalize' - , - - // Start of '/Normalize' - { - -1.0 - } - // End of '/Normalize' - , - - // Start of '/Normalize' - { - -1.0 - } - // End of '/Normalize' - , - - // Start of '/CoreSubsys' - { - 0.2604F, - - { 1.0F, -0.7396F }, - 0.0F, - 0.0F, - 0.0F - } - // End of '/CoreSubsys' - , - - // Start of '/CoreSubsys' - { - 0.2604F, - - { 1.0F, -0.7396F }, - 0.0F, - 0.0F - } - // End of '/CoreSubsys' -}; - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_private.h b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_private.h deleted file mode 100644 index 3c20d88349..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_private.h +++ /dev/null @@ -1,66 +0,0 @@ -// -// File: est_estimator_private.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_est_estimator_private_h_ -#define RTW_HEADER_est_estimator_private_h_ -#include "rtwtypes.h" -#include "est_estimator.h" -#if !defined(rt_VALIDATE_MEMORY) -#define rt_VALIDATE_MEMORY(S, ptr) if(!(ptr)) {\ - rtmSetErrorStatus(est_estimator_M, RT_MEMORY_ALLOCATION_ERROR);\ - } -#endif - -#if !defined(rt_FREE) -#if !defined(_WIN32) -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((ptr));\ - (ptr) = (NULL);\ - } -#else - -// Visual and other windows compilers declare free without const -#define rt_FREE(ptr) if((ptr) != (NULL)) {\ - free((void *)(ptr));\ - (ptr) = (NULL);\ - } -#endif -#endif - -void est_estima_MATLABFunction1_Init(DW_MATLABFunction1_est_estima_T *localDW); -void est_estim_MATLABFunction1_Start(B_MATLABFunction1_est_estimat_T *localB); -void est_estimator_MATLABFunction1(real_T rtu_Ts, real32_T rtu_impeller_speed, - B_MATLABFunction1_est_estimat_T *localB, DW_MATLABFunction1_est_estima_T - *localDW); -void est_estimator_MATLABFunction(const real32_T rtu_u[16], - B_MATLABFunction_est_estimato_T *localB); -void est_estimator_Normalize(const real32_T rtu_q_in[4], real32_T - rty_positive_scalar_q[4], P_Normalize_est_estimator_T *localP); -void est_estimator_Normalize_p(const real32_T rtu_Vec[4], real32_T rtu_Magnitude, - real32_T rty_Normalized_Vec[4]); -void est_estimato_IfActionSubsystem1(const kfl_msg *rtu_state_in, const - ase_cov_datatype rtu_P_in[13689], const real_T rtu_aug_velocity[3], const - real_T rtu_aug_velocity_p[3], const real_T rtu_aug_velocity_l[48], const - real_T rtu_aug_velocity_o[48], kfl_msg *rty_state_out, ase_cov_datatype - rty_P_out[13689], real_T rty_aug_velocity_out[3], real_T - rty_aug_velocity_out_p[3], real_T rty_aug_velocity_out_l[48], real_T - rty_aug_velocity_out_o[48]); - -#endif // RTW_HEADER_est_estimator_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_ref.rsp b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_ref.rsp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_types.h b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_types.h deleted file mode 100644 index cbaa3256b9..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_estimator_types.h +++ /dev/null @@ -1,186 +0,0 @@ -// -// File: est_estimator_types.h -// -// Code generated for Simulink model 'est_estimator'. -// -// Model version : 1.1142 -// Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 -// C/C++ source code generated on : Mon Sep 23 17:43:06 2019 -// -// Target selection: ert.tlc -// Embedded hardware selection: 32-bit Generic -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_est_estimator_types_h_ -#define RTW_HEADER_est_estimator_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_cvs_landmark_msg_ -#define DEFINED_TYPEDEF_FOR_cvs_landmark_msg_ - -typedef struct { - uint32_T cvs_timestamp_sec; - uint32_T cvs_timestamp_nsec; - real32_T cvs_landmarks[150]; - real32_T cvs_observations[100]; - uint8_T cvs_valid_flag[50]; -} cvs_landmark_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cvs_registration_pulse_ -#define DEFINED_TYPEDEF_FOR_cvs_registration_pulse_ - -typedef struct { - uint8_T cvs_ar_tag_pulse; - uint8_T cvs_landmark_pulse; - uint8_T cvs_optical_flow_pulse; - uint8_T cvs_handrail_pulse; -} cvs_registration_pulse; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cvs_optical_flow_msg_ -#define DEFINED_TYPEDEF_FOR_cvs_optical_flow_msg_ - -typedef struct { - uint32_T cvs_timestamp_sec; - uint32_T cvs_timestamp_nsec; - real32_T cvs_observations[1600]; - uint8_T cvs_valid_flag[800]; - real32_T cvs_id_tag[50]; -} cvs_optical_flow_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cvs_handrail_msg_ -#define DEFINED_TYPEDEF_FOR_cvs_handrail_msg_ - -typedef struct { - uint32_T cvs_timestamp_sec; - uint32_T cvs_timestamp_nsec; - real32_T cvs_landmarks[150]; - real32_T cvs_observations[150]; - uint8_T cvs_valid_flag[50]; - uint8_T cvs_3d_knowledge_flag; - real32_T cvs_handrail_local_pos[3]; - real32_T cvs_handrail_local_quat[4]; - uint8_T cvs_handrail_update_global_pose_flag; -} cvs_handrail_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_imu_msg_ -#define DEFINED_TYPEDEF_FOR_imu_msg_ - -typedef struct { - uint32_T imu_timestamp_sec; - uint32_T imu_timestamp_nsec; - real32_T imu_A_B_ECI_sensor[3]; - real32_T imu_accel_bias[3]; - real32_T imu_omega_B_ECI_sensor[3]; - real32_T imu_gyro_bias[3]; - uint8_T imu_validity_flag; - uint8_T imu_sat_flag; -} imu_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cmc_state_cmd_ -#define DEFINED_TYPEDEF_FOR_cmc_state_cmd_ - -typedef struct { - uint32_T timestamp_sec; - uint32_T timestamp_nsec; - real32_T P_B_ISS_ISS[3]; - real32_T V_B_ISS_ISS[3]; - real32_T A_B_ISS_ISS[3]; - real32_T quat_ISS2B[4]; - real32_T omega_B_ISS_B[3]; - real32_T alpha_B_ISS_B[3]; -} cmc_state_cmd; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_cmc_msg_ -#define DEFINED_TYPEDEF_FOR_cmc_msg_ - -typedef struct { - cmc_state_cmd cmc_state_cmd_a; - cmc_state_cmd cmc_state_cmd_b; - uint8_T cmc_mode_cmd; - uint8_T speed_gain_cmd; - uint8_T localization_mode_cmd; - real32_T att_kp[3]; - real32_T att_ki[3]; - real32_T omega_kd[3]; - real32_T pos_kp[3]; - real32_T pos_ki[3]; - real32_T vel_kd[3]; - real32_T center_of_mass[3]; - real32_T inertia_matrix[9]; - real32_T mass; -} cmc_msg; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ase_cov_datatype_ -#define DEFINED_TYPEDEF_FOR_ase_cov_datatype_ - -typedef real32_T ase_cov_datatype; -typedef creal32_T case_cov_datatype; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_kfl_msg_ -#define DEFINED_TYPEDEF_FOR_kfl_msg_ - -typedef struct { - real32_T quat_ISS2B[4]; - real32_T omega_B_ISS_B[3]; - real32_T gyro_bias[3]; - real32_T V_B_ISS_ISS[3]; - real32_T A_B_ISS_ISS[3]; - real32_T accel_bias[3]; - real32_T P_B_ISS_ISS[3]; - uint8_T confidence; - uint32_T aug_state_enum; - real32_T ml_quat_ISS2cam[4]; - real32_T ml_P_cam_ISS_ISS[3]; - real32_T of_quat_ISS2cam[64]; - real32_T of_P_cam_ISS_ISS[48]; - ase_cov_datatype cov_diag[117]; - uint16_T kfl_status; - uint8_T update_OF_tracks_cnt; - uint8_T update_ML_features_cnt; - real_T of_mahal_distance[50]; - real_T ml_mahal_distance[50]; - real32_T hr_P_hr_ISS_ISS[3]; - real32_T hr_quat_ISS2hr[4]; - real32_T P_EST_ISS_ISS[3]; -} kfl_msg; - -#endif - -// Parameters for system: '/CoreSubsys' -typedef struct P_CoreSubsys_est_estimator_T_ P_CoreSubsys_est_estimator_T; - -// Parameters for system: '/CoreSubsys' -typedef struct P_CoreSubsys_est_estimator_g_T_ P_CoreSubsys_est_estimator_g_T; - -// Parameters for system: '/Normalize' -typedef struct P_Normalize_est_estimator_T_ P_Normalize_est_estimator_T; - -// Parameters (auto storage) -typedef struct P_est_estimator_T_ P_est_estimator_T; - -// Forward declaration for rtModel -typedef struct tag_RTM_est_estimator_T RT_MODEL_est_estimator_T; - -#endif // RTW_HEADER_est_estimator_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.cpp b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.cpp deleted file mode 100644 index 4f8553077e..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// Autogenerated file, see write_tunable_param_functions for details - -#include "est_tunable_funcs.h" -#include -#include -#include - -// Do config setup and add includes -void est_ReadParams(config_reader::ConfigReader* config, RT_MODEL_est_estimator_T* est) { - - auto & p = est->defaultParam; - - if (!msg_conversions::config_read_array(config, "tun_abp_p_dockcam_imu_est", 3 , p->tun_abp_p_dockcam_imu_est)) - ROS_FATAL("Unspecified tun_abp_p_dockcam_imu_est."); - if (!msg_conversions::config_read_array(config, "tun_abp_p_imu_body_body", 3 , p->tun_abp_p_imu_body_body)) - ROS_FATAL("Unspecified tun_abp_p_imu_body_body."); - if (!msg_conversions::config_read_array(config, "tun_abp_p_navcam_imu_est", 3 , p->tun_abp_p_navcam_imu_est)) - ROS_FATAL("Unspecified tun_abp_p_navcam_imu_est."); - if (!msg_conversions::config_read_array(config, "tun_abp_p_perchcam_imu_est", 3 , p->tun_abp_p_perchcam_imu_est)) - ROS_FATAL("Unspecified tun_abp_p_perchcam_imu_est."); - if (!msg_conversions::config_read_array(config, "tun_abp_q_body2dockcam", 4 , p->tun_abp_q_body2dockcam)) - ROS_FATAL("Unspecified tun_abp_q_body2dockcam."); - if (!msg_conversions::config_read_array(config, "tun_abp_q_body2navcam", 4 , p->tun_abp_q_body2navcam)) - ROS_FATAL("Unspecified tun_abp_q_body2navcam."); - if (!msg_conversions::config_read_array(config, "tun_abp_q_body2perchcam", 4 , p->tun_abp_q_body2perchcam)) - ROS_FATAL("Unspecified tun_abp_q_body2perchcam."); - if (!msg_conversions::config_read_array(config, "tun_abp_quat_body2imu", 4 , p->tun_abp_quat_body2imu)) - ROS_FATAL("Unspecified tun_abp_quat_body2imu."); - if (!msg_conversions::config_read_array(config, "tun_ase_Q_imu", 12 , p->tun_ase_Q_imu)) - ROS_FATAL("Unspecified tun_ase_Q_imu."); - if (!config->GetReal("tun_ase_acquired_ticks", &p->tun_ase_acquired_ticks)) - ROS_FATAL("Unspecified tun_ase_acquired_ticks."); - if (!config->GetReal("tun_ase_converged_thresh", &p->tun_ase_converged_thresh)) - ROS_FATAL("Unspecified tun_ase_converged_thresh."); - if (!config->GetReal("tun_ase_diverged_thresh", &p->tun_ase_diverged_thresh)) - ROS_FATAL("Unspecified tun_ase_diverged_thresh."); - if (!config->GetReal("tun_ase_dock_r_mag", &p->tun_ase_dock_r_mag)) - ROS_FATAL("Unspecified tun_ase_dock_r_mag."); - if (!config->GetReal("tun_ase_dockcam_distortion", &p->tun_ase_dockcam_distortion)) - ROS_FATAL("Unspecified tun_ase_dockcam_distortion."); - if (!config->GetReal("tun_ase_dockcam_inv_focal_length", &p->tun_ase_dockcam_inv_focal_length)) - ROS_FATAL("Unspecified tun_ase_dockcam_inv_focal_length."); - if (!msg_conversions::config_read_array(config, "tun_ase_gravity_accel", 3 , p->tun_ase_gravity_accel)) - ROS_FATAL("Unspecified tun_ase_gravity_accel."); - if (!config->GetReal("tun_ase_hr_distance_r", &p->tun_ase_hr_distance_r)) - ROS_FATAL("Unspecified tun_ase_hr_distance_r."); - if (!config->GetReal("tun_ase_hr_r_mag", &p->tun_ase_hr_r_mag)) - ROS_FATAL("Unspecified tun_ase_hr_r_mag."); - if (!config->GetReal("tun_ase_mahal_distance_max", &p->tun_ase_mahal_distance_max)) - ROS_FATAL("Unspecified tun_ase_mahal_distance_max."); - if (!config->GetReal("tun_ase_map_error", &p->tun_ase_map_error)) - ROS_FATAL("Unspecified tun_ase_map_error."); - if (!config->GetReal("tun_ase_max_accel", &p->tun_ase_max_accel)) - ROS_FATAL("Unspecified tun_ase_max_accel."); - if (!config->GetReal("tun_ase_max_gyro", &p->tun_ase_max_gyro)) - ROS_FATAL("Unspecified tun_ase_max_gyro."); - if (!config->GetReal("tun_ase_min_ar_meas", &p->tun_ase_min_ar_meas)) - ROS_FATAL("Unspecified tun_ase_min_ar_meas."); - if (!config->GetReal("tun_ase_min_ml_meas", &p->tun_ase_min_ml_meas)) - ROS_FATAL("Unspecified tun_ase_min_ml_meas."); - if (!config->GetReal("tun_ase_ml_forward_projection_time", &p->tun_ase_ml_forward_projection_time)) - ROS_FATAL("Unspecified tun_ase_ml_forward_projection_time."); - if (!config->GetReal("tun_ase_navcam_distortion", &p->tun_ase_navcam_distortion)) - ROS_FATAL("Unspecified tun_ase_navcam_distortion."); - if (!config->GetReal("tun_ase_navcam_inv_focal_length", &p->tun_ase_navcam_inv_focal_length)) - ROS_FATAL("Unspecified tun_ase_navcam_inv_focal_length."); - if (!config->GetReal("tun_ase_of_r_mag", &p->tun_ase_of_r_mag)) - ROS_FATAL("Unspecified tun_ase_of_r_mag."); - if (!config->GetReal("tun_ase_q_saturated_accel", &p->tun_ase_q_saturated_accel)) - ROS_FATAL("Unspecified tun_ase_q_saturated_accel."); - if (!config->GetReal("tun_ase_q_saturated_gyro", &p->tun_ase_q_saturated_gyro)) - ROS_FATAL("Unspecified tun_ase_q_saturated_gyro."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_P_B_ISS_ISS", 3 , p->tun_ase_state_ic_P_B_ISS_ISS)) - ROS_FATAL("Unspecified tun_ase_state_ic_P_B_ISS_ISS."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_P_EST_ISS_ISS", 3 , p->tun_ase_state_ic_P_EST_ISS_ISS)) - ROS_FATAL("Unspecified tun_ase_state_ic_P_EST_ISS_ISS."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_V_B_ISS_ISS", 3 , p->tun_ase_state_ic_V_B_ISS_ISS)) - ROS_FATAL("Unspecified tun_ase_state_ic_V_B_ISS_ISS."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_cov_diag", 15 , p->tun_ase_state_ic_cov_diag)) - ROS_FATAL("Unspecified tun_ase_state_ic_cov_diag."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_omega_B_ISS_B", 3 , p->tun_ase_state_ic_omega_B_ISS_B)) - ROS_FATAL("Unspecified tun_ase_state_ic_omega_B_ISS_B."); - if (!msg_conversions::config_read_array(config, "tun_ase_state_ic_quat_ISS2B", 4 , p->tun_ase_state_ic_quat_ISS2B)) - ROS_FATAL("Unspecified tun_ase_state_ic_quat_ISS2B."); - if (!config->GetReal("tun_ase_vis_r_mag", &p->tun_ase_vis_r_mag)) - ROS_FATAL("Unspecified tun_ase_vis_r_mag."); - if (!config->GetReal("tun_ase_vocam_inv_focal_length", &p->tun_ase_vocam_inv_focal_length)) - ROS_FATAL("Unspecified tun_ase_vocam_inv_focal_length."); - if (!config->GetReal("tun_aug_ic_cov", &p->tun_aug_ic_cov)) - ROS_FATAL("Unspecified tun_aug_ic_cov."); - if (!msg_conversions::config_read_array(config, "tun_grav_hp_den", 2 , p->tun_grav_hp_den)) - ROS_FATAL("Unspecified tun_grav_hp_den."); - if (!msg_conversions::config_read_array(config, "tun_grav_hp_num", 2 , p->tun_grav_hp_num)) - ROS_FATAL("Unspecified tun_grav_hp_num."); - if (!config->GetReal("tun_ic_cov_pos", &p->tun_ic_cov_pos)) - ROS_FATAL("Unspecified tun_ic_cov_pos."); - if (!config->GetReal("tun_ic_cov_quat", &p->tun_ic_cov_quat)) - ROS_FATAL("Unspecified tun_ic_cov_quat."); - if (!config->GetReal("tun_max_mahal_reject_frames", &p->tun_max_mahal_reject_frames)) - ROS_FATAL("Unspecified tun_max_mahal_reject_frames."); - bool ase_enable_of; - if (!config->GetBool("tun_ase_enable_of", &ase_enable_of)) - ROS_FATAL("Unspecified tun_ase_enable_of."); - p->tun_ase_enable_of = ase_enable_of; - bool ase_gravity_removal; - if (!config->GetBool("tun_ase_gravity_removal", &ase_gravity_removal)) - ROS_FATAL("Unspecified tun_ase_gravity_removal."); - p->tun_ase_gravity_removal = ase_gravity_removal; - bool grav_hp_enable_f; - if (!config->GetBool("tun_grav_hp_enable_f", &grav_hp_enable_f)) - ROS_FATAL("Unspecified tun_grav_hp_enable_f."); - p->tun_grav_hp_enable_f = grav_hp_enable_f; -} diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.h b/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.h deleted file mode 100644 index 936b2baafb..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/est_tunable_funcs.h +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef READ_PARAMS_CPP -#define READ_PARAMS_CPP - -namespace config_reader { - class ConfigReader; -} - -#include "est_estimator.h" -void est_ReadParams(config_reader::ConfigReader* config, RT_MODEL_est_estimator_T* est); - -#endif \ No newline at end of file diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/modelsources.txt b/gnc/matlab/code_generation/est_estimator_ert_rtw/modelsources.txt deleted file mode 100644 index bd11587882..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/modelsources.txt +++ /dev/null @@ -1 +0,0 @@ - est_estimator.cpp compute_delta_state_and_cov.cpp apply_delta_state.cpp of_residual_and_h.cpp matrix_multiply.cpp diff --git a/gnc/matlab/code_generation/est_estimator_ert_rtw/rtw_proj.tmw b/gnc/matlab/code_generation/est_estimator_ert_rtw/rtw_proj.tmw deleted file mode 100644 index e83d0266b9..0000000000 --- a/gnc/matlab/code_generation/est_estimator_ert_rtw/rtw_proj.tmw +++ /dev/null @@ -1,4 +0,0 @@ -Simulink Coder project for est_estimator using . MATLAB root = /usr/local/MATLAB/R2016b. SimStruct date: 22-Jul-2016 13:46:14 -This file is generated by Simulink Coder for use by the make utility -to determine when to rebuild objects when the name of the current Simulink Coder project changes. -The rtwinfomat located at: ../slprj/ert/est_estimator/tmwinternal/binfo.mat diff --git a/gnc/readme.md b/gnc/readme.md index 4349e90c35..f3f6b2038c 100644 --- a/gnc/readme.md +++ b/gnc/readme.md @@ -1,24 +1,20 @@ \page gnc Guidance, Navigation & Control -The Guidance, Navigation & Control subsystem is responsible for tracking the -robot's pose and controlling the robot's motion. +The Guidance, Navigation & Control subsystem is responsible for controlling the robot's motion. ## GNC Subsystems GNC consists of four main subsystems, that run in a chain: -1. \subpage ekf : The extended Kalman filter integrates IMU measurements, as well as -visual measurements from the localization subsystem, to track the robot's pose. - -2. \subpage ctl : The control subsystem takes the robot's current state from the EKF, +1. \subpage ctl : The control subsystem takes the robot's current state from the localizer, and determines the forces and torques needed to execute the commands sent from the mobility subsystem. -3. \subpage fam : The force allocation module takes the force and torque commands +2. \subpage fam : The force allocation module takes the force and torque commands from the control subsystem, and determines the PMC speeds and nozzle positions to best execute these forces and torques. -4. \subpage simwrapper : The simulator simulates the inputs to the EKF, and simulates the +3. \subpage simwrapper : The simulator simulates the inputs to the localizer, and simulates the robot's motion based on the outputs of the FAM. ## GNC Code Organization @@ -27,7 +23,7 @@ Unlike the rest of flight software, the GNC systems are written mainly in Simulink. Simulink drag and drop box diagrams are then converted to automatically generated C code. The structure of the GNC code is: -1. [EKF](@ref ekf), [CTL](@ref ctl) and [FAM](@ref fam) are GN&C ROS Wrapper. +1. [CTL](@ref ctl) and [FAM](@ref fam) are GN&C ROS Wrapper. [Sim Wrapper](@ref simwrapper) is the model side of the GN&C ROS Wrapper. These folders contain ROS nodelets that mainly convert inputs and outputs between ROS messages and Simulink. diff --git a/gnc/sim_wrapper/tools/test_sim.cc b/gnc/sim_wrapper/tools/test_sim.cc index a551d32ac9..fe3e4ee607 100644 --- a/gnc/sim_wrapper/tools/test_sim.cc +++ b/gnc/sim_wrapper/tools/test_sim.cc @@ -18,8 +18,6 @@ #include #include -#include -#include #define COMPARE_INT(a, b) if (a != b) {fprintf(stderr, "Comparison failed at line %d. (%d, %d)\n", __LINE__, \ (a), (b)); return 1;} @@ -73,38 +71,12 @@ int verify_sim_output(const gnc_autocode::GncSimAutocode & sim, const gnc_autoco return 0; } -int verify_ekf_output(const gnc_autocode::GncEkfAutocode & ekf, const gnc_autocode::GncEkfCSV & csv) { - const kfl_msg & k1 = ekf.kfl_; - const kfl_msg & k2 = csv.kfl_; - COMPARE_FLOAT_VECTOR(k1.quat_ISS2B, k2.quat_ISS2B, 4, 0.001); - COMPARE_FLOAT_VECTOR(k1.omega_B_ISS_B, k2.omega_B_ISS_B, 3, 0.001); - COMPARE_FLOAT_VECTOR(k1.gyro_bias, k2.gyro_bias, 3, 0.001); - COMPARE_FLOAT_VECTOR(k1.V_B_ISS_ISS, k2.V_B_ISS_ISS, 3, 0.07); - COMPARE_FLOAT_VECTOR(k1.A_B_ISS_ISS, k2.A_B_ISS_ISS, 3, 0.07); - COMPARE_FLOAT_VECTOR(k1.accel_bias, k2.accel_bias, 3, 0.003); - COMPARE_FLOAT_VECTOR(k1.P_B_ISS_ISS, k2.P_B_ISS_ISS, 3, 0.005); - COMPARE_INT(k1.confidence, k2.confidence); - COMPARE_INT(k1.aug_state_enum, k2.aug_state_enum); - COMPARE_FLOAT_VECTOR(k1.ml_quat_ISS2cam, k2.ml_quat_ISS2cam, 4, 0.001); - COMPARE_FLOAT_VECTOR(k1.ml_P_cam_ISS_ISS, k2.ml_P_cam_ISS_ISS, 3, 0.005); - COMPARE_FLOAT_VECTOR(k1.of_quat_ISS2cam, k2.of_quat_ISS2cam, 20, 0.001); - COMPARE_FLOAT_VECTOR(k1.of_P_cam_ISS_ISS, k2.of_P_cam_ISS_ISS, 15, 0.005); - COMPARE_FLOAT_VECTOR(k1.cov_diag, k2.cov_diag, 51, 0.005); - COMPARE_INT(k1.kfl_status, k2.kfl_status); - return 0; -} - int main(int argc, char** argv) { gnc_autocode::GncSimCSV csv_sim; gnc_autocode::GncSimAutocode sim; - gnc_autocode::GncEkfCSV csv_ekf; - gnc_autocode::GncEkfAutocode ekf; - csv_sim.Initialize(std::string(".")); - csv_ekf.Initialize(std::string(".")); sim.Initialize(); - ekf.Initialize(); while (true) { csv_sim.Step(); @@ -118,20 +90,5 @@ int main(int argc, char** argv) { csv_sim.act_msg_.act_timestamp_sec + static_cast(csv_sim.act_msg_.act_timestamp_nsec) / 1e9); break; } - - csv_ekf.Step(); - - // now copy the inputs to the EKF - memcpy(&ekf.vis_, &csv_sim.landmark_msg_, sizeof(cvs_landmark_msg)); - memcpy(&ekf.reg_, &csv_sim.reg_pulse_, sizeof(cvs_registration_pulse)); - memcpy(&ekf.of_, &csv_sim.optical_msg_, sizeof(cvs_optical_flow_msg)); - memcpy(&ekf.imu_, &csv_sim.imu_msg_, sizeof(imu_msg)); - ekf.Step(); - - if (verify_ekf_output(ekf, csv_ekf)) { - fprintf(stderr, "EKF failed at time %g.\n", - csv_sim.act_msg_.act_timestamp_sec + static_cast(csv_sim.act_msg_.act_timestamp_nsec) / 1e9); - break; - } } } diff --git a/tools/ekf_bag/CMakeLists.txt b/tools/ekf_bag/CMakeLists.txt deleted file mode 100644 index 3840dfe592..0000000000 --- a/tools/ekf_bag/CMakeLists.txt +++ /dev/null @@ -1,103 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -cmake_minimum_required(VERSION 3.0) -project(ekf_bag) - -## Compile as C++14, supported in ROS Kinetic and newer -add_compile_options(-std=c++14) - -## Find catkin macros and libraries -find_package(catkin2 REQUIRED COMPONENTS - roscpp - config_reader - camera - ekf - lk_optical_flow - localization_node -) - - -# Find OpenCV -SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") -find_package(OpenCV331 REQUIRED) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS roscpp config_reader camera ekf lk_optical_flow localization_node -) - -########### -## Build ## -########### - -# Specify additional locations of header files -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -# Declare C++ libraries -add_library(ekf_bag - src/ekf_bag.cc - src/ekf_bag_csv.cc - src/tracked_features.cc -) -add_dependencies(ekf_bag ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ekf_bag ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) - -## Declare a C++ executable: bag_to_csv -add_executable(bag_to_csv tools/bag_to_csv.cc) -add_dependencies(bag_to_csv ${catkin_EXPORTED_TARGETS}) -target_link_libraries(bag_to_csv - ekf_bag gflags glog ${catkin_LIBRARIES}) - -## Declare a C++ executable: bag_to_csv -add_executable(sparse_map_bag tools/sparse_map_bag.cc) -add_dependencies(sparse_map_bag ${catkin_EXPORTED_TARGETS}) -target_link_libraries(sparse_map_bag - ekf_bag gflags glog ${catkin_LIBRARIES}) - -############# -## Install ## -############# - -# Mark libraries for installation -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -# Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE -) - -# Install C++ executables -install(TARGETS bag_to_csv DESTINATION bin) -install(TARGETS sparse_map_bag DESTINATION bin) -install(CODE "execute_process( - COMMAND ln -s ../../bin/bag_to_csv share/${PROJECT_NAME} - COMMAND ln -s ../../bin/sparse_map_bag share/${PROJECT_NAME} - WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} - OUTPUT_QUIET - ERROR_QUIET - )") diff --git a/tools/ekf_bag/include/ekf_bag/ekf_bag.h b/tools/ekf_bag/include/ekf_bag/ekf_bag.h deleted file mode 100644 index 5af065b4d1..0000000000 --- a/tools/ekf_bag/include/ekf_bag/ekf_bag.h +++ /dev/null @@ -1,94 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef EKF_BAG_EKF_BAG_H_ -#define EKF_BAG_EKF_BAG_H_ - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -namespace ekf_bag { - -class EkfBag { - public: - EkfBag(const char* bagfile, const char* mapfile, bool run_ekf = true, - bool gen_features = true, const char* biasfile = NULL, - std::string image_topic = std::string(TOPIC_HARDWARE_NAV_CAM), const std::string& gnc_config = "gnc.config"); - virtual ~EkfBag(void); - - void Run(void); - - protected: - virtual void ReadParams(config_reader::ConfigReader* config); - - virtual void UpdateImu(const ros::Time& time, const sensor_msgs::Imu& imu); - virtual void UpdateImage(const ros::Time& time, - const sensor_msgs::ImageConstPtr& image); - virtual void UpdateGroundTruth(const geometry_msgs::PoseStamped& pose); - - virtual void UpdateEKF(const ff_msgs::EkfState& state) {} - // called when image received, not when finished processing - virtual void UpdateOpticalFlow(const ff_msgs::Feature2dArray& of); - virtual void UpdateSparseMap(const ff_msgs::VisualLandmarks& vl); - virtual void UpdateOpticalFlowReg(const ff_msgs::CameraRegistration& reg); - virtual void UpdateSparseMapReg(const ff_msgs::CameraRegistration& reg); - - rosbag::Bag bag_; - sparse_mapping::SparseMap map_; - localization_node::Localizer loc_; - - lk_optical_flow::LKOpticalFlow of_; - - ekf::Ekf ekf_; - - ros::Time bag_start_time_; - - // most recent ground truth pose - geometry_msgs::Pose ground_truth_; - - private: - void EstimateBias(void); - - bool run_ekf_; - bool gen_features_; - const char* bias_file_; - std::string image_topic_; - std::string gnc_config_; - // configuration parameters - float sparse_map_delay_, of_delay_; - - // variables used for sending visual features - bool processing_of_, processing_sparse_map_; - int of_id_, vl_id_; // message ids - ros::Time of_send_time_, vl_send_time_; // time to send features - ff_msgs::Feature2dArray of_features_; // save to send later - ff_msgs::VisualLandmarks vl_features_; -}; - -} // end namespace ekf_bag - -#endif // EKF_BAG_EKF_BAG_H_ diff --git a/tools/ekf_bag/include/ekf_bag/ekf_bag_csv.h b/tools/ekf_bag/include/ekf_bag/ekf_bag_csv.h deleted file mode 100644 index 03abe64b8d..0000000000 --- a/tools/ekf_bag/include/ekf_bag/ekf_bag_csv.h +++ /dev/null @@ -1,61 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef EKF_BAG_EKF_BAG_CSV_H_ -#define EKF_BAG_EKF_BAG_CSV_H_ - -#include -#include - -#include - -namespace ekf_bag { - -class EkfBagCsv : public EkfBag { - public: - EkfBagCsv(const char* bagfile, const char* mapfile, const char* csvfile, - bool run_ekf = true, bool gen_features = true, const char* biasfile = NULL, - std::string image_topic = std::string(TOPIC_HARDWARE_NAV_CAM), const std::string& gnc_config = "gnc.config"); - virtual ~EkfBagCsv(void); - - protected: - virtual void UpdateGroundTruth(const geometry_msgs::PoseStamped & pose); - - virtual void UpdateEKF(const ff_msgs::EkfState & state); - virtual void UpdateOpticalFlow(const ff_msgs::Feature2dArray & of); - virtual void UpdateSparseMap(const ff_msgs::VisualLandmarks & vl); - virtual void UpdateOpticalFlowReg(const ff_msgs::CameraRegistration & reg); - virtual void UpdateSparseMapReg(const ff_msgs::CameraRegistration & reg); - - virtual void ReadParams(config_reader::ConfigReader* config); - - TrackedOFFeatures tracked_of_; - - private: - FILE* f_; - - bool start_time_set_; - ros::Time start_time_; - ros::Time ml_reg_time_, of_reg_time_; - unsigned int ml_cam_id_, of_cam_id_; -}; - -} // end namespace ekf_bag - -#endif // EKF_BAG_EKF_BAG_CSV_H_ - diff --git a/tools/ekf_bag/include/ekf_bag/tracked_features.h b/tools/ekf_bag/include/ekf_bag/tracked_features.h deleted file mode 100644 index 92071e6d3e..0000000000 --- a/tools/ekf_bag/include/ekf_bag/tracked_features.h +++ /dev/null @@ -1,90 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef EKF_BAG_TRACKED_FEATURES_H_ -#define EKF_BAG_TRACKED_FEATURES_H_ - -#include -#include -#include -#include -#include -#include -#include - -namespace ekf_bag { - -struct OFFeature { - int id; - float time; - float x; - float y; -}; - -struct SMFeature { - Eigen::Vector3f pos; - Eigen::Vector2f pixel; - float time; -}; - -class TrackedOFFeatures { - public: - TrackedOFFeatures(void) : - params_(Eigen::Vector2i(-1, -1), Eigen::Vector2d::Constant(-1), - Eigen::Vector2d(-1, -1)) {} - ~TrackedOFFeatures(void) {} - - void SetCameraParameters(const camera::CameraParameters & params) {params_ = params;} - - void UpdateFeatures(const ff_msgs::Feature2dArray & of, float time); - std::map::iterator begin() {return f_.begin();} - std::map::iterator end() {return f_.end();} - - private: - std::map f_; - camera::CameraParameters params_; -}; - -class TrackedSMFeatures { - public: - TrackedSMFeatures(void) : - params_(Eigen::Vector2i(-1, -1), Eigen::Vector2d::Constant(-1), - Eigen::Vector2d(-1, -1)) {} - ~TrackedSMFeatures(void) {} - - void SetCameraParameters(const camera::CameraParameters & params) {params_ = params;} - void SetCameraToBody(const Eigen::Affine3d cam_to_body) {camera_to_body_ = cam_to_body; - body_to_camera_ = cam_to_body.inverse();} - - void UpdateFeatures(const ff_msgs::VisualLandmarks & vl, float time); - void UpdatePose(const geometry_msgs::Pose & pose); - - std::vector::iterator begin() {return f_.begin();} - std::vector::iterator end() {return f_.end();} - Eigen::Vector2d FeatureToCurrentPixel(const struct SMFeature & f); - - private: - std::vector f_; - camera::CameraParameters params_; - Eigen::Affine3d camera_to_body_, body_to_camera_, pose_transform_; -}; - -} // end namespace ekf_bag - -#endif // EKF_BAG_TRACKED_FEATURES_H_ - diff --git a/tools/ekf_bag/package.xml b/tools/ekf_bag/package.xml deleted file mode 100644 index 10ffc14d6c..0000000000 --- a/tools/ekf_bag/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - ekf_bag - 0.0.0 - - The ekf_bag package runs the EKF on an entire bag and draws the output. - - - Apache License, Version 2.0 - - - Astrobee Flight Software - - - Astrobee Flight Software - - catkin - roscpp - config_reader - camera - ekf - lk_optical_flow - localization_node - roscpp - config_reader - camera - ekf - lk_optical_flow - localization_node - - diff --git a/tools/ekf_bag/readme.md b/tools/ekf_bag/readme.md deleted file mode 100644 index 3c6e3da82e..0000000000 --- a/tools/ekf_bag/readme.md +++ /dev/null @@ -1,86 +0,0 @@ -\page ekfbag EKF Bag - -This package provides scripts used to evaluate the EKF and -localization on recorded bag files. To record the bags, use the -command - - rosbag record /hw/cam_nav /hw/imu /loc/truth - -The bag files should begin with at least 5 seconds of the robot -remaining stationary so that we can estimate the imu bias. - -# ekf_graph - -Run `ekf_graph bag.bag` to create a pdf showing the results of the EKF -on the recorded bag. - -# rosbag_to_csv - -Run `rosbag_to_csv bag.bag` to create a CSV file detailing the -results of a run, which can then be passed to the GNC Matlab code for -testing. - -# sparse_map_eval - -This tool evaluates how well a robot localizes images from a bag -against a sparse map. - -To use, first copy the desired robot's config file to the current -directory, for example, as: - -cp ~/astrobee/astrobee/config/robots/bumble.config robot.config - -Then run the tool as: - -python ~/astrobee/tools/ekf_bag/scripts/sparse_map_eval mybag.bag mymap.map - -It will print on output some text like: - -Localized 70 / 71 images with mean of 33 features.=====================] 100% -Time per Frame: 0.32161 s -CPU Time per Frame: 0.27612 s -Run time: 23.9871 - -If, before launching this tool one runs: - - export SAVE_FAILED_IMAGES_DIR=failed_images - mkdir -p $SAVE_FAILED_IMAGES_DIR - -it will save the images for which localization failed (with the image -timestamp as image name) in the specified directory. - -# ekf_diff - -This is a tool for regression testing localization. It plots the difference between the ekf output of two sets of data generated from a single bag file, and has several features defined as flags. -To run, first generate text files containing EKF output using ekf_graph. Then, run ekf_diff:: -'rosrun ekf_bag ekf_diff bag1.txt bag2.txt' -The flags are defined as follows: --s: define start time --e: define end time --p: define a minimum time difference between poses to compare, rather than all of them --i: resample data to match the first .txt file rather than selecting the neares timestamped data --r: use relative poses; at each measurement, report the difference between the current measurement and the last one rather than absolute positions on the global map --m: generate a heatmap of where in the ISS localization is less accurate --sm: use the sparse mapping poses from the first text file supplied as ground truth -This tool relies on scipy 1.2.0: to upgrade this, run: 'python -m pip install scipy==1.2' - -# streamlit webserver - -This is a webserver to run some of the ekf_bag scripts and automatically cache results. -The server requires python >= 3.6 (which will not be already installed from ROS) and the streamlit package installed using pip. After installing python3.6, run 'python3.6 -m pip install streamlit'. If this command fails, upgrade pip and retry. Additionally, install pdf2image by running: 'python3.6 -m pip install pdf2image'. Create a folder to store all data in. Then, inside that folder, create folders named "bags" and "maps". Put any map files in the "maps" folder, and any bag files in the "bags" folder. The "bags" folder also must contain a configuration file named robot.config (can be copied from astrobee/astrobee/config directory). Then, from the first folder you created, run: -'streamlit run /path/to/streamlit_server.py' - -# parameter_sweep.py -This tool runs ekf_graph tests for a set of parameter combinations. Parameter ranges and names are defined in parameter_sweep.py and a gnc.config file is created for each parameter combination in the sweep. Each parameter combination is tested in parallel and results are saved locally to a timestamped directory or user defined directory. - -# create_plots.py -Uses output of parameter_sweep.py and generates plots comparing the results of the sweep. - -# bag_sweep.py -Runs ekf tests using a given gnc config file on a set of bag files in parallel. - -# bag_and_parameter_sweep.py -Calls parameter_sweep on a set of bag files. - -# create_average_plots.py -Averages results for each job_id run over multiple tests and creates plots. Used for plotting bag_sweep.py results. diff --git a/tools/ekf_bag/scripts/bag_and_parameter_sweep.py b/tools/ekf_bag/scripts/bag_and_parameter_sweep.py deleted file mode 100755 index d06b7f8ee5..0000000000 --- a/tools/ekf_bag/scripts/bag_and_parameter_sweep.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import argparse -import os - -import parameter_sweep -import utilities - - -def bag_and_parameter_sweep(bag_files, output_dir, map_file, image_topic, gnc_config): - for bag_file in bag_files: - # Save parameter sweep output in different directory for each bagfile, name directory using bagfile - bag_name_prefix = os.path.splitext(os.path.basename(bag_file))[0] - bag_output_dir = os.path.join(output_dir, bag_name_prefix) - parameter_sweep.make_values_and_parameter_sweep( - bag_output_dir, bag_file, map_file, image_topic, gnc_config - ) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("map_file", help="Full path to map file.") - parser.add_argument("gnc_config", help="Full path to gnc config file.") - parser.add_argument( - "-d", - "--bag_directory", - default=None, - help="Full path to directory containing bag files.", - ) - parser.add_argument( - "-o", - "--output_directory", - default=None, - help="Full path to output directory where files will be saved. If not specified, timestamped directory will be created in current path.", - ) - - parser.add_argument( - "-r", - "--robot_name", - metavar="ROBOT", - help="Specify the robot to use (just name, not path).", - ) - parser.add_argument( - "-i", - "--image_topic", - dest="image_topic", - default=None, - help="Use specified image topic.", - ) - parser.add_argument( - "--save_stats", action="store_true", help="Save stats to csv file." - ) - parser.add_argument( - "--make_plots", type=bool, default=True, help="Make pdf of plots of results." - ) - - args, args_unkonown = parser.parse_known_args() - - bag_directory = args.bag_directory - if bag_directory == None: - bag_directory = os.getcwd() - - if not os.path.exists(bag_directory): - print(("Bag directory {} does not exist.".format(bag_directory))) - exit() - bag_files = utilities.get_files(bag_directory, "*.bag") - print(("Found {} bag files in {}.".format(len(bag_files), bag_directory))) - - output_dir = utilities.create_directory(args.output_directory) - print(("Output directory for results is {}".format(output_dir))) - - bag_and_parameter_sweep( - bag_files, output_dir, args.map_file, args.image_topic, args.gnc_config - ) diff --git a/tools/ekf_bag/scripts/bag_sweep.py b/tools/ekf_bag/scripts/bag_sweep.py deleted file mode 100755 index a4b1728dde..0000000000 --- a/tools/ekf_bag/scripts/bag_sweep.py +++ /dev/null @@ -1,150 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import argparse -import itertools -import multiprocessing -import os - -import ekf_graph -import pandas as pd -import utilities - - -# Run ekf on bag file. -# Add traceback so errors are forwarded, otherwise -# some errors are suppressed due to the multiprocessing -# library call -@utilities.full_traceback -def test_on_bag(bag_file, output_dir, args): - print(("Running EKF test on {}".format(bag_file))) - name_prefix = os.path.splitext(os.path.basename(bag_file))[0] - ekf_output_file = os.path.join(output_dir, name_prefix + "_ekf.txt") - pdf_output_file = os.path.join(output_dir, name_prefix + "_plots.pdf") - results_csv_output_file = os.path.join(output_dir, name_prefix + "_results.csv") - options = ekf_graph.RunEKFOptions( - bag_file, - args.map_file, - ekf_output_file, - pdf_output_file, - results_csv_output_file, - ) - options.set_bag_sweep_params(args) - options.features_in_bag = True - ekf_graph.run_ekf_and_save_stats(options) - - -# Helper that unpacks arguments and calls original function -# Aides running jobs in parallel as pool only supports -# passing a single argument to workers -def test_on_bag_helper(zipped_vals): - return test_on_bag(*zipped_vals) - - -# TODO: replace args with individual params -def bag_sweep(bag_files, output_dir, args): - num_processes = 6 - pool = multiprocessing.Pool(num_processes) - # izip arguments so we can pass as one argument to pool worker - pool.map( - test_on_bag_helper, - list(zip(bag_files, itertools.repeat(output_dir), itertools.repeat(args))), - ) - - -def combined_results(csv_files): - dataframes = [pd.read_csv(file) for file in csv_files] - if not dataframes: - print("Failed to create dataframes") - exit() - names = dataframes[0].columns - combined_dataframes = pd.DataFrame(None, None, names) - for dataframe in dataframes: - trimmed_dataframe = pd.DataFrame(dataframe.values[0:1], columns=names) - combined_dataframes = combined_dataframes.append( - trimmed_dataframe, ignore_index=True - ) - return combined_dataframes - - -def combine_results_in_csv_file(bag_files, output_dir): - # Don't save this as *stats.csv otherwise it will be including when averaging bag results in average_results.py - combined_results_csv_file = os.path.join(output_dir, "bag_sweep_stats_combined.csv") - output_csv_files = [] - for bag_file in bag_files: - bag_name = os.path.splitext(os.path.basename(bag_file))[0] - output_csv_files.append(os.path.join(output_dir, bag_name + "_results.csv")) - combined_dataframe = combined_results(output_csv_files) - combined_dataframe.to_csv(combined_results_csv_file, index=False) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("map_file", help="Full path to map file.") - parser.add_argument("gnc_config", help="Full path to gnc config file.") - parser.add_argument( - "-d", - "--bag_directory", - default=None, - help="Full path to directory containing bag files.", - ) - parser.add_argument( - "-o", - "--output_directory", - default=None, - help="Full path to output directory where files will be saved. If not specified, timestamped directory will be created in current path.", - ) - - parser.add_argument( - "-r", - "--robot_name", - metavar="ROBOT", - help="Specify the robot to use (just name, not path).", - ) - parser.add_argument( - "-i", - "--image_topic", - dest="image_topic", - default=None, - help="Use specified image topic.", - ) - parser.add_argument( - "--save_stats", action="store_true", help="Save stats to csv file." - ) - parser.add_argument( - "--make_plots", type=bool, default=True, help="Make pdf of plots of results." - ) - - args, args_unkonown = parser.parse_known_args() - - bag_directory = args.bag_directory - if bag_directory == None: - bag_directory = os.getcwd() - - if not os.path.exists(bag_directory): - print(("Bag directory {} does not exist.".format(bag_directory))) - exit() - bag_files = utilities.get_files(bag_directory, "*.bag") - print(("Found {} bag files in {}.".format(len(bag_files), bag_directory))) - - output_dir = utilities.create_directory(args.output_directory) - print(("Output directory for results is {}".format(output_dir))) - - bag_sweep(bag_files, output_dir, args) - combine_results_in_csv_file(bag_files, output_dir) diff --git a/tools/ekf_bag/scripts/config_creator.py b/tools/ekf_bag/scripts/config_creator.py deleted file mode 100644 index 546402ddbe..0000000000 --- a/tools/ekf_bag/scripts/config_creator.py +++ /dev/null @@ -1,50 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - - -def check_and_fill_line(value_map, config_file_line): - line_strings = config_file_line.split() - # Overwrite val if config variable is in value map - if len(line_strings) > 0 and line_strings[0] in value_map: - return line_strings[0] + " = " + str(value_map[line_strings[0]]) + "\n" - return config_file_line - - -def fill_in_values(original_gnc_config, value_map, new_gnc_config): - original_config_file = open(original_gnc_config, "r") - new_config_file = open(new_gnc_config, "w") - for config_file_line in original_config_file: - new_config_file.write(check_and_fill_line(value_map, config_file_line)) - - -def make_value_map(values, value_names): - value_map = {} - if len(values) != len(value_names): - print("values and value_names not same length!") - exit() - - for index, value_name in enumerate(value_names): - value_map[value_name] = values[index] - - return value_map - - -def make_config(values, value_names, original_gnc_config, new_gnc_config): - value_map = make_value_map(values, value_names) - fill_in_values(original_gnc_config, value_map, new_gnc_config) diff --git a/tools/ekf_bag/scripts/create_averaged_plots.py b/tools/ekf_bag/scripts/create_averaged_plots.py deleted file mode 100755 index 444e640122..0000000000 --- a/tools/ekf_bag/scripts/create_averaged_plots.py +++ /dev/null @@ -1,66 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import os - -import plot_creator -import utilities - - -def get_subdirectory_files(file_string): - subdirectory_csv_files = [] - _, subdirectories, _ = next(os.walk(os.getcwd())) - for subdirectory in subdirectories: - subdirectory_path = os.path.join(os.getcwd(), subdirectory) - for subdirectory_csv_file in utilities.get_files( - subdirectory_path, file_string - ): - subdirectory_csv_files.append(subdirectory_csv_file) - return subdirectory_csv_files - - -def remove_repeat_job_id_rows(dataframe): - return dataframe.drop_duplicates("job_id") - - -def average_same_jobs(dataframe): - return dataframe.groupby(["job_id"]).mean() - - -# Averages results from all *results.csv files in subdirectories in a directory. Only looks for files at a depth of one (one subdirectory, doesn't search recursively) -if __name__ == "__main__": - dataframes = [] - - results_csv_files = get_subdirectory_files("*results.csv") - results_dataframe = plot_creator.load_dataframe(results_csv_files) - averaged_results_dataframe = average_same_jobs(results_dataframe) - averaged_results_dataframe.reset_index(inplace=True) - dataframes.append(averaged_results_dataframe) - - values_csv_files = get_subdirectory_files("*values.csv") - values_dataframe = plot_creator.load_dataframe(values_csv_files) - values_dataframe.columns.name = "values" - values_dataframe = remove_repeat_job_id_rows(values_dataframe) - dataframes.append(values_dataframe) - a = values_dataframe["job_id"] - - pdf_filename = "averaged_result_plots.pdf" - plot_creator.create_pdf( - dataframes, pdf_filename, True, os.path.basename(os.getcwd()) - ) diff --git a/tools/ekf_bag/scripts/create_plots.py b/tools/ekf_bag/scripts/create_plots.py deleted file mode 100755 index 87fa20b837..0000000000 --- a/tools/ekf_bag/scripts/create_plots.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import argparse -import os - -import plot_creator -import utilities - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument( - "--directory", default=None, help="Directory where results csv files are stored" - ) - parser.add_argument( - "--write_values", - "-v", - action="store_true", - help="Write parameter sweep values to a plot in pdf", - ) - parser.add_argument( - "--write_values_table", - "-t", - action="store_true", - help="Write parameter sweep values to a table in pdf", - ) - - args = parser.parse_args() - if args.write_values_table and not args.write_values: - print("If write_values_table enabled, write_values must be as well.") - exit() - - directory = args.directory - if directory == None: - directory = os.getcwd() - - dataframes = [] - - results_filestring = "*results.csv" - results_files = utilities.get_files(directory, results_filestring) - if len(results_files) == 0: - print(("No results csv files found in directory " + directory)) - exit() - - dataframes.append(plot_creator.load_dataframe(results_files)) - - if args.write_values: - values_filestring = "*values.csv" - values_files = utilities.get_files(directory, values_filestring) - values_dataframe = plot_creator.load_dataframe(values_files) - values_dataframe.columns.name = "values" - dataframes.append(values_dataframe) - - pdf_filename = "result_plots.pdf" - plot_creator.create_pdf( - dataframes, pdf_filename, args.write_values_table, os.path.basename(directory) - ) diff --git a/tools/ekf_bag/scripts/ekf_diff b/tools/ekf_bag/scripts/ekf_diff deleted file mode 100755 index e9e2ab89f0..0000000000 --- a/tools/ekf_bag/scripts/ekf_diff +++ /dev/null @@ -1,601 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import matplotlib -from scipy.spatial.transform import Rotation as R -from scipy.spatial.transform import Slerp - -matplotlib.use('pdf') -import argparse -import copy -import math -import os -import os.path - -import matplotlib.pyplot as plt -from matplotlib.backends.backend_pdf import PdfPages - - -def quat_to_euler(q): - q2q2 = q[1] * q[1] - x = math.atan2(2 * (q[0] * q[3] + q[1] * q[2]), - 1 - 2 * (q[0] * q[0] + q2q2)) - arg = max(-1.0, min(1.0, 2 * (q[1] * q[3] - q[0] * q[2]))) - y = math.asin(arg) - z = math.atan2(2 * (q[0] * q[1] + q[2] * q[3]), - 1 - 2 * (q2q2 + q[2] * q[2])) - return (x, y, z) - -def parse_args(): - epilog_msg = """ - Input > 2 text files containing EKF output - """ - parser = argparse.ArgumentParser( - description='Graph the EKF from a ros bag file.', - epilog=epilog_msg, - formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument( - '-s', - '--start', - dest='start_time', - type=float, - default=float('-inf'), - help='time to start parsing in seconds') - parser.add_argument( - '-e', - '--stop', - dest='stop_time', - type=float, - default=float('inf'), - help='time to stop parsing in seconds') - parser.add_argument( - '-p', - '--period', - dest='period', - type=float, - default=0, - help='minimum time difference between pose estimations in seconds') - parser.add_argument( - '-i', - '--interpolate', - dest='interpolate', - action='store_true', - help='resample data to match the first text file rather than nearest neighbor') - parser.add_argument( - '-r', - '--relative_poses', - dest='relative_poses', - action='store_true', - help='plot relative poses rather than absolute poses') - parser.add_argument('-m', '--map', dest='map', action='store_true', - help='generate a heatmap based on the first text file') - parser.add_argument( - '-sm', - '---sparse_map', - dest='sparse_map', - action='store_true', - help='use the sparse mapping poses instead of ekf poses from the first text file (ignore covariance plots)') - parser.add_argument( - '-l', - '--limit', - dest='interpolation_limit', - type=float, - default=float('0'), - help='if interpolation is enabled, cutoff to interpolate rather than use nearest neighbor') - parser.add_argument('ekf_files', metavar='EKF', nargs='+', - help='text files containing ekf outputs from ekf_graph') - args, args_unknown = parser.parse_known_args() - return args - -def csv_to_dictionaries(args): - ekfs = [] - ekfs.append({'t': [], - 'x': [], - 'y': [], - 'z': [], - 'angle1': [], - 'angle2': [], - 'angle3': [], - 'vx': [], - 'vy': [], - 'vz': [], - 'ox': [], - 'oy': [], - 'oz': [], - 'ax': [], - 'ay': [], - 'az': [], - 'abx': [], - 'aby': [], - 'abz': [], - 'gbx': [], - 'gby': [], - 'gbz': [], - 'c': [], - 's': [], - 'ml_count': [], - 'of_count': [], - 'mahal': []}) - for i in range(1, 16): - ekfs[0]['cov_' + str(i)] = [] - for i in range(len(args.ekf_files) - 1): - ekfs.append(copy.deepcopy(ekfs[0])) - - mahals = [] - mahals.append({'times': [], 'boxes': []}) - for i in range(len(args.ekf_files) - 1): - mahals.append(copy.deepcopy(mahals[0])) - - filenames = [] - original_ekf = [] - - counter = 0 - - # generate a dictionary for each individual text file - for i in range(len(args.ekf_files)): - filenames.append(args.ekf_files[i]) - f1 = open(filenames[i], 'r') - last_time = float('-inf') - original_ekf.append({'x': [], - 'y': [], - 'z': [], - 'angle1': [], - 'angle2': [], - 'angle3': [], - 'cov_1': [], - 'cov_2': [], - 'cov_3': [], - 'cov_7': [], - 'cov_8': [], - 'cov_9': [], - 'cov_13': [], - 'cov_14': [], - 'cov_15': []}) - for l in f1: - p = l.split(' ') - if ( - i != 0 or not args.sparse_map) and l.startswith('EKF ') and float( - p[1]) >= args.start_time and float( - p[1]) <= args.stop_time and float( - p[1]) - last_time > args.period: - last_time = float(p[1]) - ekfs[i]['t'] .append(float(p[1])) - ekfs[i]['x'] .append(float(p[2])) - ekfs[i]['y'] .append(float(p[3])) - ekfs[i]['z'] .append(float(p[4])) - ekfs[i]['angle1'] .append(float(p[5]) * 180 / math.pi) - ekfs[i]['angle2'] .append(float(p[6]) * 180 / math.pi) - ekfs[i]['angle3'] .append(float(p[7]) * 180 / math.pi) - ekfs[i]['vx'] .append(float(p[8])) - ekfs[i]['vy'] .append(float(p[9])) - ekfs[i]['vz'] .append(float(p[10])) - ekfs[i]['ox'] .append(float(p[11]) * 180 / math.pi) - ekfs[i]['oy'] .append(float(p[12]) * 180 / math.pi) - ekfs[i]['oz'] .append(float(p[13]) * 180 / math.pi) - ekfs[i]['ax'] .append(float(p[14])) - ekfs[i]['ay'] .append(float(p[15])) - ekfs[i]['az'] .append(float(p[16])) - ekfs[i]['abx'] .append(float(p[17])) - ekfs[i]['aby'] .append(float(p[18])) - ekfs[i]['abz'] .append(float(p[19])) - ekfs[i]['gbx'] .append(float(p[20]) * 180 / math.pi) - ekfs[i]['gby'] .append(float(p[21]) * 180 / math.pi) - ekfs[i]['gbz'] .append(float(p[22]) * 180 / math.pi) - ekfs[i]['c'] .append(int(p[23])) - ekfs[i]['s'] .append(int(p[24])) - ekfs[i]['ml_count'] .append(int(p[25])) - if ekfs[i]['ml_count'][-1] == 0: - ekfs[i]['ml_count'][-1] = float('nan') - ekfs[i]['of_count'] .append(int(p[26])) - if ekfs[i]['of_count'][-1] == 0: - ekfs[i]['of_count'][-1] = float('nan') - for j in range(1, 16): - ekfs[i]['cov_' + - str(j)].append(math.sqrt(abs(float(p[26 + j])))) - # convert quaternion to euler angles - q = (0.5 * ekfs[i]['cov_1'][-1], 0.5 * ekfs[i] - ['cov_2'][-1], 0.5 * ekfs[i]['cov_3'][-1], 1.0) - euler = quat_to_euler(q) - ekfs[i]['cov_1'][-1] = euler[0] - ekfs[i]['cov_2'][-1] = euler[1] - ekfs[i]['cov_3'][-1] = euler[2] - m = [] - MAHAL_MAX = 30.0 - for j in range(50): - t = float(p[42 + j]) - if not math.isnan(t) and t > 0.0: - m.append(min(t, MAHAL_MAX)) - if len(m) > 0: - mahals[i]['times'].append(float(p[1])) - mahals[i]['boxes'].append(m) - ekfs[i]['mahal'].append(m) - # if -r is used, record relative differences between poses - for key in original_ekf[i]: - original_ekf[i][key].append(ekfs[i][key][-1]) - if args.relative_poses and not args.sparse_map and len( - ekfs[i]['t']) > 1: - for key in original_ekf[i]: - ekfs[i][key][-1] = (ekfs[i][key][-1] - - original_ekf[i][key][-2]) - elif i == 0 and args.sparse_map and l.startswith('VL ') and float(p[1]) >= args.start_time and float(p[1]) <= args.stop_time and float(p[1]) - last_time > args.period: - last_time = float(p[1]) - ekfs[i]['t'] .append(float(p[1])) - ekfs[i]['x'] .append(float(p[3])) - ekfs[i]['y'] .append(float(p[4])) - ekfs[i]['z'] .append(float(p[5])) - ekfs[i]['angle1'] .append(float(p[6]) * 180 / math.pi) - ekfs[i]['angle2'] .append(float(p[7]) * 180 / math.pi) - ekfs[i]['angle3'] .append(float(p[8]) * 180 / math.pi) - for j in range(1, 16): - ekfs[i]['cov_' + str(j)].append(0) - # if -r is used, record positions relative to the initial pose to - # fix for vio - for key in original_ekf[i]: - original_ekf[i][key].append(ekfs[i][key][-1]) - - if args.relative_poses and len(ekfs[i]['t']) > 1: - for key in ['t', 'x', 'y', 'z', 'angle1', 'angle2', 'angle3']: - ekfs[i][key][-1] = (ekfs[i][key][-1] - ekfs[i][key][0]) - counter += 1 - - if args.relative_poses: # with relative poses, the starting pose is the origin - ekfs[i]['x'][0] = 0 - ekfs[i]['y'][0] = 0 - ekfs[i]['z'][0] = 0 - ekfs[i]['angle1'][0] = 0 - ekfs[i]['angle2'][0] = 0 - ekfs[i]['angle3'][0] = 0 - return filenames, ekfs, original_ekf - -# record the differences between each possible pair of datasets -def generate_pdfs(filenames, ekfs, original_ekf, args): - for i in range(len(ekfs)): - - # add baseline data to the heatmap (assuming first dataset is ground - # truth/what we want to compare) - if args.map and i == 0: - heatMapData = [[], [], []] - covMapData = {'p': [[], [], []], 'v': [[], [], []], 'o': [[], [], []]} - for j in range(len(ekfs[i]['t'])): - heatMapData[0].append( - ((ekfs[i]['x'][j]**2) + (ekfs[i]['y'][j]**2) + (ekfs[i]['z'][j]**2))**0.5) - heatMapData[1].append([]) - - covMapData['p'][0].append( - (abs(ekfs[i]['cov_13'][j]) + abs(ekfs[i]['cov_14'][j]) + abs(ekfs[i]['cov_15'][j]))**0.5) - covMapData['p'][1].append([]) - covMapData['v'][0].append( - (abs(ekfs[i]['cov_7'][j]) + abs(ekfs[i]['cov_8'][j]) + abs(ekfs[i]['cov_9'][j]))**0.5) - covMapData['v'][1].append([]) - covMapData['o'][0].append( - (abs(ekfs[i]['cov_1'][j]) + abs(ekfs[i]['cov_2'][j]) + abs(ekfs[i]['cov_3'][j]))**0.5) - covMapData['o'][1].append([]) - - for j in range(i + 1, len(ekfs)): - diffs = { - 't': [], - 'x': [], - 'y': [], - 'z': [], - 'angle1': [], - 'angle2': [], - 'angle3': []} - averages = { - 'x': 0, - 'y': 0, - 'z': 0, - 'angle1': 0, - 'angle2': 0, - 'angle3': 0} - n = 0 - for ki in range( - len(ekfs[i]['t'])): # loop through indices in the first dataset - n += 1 - - if ki >= len(ekfs[i]['t']) or ki >= len(ekfs[j]['t']): - break - kj = ki - error = ekfs[i]['t'][ki] - ekfs[j]['t'][ki] - - # use nearest neighbor to pick an index for the second dataset - if error > 0: - error = abs(error) - while kj + 1 < len(ekfs[j]['t']): - newerror = abs(ekfs[i]['t'][ki] - ekfs[j]['t'][kj + 1]) - if newerror < error: - error = newerror - kj += 1 - else: - break - elif error < 0: - error = abs(error) - while kj - 1 >= 0: - newerror = abs(ekfs[i]['t'][ki] - ekfs[j]['t'][kj - 1]) - if newerror < error: - error = newerror - kj -= 1 - else: - break - - diffs['t'].append(ekfs[i]['t'][ki]) - error = ekfs[j]['t'][kj] - ekfs[i]['t'][ki] - - # interpolate data if needed - if args.interpolate and error > args.interpolation_limit: - if error > 0: - dt = ekfs[j]['t'][kj] - ekfs[j]['t'][kj - 1] - currVars = dict() - for var in ['x', 'y', 'z']: - dVar = ekfs[j][var][kj] - ekfs[j][var][kj - 1] - currVars[var] = ekfs[j][var][kj - 1] + \ - dVar * error / dt - diffs[var].append( - abs(ekfs[i][var][ki] - currVars[var])) - - startPos = [ekfs[j]['angle1'][kj - 1], ekfs[j] - ['angle2'][kj - 1], ekfs[j]['angle3'][kj - 1]] - endPos = [ - ekfs[j]['angle1'][kj], - ekfs[j]['angle2'][kj], - ekfs[j]['angle3'][kj]] - key_rots = R.from_euler( - 'xyz', [startPos, endPos], degrees=True) - key_times = [ekfs[j]['t'][kj - 1], ekfs[j]['t'][kj]] - slerp = Slerp(key_times, key_rots) - times = [ekfs[i]['t'][ki]] - interp_rots = slerp(times).as_euler('xyz', degrees=True)[0] - diffs['angle1'].append( - abs(ekfs[i]['angle1'][ki] - interp_rots[0])) - diffs['angle2'].append( - abs(ekfs[i]['angle2'][ki] - interp_rots[1])) - diffs['angle3'].append( - abs(ekfs[i]['angle3'][ki] - interp_rots[2])) - elif error < 0: - dt = ekfs[j]['t'][kj + 1] - ekfs[j]['t'][kj] - currVars = dict() - for var in ['x', 'y', 'z']: - dVar = ekfs[j][var][kj + 1] - ekfs[j][var][kj] - currVars[var] = ekfs[j][var][kj] + dVar * error / dt - diffs[var].append( - abs(ekfs[i][var][ki] - currVars[var])) - - startPos = [ - ekfs[j]['angle1'][kj], - ekfs[j]['angle2'][kj], - ekfs[j]['angle3'][kj]] - endPos = [ekfs[j]['angle1'][kj + 1], ekfs[j] - ['angle2'][kj + 1], ekfs[j]['angle3'][kj + 1]] - key_rots = R.from_euler( - 'xyz', [startPos, endPos], degrees=True) - key_times = [ekfs[j]['t'][kj], ekfs[j]['t'][kj + 1]] - slerp = Slerp(key_times, key_rots) - times = [ekfs[i]['t'][ki]] - interp_rots = slerp(times).as_euler('xyz', degrees=True)[0] - diffs['angle1'].append( - abs(ekfs[i]['angle1'][ki] - interp_rots[0])) - diffs['angle2'].append( - abs(ekfs[i]['angle2'][ki] - interp_rots[1])) - diffs['angle3'].append( - abs(ekfs[i]['angle3'][ki] - interp_rots[2])) - if args.map and i == 0: # add current point to the heatmap - heatMapData[1][ki].append( - ((currVars['x']**2) + (currVars['y']**2) + (currVars['z']**2))**0.5) - else: # store nearest neighbor data - diffs['x'].append(abs(ekfs[i]['x'][ki] - ekfs[j]['x'][kj])) - diffs['y'].append(abs(ekfs[i]['y'][ki] - ekfs[j]['y'][kj])) - diffs['z'].append(abs(ekfs[i]['z'][ki] - ekfs[j]['z'][kj])) - diffs['angle1'].append( - abs(ekfs[i]['angle1'][ki] - ekfs[j]['angle1'][kj])) - diffs['angle2'].append( - abs(ekfs[i]['angle2'][ki] - ekfs[j]['angle2'][kj])) - diffs['angle3'].append( - abs(ekfs[i]['angle3'][ki] - ekfs[j]['angle3'][kj])) - - if args.map and i == 0: # add current point to the heatmap - heatMapData[1][ki].append( - ((ekfs[j]['x'][kj]**2) + (ekfs[j]['y'][kj]**2) + (ekfs[j]['z'][kj]**2))**0.5) - - if args.map and i == 0: # add covariances to their respective heatmaps - covMapData['p'][1][ki].append( - (abs(ekfs[j]['cov_13'][kj]) + abs(ekfs[j]['cov_14'][kj]) + abs(ekfs[j]['cov_15'][kj]))**0.5) - covMapData['v'][1][ki].append( - (abs(ekfs[j]['cov_7'][kj]) + abs(ekfs[j]['cov_8'][kj]) + abs(ekfs[j]['cov_9'][kj]))**0.5) - covMapData['o'][1][ki].append( - (abs(ekfs[j]['cov_1'][kj]) + abs(ekfs[j]['cov_2'][kj]) + abs(ekfs[j]['cov_3'][kj]))**0.5) - - averages['x'] += diffs['x'][-1]**2 - averages['y'] += diffs['y'][-1]**2 - averages['z'] += diffs['z'][-1]**2 - averages['angle1'] += diffs['angle1'][-1]**2 - averages['angle2'] += diffs['angle2'][-1]**2 - averages['angle3'] += diffs['angle3'][-1]**2 - - # add graphs and RMSE data to the pdf for this pair - with PdfPages(filenames[i][:filenames[i].rfind('.')] + '_' + filenames[j][:filenames[j].rfind('.')] + ".pdf") as pdf: - plt.figure() - plt.plot( - diffs['t'], - diffs['x'], - 'r', - linewidth=0.5, - label='EKF Pos. Dif. (X)') - plt.plot( - diffs['t'], - diffs['y'], - 'b', - linewidth=0.5, - label='EKF Pos. Dif. (Y)') - plt.plot( - diffs['t'], - diffs['z'], - 'g', - linewidth=0.5, - label='EKF Pos. Dif. (Z)') - plt.xlabel('Time (s)') - plt.ylabel('Position Difference (m)') - plt.title('Absolute Position Difference') - plt.legend(prop={"size": 6}) - plt.autoscale(True) - pdf.savefig() - plt.close() - - plt.figure() - plt.plot( - diffs['t'], - diffs['angle1'], - 'r', - linewidth=0.5, - label='EKF Angle Dif. (1)') - plt.plot( - diffs['t'], - diffs['angle2'], - 'b', - linewidth=0.5, - label='EKF Angle Dif. (2)') - plt.plot( - diffs['t'], - diffs['angle3'], - 'g', - linewidth=0.5, - label='EKF Angle Dif. (3)') - plt.xlabel('Time (s)') - plt.ylabel(r'Angle Difference ($^\circ$)') - plt.title('Absolute Angle Difference') - plt.legend(prop={"size": 6}) - plt.autoscale(True) - pdf.savefig() - plt.close() - - plt.figure() - plt.axis('off') - text = "" - for key in averages: - averages[key] = (averages[key] / n)**0.5 - text += key + " RMSE: " + str(averages[key]) + "\n" - plt.text(0.0, 0.5, text) - pdf.savefig() - plt.close() - - # draw the heatmap - if args.map and i == 0: - # average the data at each point - for j in range(len(heatMapData[1])): - heatMapData[1][j] = sum(heatMapData[1][j]) / len(heatMapData[1][j]) - heatMapData[2].append(abs(heatMapData[0][j] - heatMapData[1][j])) - for key in covMapData: - covMapData[key][1][j] = sum( - covMapData[key][1][j]) / len(covMapData[key][1][j]) - covMapData[key][2].append(abs(covMapData[key][1][j])) - - maxData = max(heatMapData[2]) - covMaxData = {'p': 0, 'v': 0, 'o': 0} - for key in covMapData: - covMaxData[key] = max(covMapData[key][2]) - - with PdfPages(filenames[i][:filenames[i].rfind('.')] + '_heatmap' + ".pdf") as pdf: - plt.figure() - for j in range(len(heatMapData[2])): - x, y = original_ekf[0]['x'][j], original_ekf[0]['y'][j] - red = heatMapData[2][j] / maxData if maxData != 0 else 0 - blue = 1 - red - plt.plot(x, y, color=(red, 0, blue), marker='.') - plt.title('Pose Estimation Error in XY-Plane') - plt.xlabel('X (m)') - plt.ylabel('Y (m)') - plt.xlim(-20, 20) - plt.ylim(-20, 20) - pdf.savefig() - plt.close() - - plt.figure() - for j in range(len(heatMapData[2])): - x, z = original_ekf[0]['x'][j], original_ekf[0]['z'][j] - red = heatMapData[2][j] / maxData if maxData != 0 else 0 - blue = 1 - red - plt.plot(x, z, color=(red, 0, blue), marker='.') - plt.title('Pose Estimation Error in XZ-Plane') - plt.xlabel('X (m)') - plt.ylabel('Z (m)') - plt.xlim(-20, 20) - plt.ylim(-20, 20) - pdf.savefig() - plt.close() - - plt.figure() - for j in range(len(heatMapData[2])): - y, z = original_ekf[0]['y'][j], original_ekf[0]['z'][j] - red = heatMapData[2][j] / maxData if maxData != 0 else 0 - blue = 1 - red - plt.plot(y, z, color=(red, 0, blue), marker='.') - plt.title('Pose Estimation Error in YZ-Plane') - plt.xlabel('Y (m)') - plt.ylabel('Z (m)') - plt.xlim(-20, 20) - plt.ylim(-20, 20) - pdf.savefig() - plt.close() - - plt.figure() - for j in range(len(heatMapData[2])): - x, y = original_ekf[0]['x'][j], original_ekf[0]['y'][j] - red = covMapData['p'][2][j] / \ - covMaxData['p'] if covMaxData['p'] != 0 else 0 - blue = 1 - red - plt.plot(x, y, color=(red, 0, blue), marker='.') - plt.title('Position Covariance in XY-Plane') - plt.xlabel('X (m)') - plt.ylabel('Y (m)') - plt.xlim(-20, 20) - plt.ylim(-20, 20) - pdf.savefig() - plt.close() - - plt.figure() - for j in range(len(heatMapData[2])): - x, y = original_ekf[0]['x'][j], original_ekf[0]['y'][j] - red = covMapData['v'][2][j] / \ - covMaxData['v'] if covMaxData['v'] != 0 else 0 - blue = 1 - red - plt.plot(x, y, color=(red, 0, blue), marker='.') - plt.title('Velocity Covariance in XY-Plane') - plt.xlabel('X (m)') - plt.ylabel('Y (m)') - plt.xlim(-20, 20) - plt.ylim(-20, 20) - pdf.savefig() - plt.close() - - plt.figure() - for j in range(len(heatMapData[2])): - x, y = original_ekf[0]['x'][j], original_ekf[0]['y'][j] - red = covMapData['o'][2][j] / \ - covMaxData['o'] if covMaxData['o'] != 0 else 0 - blue = 1 - red - plt.plot(x, y, color=(red, 0, blue), marker='.') - plt.title('Orientation Covariance in XY-Plane') - plt.xlabel('X (m)') - plt.ylabel('Y (m)') - plt.xlim(-20, 20) - plt.ylim(-20, 20) - pdf.savefig() - plt.close() - -args = parse_args() -filenames, ekfs, original_ekf = csv_to_dictionaries(args) -generate_pdfs(filenames, ekfs, original_ekf, args) diff --git a/tools/ekf_bag/scripts/ekf_graph b/tools/ekf_bag/scripts/ekf_graph deleted file mode 100755 index f2f4558554..0000000000 --- a/tools/ekf_bag/scripts/ekf_graph +++ /dev/null @@ -1,81 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import argparse -import os - -import ekf_graph - -if __name__ == '__main__': - epilog_msg = """ - The arguments BAG or MAP could be ommitted if specificed with the following - environement variables: - - ASTROBEE_BAG instead of positional argument 1 BAG - - ASTROBEE_MAP instead of positional argument 2 MAP - - If [-r ROBOT] in not used, then the robot configuration file should be copied - locally as 'robot.config'. - """ - parser = argparse.ArgumentParser(description='Graph the EKF from a ros bag file.', - epilog=epilog_msg, - formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument('-e', - '--ekf_in_bag', - dest='ekf_in_bag', - action='store_true', - help='Use EKF message from bag instead of running EKF on images.') - parser.add_argument('-f', - '--features_in_bag', - dest='features_in_bag', - action='store_true', - help='Use feature message from bag in EKF instead of generating from images.') - parser.add_argument('-r', - '--robot_name', - dest='robot_name', - metavar='ROBOT', - help='Specify the robot to use (just name, not path).') - parser.add_argument('-i', '--image_topic', dest='image_topic', default=None, help='Use specified image topic.') - parser.add_argument('--start', - dest='start_time', - type=float, - default=float('-inf'), - help='Only graph starting from this time.') - parser.add_argument('--end', - dest='end_time', - type=float, - default=float('inf'), - help='Only graph ending from this time.') - parser.add_argument('bag_file', metavar='BAG', nargs='?', help='bag to process') - parser.add_argument('map_file', metavar='MAP', nargs='?', help='map to use') - parser.add_argument('--cached', - dest='cached', - action='store_true', - help='Do not rerun EKF, draw graph only from text file.') - parser.add_argument('--save_stats', action='store_true', help='Save stats to csv file.') - parser.add_argument('--make_plots', type=bool, default=True, help='Make pdf of plots of results.') - - args, args_unkonown = parser.parse_known_args() - name_prefix = os.path.splitext(args.bag_file)[0] - ekf_output_file = name_prefix + '.txt' - pdf_output_file = name_prefix + '.pdf' - results_csv_output_file = name_prefix + '_results.csv' - options = ekf_graph.RunEKFOptions(args.bag_file, args.map_file, ekf_output_file, pdf_output_file, - results_csv_output_file) - options.set_command_line_params(args) - ekf_graph.run_ekf_and_save_stats(options) diff --git a/tools/ekf_bag/scripts/ekf_graph.py b/tools/ekf_bag/scripts/ekf_graph.py deleted file mode 100755 index 612e33e55e..0000000000 --- a/tools/ekf_bag/scripts/ekf_graph.py +++ /dev/null @@ -1,1202 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import csv -import math -import os -import sys -import time - -import environment -import matplotlib - -matplotlib.use("pdf") -import matplotlib.pyplot as plt -import numpy as np -import scipy.spatial.transform -from matplotlib.backends.backend_pdf import PdfPages -from numpy.linalg import norm - - -def run_ekf( - astrobee_map, - astrobee_bag, - output_file, - ekf_in_bag=False, - features_in_bag=False, - image_topic=None, - gnc_config="gnc.config", -): - cmd = ( - "bag_to_csv" - if ekf_in_bag - else "bag_to_csv --run_ekf %s" % ("" if features_in_bag else "--gen_features") - ) - - robot_bias = os.path.dirname(os.path.abspath(astrobee_bag)) + "/imu_bias.config" - cmd = "rosrun ekf_bag %s %s %s %s %s" % ( - cmd, - astrobee_map, - astrobee_bag, - output_file, - gnc_config, - ) - print(("Not running EKF." if ekf_in_bag else "Running EKF.")) - if not ekf_in_bag: - print( - ( - "Using features from bag." - if features_in_bag - else "Generating features from image." - ) - ) - if os.path.isfile(robot_bias): - cmd += " " + robot_bias - if image_topic is not None: - cmd += " --image_topic " + image_topic - os.system(cmd) - - -def quat_to_euler(q): - q2q2 = q[1] * q[1] - x = math.atan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[0] * q[0] + q2q2)) - arg = max(-1.0, min(1.0, 2 * (q[1] * q[3] - q[0] * q[2]))) - y = math.asin(arg) - z = math.atan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q2q2 + q[2] * q[2])) - return (x, y, z) - - -# RMSE of matrix interpreted as n (dim(rows) vectors of dim(col) -def rmse_matrix(error_matrix): - return np.sqrt(np.mean(np.sum(np.square(error_matrix), axis=1))) - - -# Removes the ith entry of a_xs, a_ys, and a_zs if a_times[i] is -# not in b_times[i]. Assumes timestamp vectors are sorted -def prune_missing_timestamps(a_xs, a_ys, a_zs, a_times, b_times): - a_index = 0 - missing_indices = [] - pruned_a_matrix = np.empty(shape=(len(b_times), 3)) - for b_index, b_time in enumerate(b_times): - while ( - not np.isclose(a_times[a_index], b_time, rtol=0) - and a_times[a_index] < b_time - and a_index < len(a_times) - ): - a_index += 1 - if not np.isclose(a_times[a_index], b_time, rtol=0, atol=0.02): - print("Failed to find a time close to b time.") - missing_indices.append(b_index) - pruned_a_matrix[b_index] = np.array( - [a_xs[a_index], a_ys[a_index], a_zs[a_index]] - ) - return pruned_a_matrix, missing_indices - - -def orientation_rmse(a_xs, a_ys, a_zs, b_xs, b_ys, b_zs, a_times, b_times): - a_index = 0 - mean_squared_orientation_error = 0 - for b_index, b_time in enumerate(b_times): - while ( - not np.isclose(a_times[a_index], b_time, rtol=0) - and a_times[a_index] < b_time - and a_index < len(a_times) - ): - a_index += 1 - if not np.isclose(a_times[a_index], b_time, rtol=0, atol=0.02): - print("Failed to find a time close to b time.") - continue - a_rot = scipy.spatial.transform.Rotation.from_euler( - "ZYX", [a_xs[a_index], a_ys[a_index], a_zs[a_index]], degrees=False - ) - b_rot = scipy.spatial.transform.Rotation.from_euler( - "ZYX", [b_xs[b_index], b_ys[b_index], b_zs[b_index]], degrees=False - ) - rot_diff = a_rot.inv() * b_rot - rot_squared_error = np.inner(rot_diff.as_rotvec(), rot_diff.as_rotvec()) - mean_squared_orientation_error += ( - rot_squared_error - mean_squared_orientation_error - ) / (b_index + 1) - return math.sqrt(mean_squared_orientation_error) - - -# RMSE between two sequences of timestamped positions. Prunes timestamped positions in sequence a -# not present in sequence b. -def rmse_timestamped_sequences(a_xs, a_ys, a_zs, a_times, b_xs, b_ys, b_zs, b_times): - a_positions, missing_indices = prune_missing_timestamps( - a_xs, a_ys, a_zs, a_times, b_times - ) - b_positions = np.column_stack((b_xs, b_ys, b_zs)) - for i in missing_indices: - a_positions = np.delete(a_positions, i, 0) - b_positions = np.delete(b_positions, i, 0) - return rmse_matrix(a_positions - b_positions) - - -# TODO: This does some sort of l2 on stddevs, is this what we want? -def covariance_map(ekf, cov_1_index, cov_2_index, cov_3_index): - return [ - math.sqrt(x_y_z[0] + x_y_z[1] + x_y_z[2]) - for x_y_z in zip(ekf[cov_1_index], ekf[cov_2_index], ekf[cov_3_index]) - ] - - -def integrated_velocities(start_x, vs, ts): - delta_times = [j - i for i, j in zip(ts[:-1], ts[1:])] - # Make sure times are same length as velocities, ignore last velocity - delta_times.append(0) - x_increments = [velocity * delta_t for velocity, delta_t in zip(vs, delta_times)] - cumulative_x_increments = np.cumsum(x_increments) - xs = [ - start_x + cumulative_x_increment - for cumulative_x_increment in cumulative_x_increments - ] - return xs - - -class EkfLog(object): - def __init__(self, filename, start_time=float("-inf"), end_time=float("inf")): - self.ekf = { - "t": [], - "x": [], - "y": [], - "z": [], - "angle1": [], - "angle2": [], - "angle3": [], - "vx": [], - "vy": [], - "vz": [], - "ox": [], - "oy": [], - "oz": [], - "ax": [], - "ay": [], - "az": [], - "abx": [], - "aby": [], - "abz": [], - "gbx": [], - "gby": [], - "gbz": [], - "c": [], - "s": [], - "ml_count": [], - "of_count": [], - "mahal": [], - } - for i in range(1, 16): - self.ekf["cov_" + str(i)] = [] - - self.mahal = {"times": [], "boxes": []} - self.CAMERA_RES = (1280, 960) - self.HEATMAP_SIZE = (self.CAMERA_RES[0] / 32, self.CAMERA_RES[1] / 32) - self.vl_heatmap = np.zeros((self.HEATMAP_SIZE[0] + 1, self.HEATMAP_SIZE[1] + 1)) - self.of_heatmap = np.zeros((self.HEATMAP_SIZE[0] + 1, self.HEATMAP_SIZE[1] + 1)) - - self.vl = { - "t": [], - "count": [], - "x": [], - "y": [], - "z": [], - "angle1": [], - "angle2": [], - "angle3": [], - } - self.of = {"t": [], "count": [], "oldest": [], "youngest": [], "median": []} - self.gt = { - "t": [], - "x": [], - "y": [], - "z": [], - "angle1": [], - "angle2": [], - "angle3": [], - } - - f = open(filename, "r") - for l in f: - p = l.split(" ") - if l.startswith("EKF "): - t = float(p[1]) - if t < start_time or t > end_time: - continue - self.ekf["t"].append(float(p[1])) - self.ekf["x"].append(float(p[2])) - self.ekf["y"].append(float(p[3])) - self.ekf["z"].append(float(p[4])) - self.ekf["angle1"].append(float(p[5]) * 180 / math.pi) - self.ekf["angle2"].append(float(p[6]) * 180 / math.pi) - self.ekf["angle3"].append(float(p[7]) * 180 / math.pi) - self.ekf["vx"].append(float(p[8])) - self.ekf["vy"].append(float(p[9])) - self.ekf["vz"].append(float(p[10])) - self.ekf["ox"].append(float(p[11]) * 180 / math.pi) - self.ekf["oy"].append(float(p[12]) * 180 / math.pi) - self.ekf["oz"].append(float(p[13]) * 180 / math.pi) - self.ekf["ax"].append(float(p[14])) - self.ekf["ay"].append(float(p[15])) - self.ekf["az"].append(float(p[16])) - self.ekf["abx"].append(float(p[17])) - self.ekf["aby"].append(float(p[18])) - self.ekf["abz"].append(float(p[19])) - self.ekf["gbx"].append(float(p[20]) * 180 / math.pi) - self.ekf["gby"].append(float(p[21]) * 180 / math.pi) - self.ekf["gbz"].append(float(p[22]) * 180 / math.pi) - self.ekf["c"].append(int(p[23])) - self.ekf["s"].append(int(p[24])) - self.ekf["ml_count"].append(int(p[25])) - if self.ekf["ml_count"][-1] == 0: - self.ekf["ml_count"][-1] = float("nan") - self.ekf["of_count"].append(int(p[26])) - if self.ekf["of_count"][-1] == 0: - self.ekf["of_count"][-1] = float("nan") - for i in range(1, 16): - self.ekf["cov_" + str(i)].append(math.sqrt(abs(float(p[26 + i])))) - # convert quaternion to euler angles - # TODO: shouldn't be using this for covariances - q = ( - 0.5 * self.ekf["cov_1"][-1], - 0.5 * self.ekf["cov_2"][-1], - 0.5 * self.ekf["cov_3"][-1], - 1.0, - ) - euler = quat_to_euler(q) - self.ekf["cov_1"][-1] = euler[0] - self.ekf["cov_2"][-1] = euler[1] - self.ekf["cov_3"][-1] = euler[2] - m = [] - MAHAL_MAX = 30.0 - for i in range(50): - t = float(p[42 + i]) - if not math.isnan(t) and t > 0.0: - m.append(min(t, MAHAL_MAX)) - if len(m) > 0: - self.mahal["times"].append(float(p[1])) - self.mahal["boxes"].append(m) - self.ekf["mahal"].append(m) - elif l.startswith("OF ") and (len(p) - 3) / 4 >= 1: - t = float(p[1]) - if t > 0.0: - self.of["t"].append(t) - self.of["count"].append(int(p[2])) - times = [] - for i in range((len(p) - 3) / 4): - of_id = float(p[3 + 4 * i + 0]) - origt = float(p[3 + 4 * i + 1]) - u = float(p[3 + 4 * i + 2]) - v = float(p[3 + 4 * i + 3]) - times.append(self.of["t"][-1] - origt) - self.of_heatmap[ - int(round(u / self.CAMERA_RES[0] * self.HEATMAP_SIZE[0])) - + self.HEATMAP_SIZE[0] / 2, - int(round(v / self.CAMERA_RES[1] * self.HEATMAP_SIZE[1])) - + self.HEATMAP_SIZE[1] / 2, - ] += 1 - self.of["oldest"].append(np.max(times)) - self.of["youngest"].append(np.min(times)) - self.of["median"].append(np.median(times)) - elif l.startswith("VL ") and (len(p) - 9) / 5 >= 1: - if float(p[1]) > -1.0: - self.vl["t"].append(float(p[1])) - self.vl["count"].append(int(p[2])) - self.vl["x"].append(float(p[3])) - self.vl["y"].append(float(p[4])) - self.vl["z"].append(float(p[5])) - self.vl["angle1"].append(float(p[6]) * 180 / math.pi) - self.vl["angle2"].append(float(p[7]) * 180 / math.pi) - self.vl["angle3"].append(float(p[8]) * 180 / math.pi) - for i in range((len(p) - 9) / 5): - u = float(p[9 + 5 * i + 0]) - v = float(p[9 + 5 * i + 1]) - x = float(p[9 + 5 * i + 2]) - y = float(p[9 + 5 * i + 3]) - z = float(p[9 + 5 * i + 4]) - self.vl_heatmap[ - int(round(u / self.CAMERA_RES[0] * self.HEATMAP_SIZE[0])) - + self.HEATMAP_SIZE[0] / 2, - int(round(v / self.CAMERA_RES[1] * self.HEATMAP_SIZE[1])) - + self.HEATMAP_SIZE[1] / 2, - ] += 1 - elif l.startswith("GT "): - self.gt["t"].append(float(p[1])) - self.gt["x"].append(float(p[2])) - self.gt["y"].append(float(p[3])) - self.gt["z"].append(float(p[4])) - self.gt["angle1"].append(float(p[5]) * 180 / math.pi) - self.gt["angle2"].append(float(p[6]) * 180 / math.pi) - self.gt["angle3"].append(float(p[7]) * 180 / math.pi) - for key in list(self.ekf.keys()): - self.ekf[key] = np.array(self.ekf[key]) - # self.vl_heatmap = self.vl_heatmap / np.amax(self.vl_heatmap) - # self.of_heatmap = self.of_heatmap / np.amax(self.of_heatmap) - - def correct_ground_truth(self): - # find the best time offset for ground truth - best_error = float("inf") - best_offset = 0 - for i in range(125): - offset = (i - 62.5) / 62.5 - (pos_err, angle_err) = self.evaluate_error(offset) - err = 100 * pos_err + angle_err # 1 cm = 1 degree error - if err < best_error: - best_error = err - best_offset = offset - - # now actually do the shift - for i in range(len(self.gt["t"])): - self.gt["t"][i] += best_offset - return best_offset - - def evaluate_error(self, time_offset=0.0): - pos_err = 0.0 - angle_err = 0.0 - err_count = 0 - ekf_i = 0 - for gt_i in range(len(self.gt["t"])): - t = self.gt["t"][gt_i] + time_offset - while ekf_i < len(self.ekf["t"]) - 1 and self.ekf["t"][ekf_i] < t: - ekf_i += 1 - if ekf_i >= len(self.ekf["t"]) - 1: - break - if ekf_i == 0: - continue - # now gt_i falls between ekf_i - 1 and ekf_i, we will interpolate for position - u = (t - self.ekf["t"][ekf_i - 1]) / ( - self.ekf["t"][ekf_i] - self.ekf["t"][ekf_i - 1] - ) - x1 = np.array( - [ - self.ekf["x"][ekf_i - 1], - self.ekf["y"][ekf_i - 1], - self.ekf["z"][ekf_i - 1], - ] - ) - x2 = np.array( - [self.ekf["x"][ekf_i], self.ekf["y"][ekf_i], self.ekf["z"][ekf_i]] - ) - x = x1 + u * (x2 - x1) - # but just use the older angle, not worth trouble to interpolate in python - a = np.array( - [ - self.ekf["angle1"][ekf_i - 1], - self.ekf["angle2"][ekf_i - 1], - self.ekf["angle3"][ekf_i - 1], - ] - ) - - err_count += 1 - pos_err += ( - float( - norm( - np.array( - [self.gt["x"][gt_i], self.gt["y"][gt_i], self.gt["z"][gt_i]] - ) - - x - ) - ) - ** 2 - ) - # again, not best metric, but good enough for this - angle_err += ( - float( - norm( - np.array( - [ - self.gt["angle1"][gt_i], - self.gt["angle2"][gt_i], - self.gt["angle3"][gt_i], - ] - ) - - a - ) - ) - ** 2 - ) - if err_count == 0: - return (0.0, 0.0) - return (math.sqrt(pos_err / err_count), math.sqrt(angle_err / err_count)) - - def evaluate_features(self): - total_time = self.ekf["t"][len(self.ekf["t"]) - 1] - self.ekf["t"][0] - total_count = sum(self.vl["count"]) - max_gap = 0 - for i in range(1, len(self.vl["t"])): - max_gap = max(max_gap, self.vl["t"][i] - self.vl["t"][i - 1]) - return (total_count / total_time, max_gap) - - def rmse_using_single_image_sparse_mapping(self): - rmse_pos = rmse_timestamped_sequences( - self.ekf["x"], - self.ekf["y"], - self.ekf["z"], - self.ekf["t"], - self.vl["x"], - self.vl["y"], - self.vl["z"], - self.vl["t"], - ) - int_x = integrated_velocities(self.ekf["x"][0], self.ekf["vx"], self.ekf["t"]) - int_y = integrated_velocities(self.ekf["y"][0], self.ekf["vy"], self.ekf["t"]) - int_z = integrated_velocities(self.ekf["z"][0], self.ekf["vz"], self.ekf["t"]) - rmse_int_v = rmse_timestamped_sequences( - int_x, - int_y, - int_z, - self.ekf["t"], - self.vl["x"], - self.vl["y"], - self.vl["z"], - self.vl["t"], - ) - rmse_angle = orientation_rmse( - self.ekf["angle1"], - self.ekf["angle2"], - self.ekf["angle3"], - self.vl["angle1"], - self.vl["angle2"], - self.vl["angle3"], - self.ekf["t"], - self.vl["t"], - ) - return rmse_pos, rmse_angle, rmse_int_v - - def get_stats(self, job_id): - stats = [] - stat_names = [] - - # job id - stats.append(job_id) - stat_names.append("job_id") - - # OF stats - of_count_no_nans = [ - 0 if math.isnan(val) else val for val in self.ekf["of_count"] - ] - avg_num_of_features = np.mean(of_count_no_nans) - stats.append(avg_num_of_features) - stat_names.append("avg_num_of_features") - - num_of_features_added = np.sum(of_count_no_nans) - num_of_features_observed = np.sum(self.of["count"]) - ratio_of_features_added = ( - num_of_features_added / num_of_features_observed - if num_of_features_observed > 0 - else 0 - ) - stats.append(ratio_of_features_added) - stat_names.append("ratio_of_features_added") - - # ML stats - ml_count_no_nans = [ - 0 if math.isnan(val) else val for val in self.ekf["ml_count"] - ] - avg_num_ml_features = np.mean(ml_count_no_nans) - stats.append(avg_num_ml_features) - stat_names.append("avg_num_ml_features") - - num_ml_features_added = np.sum(ml_count_no_nans) - num_ml_features_observed = np.sum(self.vl["count"]) - ratio_ml_features_added = ( - num_ml_features_added / num_ml_features_observed - if num_ml_features_observed > 0 - else 0 - ) - stats.append(ratio_ml_features_added) - stat_names.append("ratio_ml_features_added") - - # Mahal - avg_mahal_distance = ( - np.mean(np.concatenate(self.mahal["boxes"])) - if len(self.mahal["boxes"]) > 0 - else 0 - ) - - stats.append(avg_mahal_distance) - stat_names.append("avg_mahal_distance") - - # Covariances - position_covariances = covariance_map(self.ekf, "cov_13", "cov_14", "cov_15") - avg_position_covariance = np.mean(position_covariances) - stats.append(avg_position_covariance) - stat_names.append("avg_position_covariance") - - orientation_covariances = covariance_map(self.ekf, "cov_1", "cov_2", "cov_3") - avg_orientation_covariance = np.mean(orientation_covariances) - stats.append(avg_orientation_covariance) - stat_names.append("avg_orientation_covariance") - - # Compare with groundtruth (if available) - # rmse_pos_vs_groundtruth, rmse_angle_vs_groundtruth = self.evaluate_error() - # stats.append(rmse_pos_vs_groundtruth) - # stat_names.append('rmse_pos_vs_groundtruth') - # stats.append(rmse_angle_vs_groundtruth) - # stat_names.append('rmse_orientation_vs_groundtruth') - - # Compare with poses from single-image sparse mapping registations - ( - rmse_pos_vs_single_image_sparse_mapping, - rmse_angle_vs_single_image_sparse_mapping, - rmse_int, - ) = self.rmse_using_single_image_sparse_mapping() - stats.append(rmse_int) - stat_names.append("integrated_rmse") - stats.append(rmse_pos_vs_single_image_sparse_mapping) - stat_names.append("rmse_pos_vs_single_image_sparse_mapping") - stats.append(rmse_angle_vs_single_image_sparse_mapping) - stat_names.append("orientation_rmse") - return stats, stat_names - - def write_results_to_csv(self, job_id, results_csv_output_file, bagfile): - with open(results_csv_output_file, "w") as results_file: - writer = csv.writer(results_file) - stats, stat_names = self.get_stats(job_id) - bag_name = os.path.splitext(os.path.basename(bagfile))[0] - # TODO(rsoussan): better way to do this - stats.append(stats[-2]) - stat_names.append("rmse") - stats.append(bag_name) - stat_names.append("Bag") - writer.writerow(stat_names) - writer.writerow(stats) - - def plot(self, filename, output_text): - colors = ["r", "b", "g"] - ekf = self.ekf - gt = self.gt - vl = self.vl - of = self.of - - with PdfPages(filename) as pdf: - # positions - plt.figure() - plt.plot(ekf["t"], ekf["x"], colors[0], linewidth=0.5, label="EKF Pos. (X)") - plt.plot(ekf["t"], ekf["y"], colors[1], linewidth=0.5, label="EKF Pos. (Y)") - plt.plot(ekf["t"], ekf["z"], colors[2], linewidth=0.5, label="EKF Pos. (Z)") - plt.fill_between( - ekf["t"], - ekf["x"] - ekf["cov_13"], - ekf["x"] + ekf["cov_13"], - facecolor=colors[0], - alpha=0.5, - ) - plt.fill_between( - ekf["t"], - ekf["y"] - ekf["cov_14"], - ekf["y"] + ekf["cov_14"], - facecolor=colors[1], - alpha=0.5, - ) - plt.fill_between( - ekf["t"], - ekf["z"] - ekf["cov_15"], - ekf["z"] + ekf["cov_15"], - facecolor=colors[2], - alpha=0.5, - ) - plt.autoscale(False) - plt.plot( - gt["t"], - gt["x"], - color=colors[0], - linewidth=0.5, - dashes=(1, 1), - label="Ground Truth (X)", - ) - plt.plot(gt["t"], gt["y"], color=colors[1], linewidth=0.5, dashes=(1, 1)) - plt.plot(gt["t"], gt["z"], color=colors[2], linewidth=0.5, dashes=(1, 1)) - plt.plot( - vl["t"], - vl["x"], - color=colors[0], - linestyle="None", - marker="o", - markersize=2, - label="Observation (X)", - ) - plt.plot( - vl["t"], - vl["y"], - color=colors[1], - linestyle="None", - marker="o", - markersize=2, - ) - plt.plot( - vl["t"], - vl["z"], - color=colors[2], - linestyle="None", - marker="o", - markersize=2, - ) - plt.xlabel("Time (s)") - plt.ylabel("Position (m)") - plt.title("Position") - plt.legend(prop={"size": 6}) - pdf.savefig() - - # angles - plt.figure() - plt.plot(ekf["t"], ekf["angle1"], colors[0], linewidth=0.5, label="EKF") - plt.plot(ekf["t"], ekf["angle2"], colors[1], linewidth=0.5) - plt.plot(ekf["t"], ekf["angle3"], colors[2], linewidth=0.5) - plt.fill_between( - ekf["t"], - ekf["angle1"] - ekf["cov_1"], - ekf["angle1"] + ekf["cov_1"], - facecolor=colors[0], - alpha=0.5, - ) - plt.fill_between( - ekf["t"], - ekf["angle2"] - ekf["cov_2"], - ekf["angle2"] + ekf["cov_2"], - facecolor=colors[1], - alpha=0.5, - ) - plt.fill_between( - ekf["t"], - ekf["angle3"] - ekf["cov_3"], - ekf["angle3"] + ekf["cov_3"], - facecolor=colors[2], - alpha=0.5, - ) - plt.autoscale(False) - plt.plot( - gt["t"], - gt["angle1"], - color=colors[0], - linewidth=0.25, - dashes=(1, 1), - label="Grount Truth", - ) - plt.plot( - gt["t"], gt["angle2"], color=colors[1], linewidth=0.25, dashes=(1, 1) - ) - plt.plot( - gt["t"], gt["angle3"], color=colors[2], linewidth=0.25, dashes=(1, 1) - ) - plt.plot( - vl["t"], - vl["angle1"], - color=colors[0], - linestyle="None", - marker="o", - markersize=2, - label="Observation", - ) - plt.plot( - vl["t"], - vl["angle2"], - color=colors[1], - linestyle="None", - marker="o", - markersize=2, - ) - plt.plot( - vl["t"], - vl["angle3"], - color=colors[2], - linestyle="None", - marker="o", - markersize=2, - ) - plt.xlabel("Time (s)") - plt.ylabel("Angle ($^\circ$)") - plt.title("Orientation") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # positions - plt.figure() - max_y = 100 - - int_x = integrated_velocities(ekf["x"][0], ekf["vx"], ekf["t"]) - int_y = integrated_velocities(ekf["y"][0], ekf["vy"], ekf["t"]) - int_z = integrated_velocities(ekf["z"][0], ekf["vz"], ekf["t"]) - - plt.plot(ekf["t"], int_x, colors[0], linewidth=0.5, label="EKF Pos. (X)") - plt.plot(ekf["t"], int_y, colors[1], linewidth=0.5, label="EKF Pos. (Y)") - plt.plot(ekf["t"], int_z, colors[2], linewidth=0.5, label="EKF Pos. (Z)") - plt.plot( - vl["t"], - vl["x"], - color=colors[0], - linestyle="None", - marker="o", - markersize=2, - label="Observation (X)", - ) - plt.plot( - vl["t"], - vl["y"], - color=colors[1], - linestyle="None", - marker="o", - markersize=2, - ) - plt.plot( - vl["t"], - vl["z"], - color=colors[2], - linestyle="None", - marker="o", - markersize=2, - ) - plt.xlabel("Time (s)") - plt.ylabel("Integrated Velocity (m)") - plt.title("Integrated Velocity") - plt.legend(prop={"size": 6}) - pdf.savefig() - - # feature counts - plt.figure() - plt.plot( - ekf["t"], - ekf["ml_count"], - linestyle="None", - marker="x", - markersize=6, - color="#FF0000", - label="Integrated VL Features", - ) - plt.plot( - ekf["t"], - ekf["of_count"], - marker="|", - markersize=2, - color="#0000FF", - label="Integrated OF Features", - ) - plt.plot( - vl["t"], - vl["count"], - linestyle="None", - marker=".", - markersize=2, - color="#B300FF", - label="Observed VL Features (at Reg. Time)", - ) - plt.plot( - of["t"], - of["count"], - linestyle="None", - marker=",", - markersize=2, - color="#00FFb3", - label="Observed OF Features (at Reg. Time)", - ) - plt.xlabel("Time (s)") - plt.ylabel("Number of Features") - plt.title("EKF Features") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # mahalnobis distance - plt.figure() - if len(self.mahal["boxes"]) > 0: - boxes = plt.boxplot( - self.mahal["boxes"], - positions=self.mahal["times"], - widths=0.2, - manage_xticks=False, - patch_artist=True, - ) - plt.setp(boxes["whiskers"], color="Black", linewidth=0.25) - plt.setp(boxes["caps"], color="Black", linewidth=0.25) - plt.setp(boxes["medians"], color="Black", linewidth=0.25) - plt.setp(boxes["fliers"], color="r", marker="x", markersize=1) - plt.setp( - boxes["boxes"], color="Black", facecolor="SkyBlue", linewidth=0.25 - ) - plt.title("VL Features Mahalnobis Distances") - plt.xlabel("Time (s)") - plt.ylabel("Mahalnobis Distance") - pdf.savefig() - plt.close() - - # linear velocity and acceleration - plt.figure() - plt.plot( - ekf["t"], ekf["vx"], color=colors[0], linewidth=0.5, label="Velocity" - ) - plt.plot(ekf["t"], ekf["vy"], color=colors[1], linewidth=0.5) - plt.plot(ekf["t"], ekf["vz"], color=colors[2], linewidth=0.5) - plt.fill_between( - ekf["t"], - ekf["vx"] - ekf["cov_7"], - ekf["vx"] + ekf["cov_7"], - facecolor=colors[0], - alpha=0.5, - ) - plt.fill_between( - ekf["t"], - ekf["vy"] - ekf["cov_8"], - ekf["vy"] + ekf["cov_8"], - facecolor=colors[1], - alpha=0.5, - ) - plt.fill_between( - ekf["t"], - ekf["vz"] - ekf["cov_9"], - ekf["vz"] + ekf["cov_9"], - facecolor=colors[2], - alpha=0.5, - ) - plt.title("Velocity") - plt.xlabel("Time (s)") - plt.ylabel("Velocity (m/s)") - plt.ylim(-0.5, 0.5) - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - plt.figure() - plt.plot( - ekf["t"], - ekf["ax"], - color=colors[0], - dashes=(1, 1), - linewidth=0.5, - label="Acceleration", - ) - plt.plot(ekf["t"], ekf["ay"], color=colors[1], dashes=(1, 1), linewidth=0.5) - plt.plot(ekf["t"], ekf["az"], color=colors[2], dashes=(1, 1), linewidth=0.5) - plt.title("Acceleration") - plt.xlabel("Time (s)") - plt.ylabel("Acceleration (m/s$^2$)") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # angle and angular velocity - plt.figure() - ax = plt.gca() - ax.plot(ekf["t"], ekf["angle1"], colors[0], linewidth=0.5, label="Angle") - ax.plot(ekf["t"], ekf["angle2"], colors[1], linewidth=0.5) - ax.plot(ekf["t"], ekf["angle3"], colors[2], linewidth=0.5) - ax2 = ax.twinx() - ax2.plot( - ekf["t"], - ekf["ox"], - color=colors[0], - linewidth=0.5, - dashes=(1, 1), - label="Angular Velocity", - ) - ax2.plot(ekf["t"], ekf["oy"], color=colors[1], linewidth=0.5, dashes=(1, 1)) - ax2.plot(ekf["t"], ekf["oz"], color=colors[2], linewidth=0.5, dashes=(1, 1)) - ax.set_title("Angular Velocity") - ax.set_xlabel("Time (s)") - ax.set_ylabel("Angle ($^\circ$)") - ax2.set_ylabel("Angular Velocity ($^\circ$/s)") - lines, labels = ax.get_legend_handles_labels() - lines2, labels2 = ax2.get_legend_handles_labels() - ax.legend(lines + lines2, labels + labels2, prop={"size": 6}) - pdf.savefig() - plt.close() - - # bias - plt.figure() - ax = plt.gca() - ax.plot( - ekf["t"], - ekf["abx"], - colors[0], - linewidth=0.5, - label="Accelerometer Bias", - ) - ax.plot(ekf["t"], ekf["aby"], colors[1], linewidth=0.5) - ax.plot(ekf["t"], ekf["abz"], colors[2], linewidth=0.5) - ax.fill_between( - ekf["t"], - ekf["abx"] - ekf["cov_10"], - ekf["abx"] + ekf["cov_10"], - facecolor=colors[0], - alpha=0.5, - ) - ax.fill_between( - ekf["t"], - ekf["aby"] - ekf["cov_11"], - ekf["aby"] + ekf["cov_11"], - facecolor=colors[1], - alpha=0.5, - ) - ax.fill_between( - ekf["t"], - ekf["abz"] - ekf["cov_12"], - ekf["abz"] + ekf["cov_12"], - facecolor=colors[2], - alpha=0.5, - ) - ax2 = ax.twinx() - ax2.plot( - ekf["t"], - ekf["gbx"], - color=colors[0], - linewidth=0.5, - dashes=(1, 1), - label="Gyrometer Bias", - ) - ax2.plot( - ekf["t"], ekf["gby"], color=colors[1], linewidth=0.5, dashes=(1, 1) - ) - ax2.plot( - ekf["t"], ekf["gbz"], color=colors[2], linewidth=0.5, dashes=(1, 1) - ) - ax2.fill_between( - ekf["t"], - ekf["gbx"] - ekf["cov_4"], - ekf["gbx"] + ekf["cov_4"], - facecolor=colors[0], - alpha=0.5, - ) - ax2.fill_between( - ekf["t"], - ekf["gby"] - ekf["cov_5"], - ekf["gby"] + ekf["cov_5"], - facecolor=colors[1], - alpha=0.5, - ) - ax2.fill_between( - ekf["t"], - ekf["gbz"] - ekf["cov_6"], - ekf["gbz"] + ekf["cov_6"], - facecolor=colors[2], - alpha=0.5, - ) - ax.set_title("Bias Terms") - ax.set_xlabel("Time (s)") - ax.set_ylabel("Accelerometer Bias (m/s$^2$)") - ax2.set_ylabel("Gyrometer Bias ($^\circ$/s)") - lines, labels = ax.get_legend_handles_labels() - lines2, labels2 = ax2.get_legend_handles_labels() - ax.legend(lines + lines2, labels + labels2, prop={"size": 6}) - pdf.savefig() - plt.close() - - # covariance - plt.figure() - plt.plot( - ekf["t"], - covariance_map(self.ekf, "cov_13", "cov_14", "cov_15"), - colors[0], - linewidth=0.5, - label="Position Covariance", - ) - plt.plot( - ekf["t"], - covariance_map(self.ekf, "cov_7", "cov_8", "cov_9"), - colors[1], - linewidth=0.5, - label="Velocity Covariance", - ) - plt.plot( - ekf["t"], - covariance_map(self.ekf, "cov_1", "cov_2", "cov_3"), - colors[2], - linewidth=0.5, - label="Orientation Covariance", - ) - plt.title("Std. Deviation") - plt.xlabel("Time (s)") - plt.ylabel("Covariance") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # mahalnobis distance histogram - plt.figure() - plt.hist( - [item for sublist in self.mahal["boxes"] for item in sublist], - bins=200, - range=(0, 50), - normed=True, - ) - plt.xlabel("Mahalnobis Distance") - plt.ylabel("pdf") - plt.title("Mahalnobis Distance Histogram") - pdf.savefig() - plt.close() - - plt.figure() - plt.imshow( - np.transpose(self.vl_heatmap), - cmap="hot", - interpolation="nearest", - vmin=0, - vmax=np.amax(self.vl_heatmap), - ) - plt.title("Visual Landmarks Density") - pdf.savefig() - plt.close() - - plt.figure() - plt.plot( - of["t"], of["oldest"], color=colors[0], linewidth=0.5, label="Oldest" - ) - plt.plot( - of["t"], of["median"], color=colors[1], linewidth=0.5, label="Median" - ) - plt.plot( - of["t"], - of["youngest"], - color=colors[2], - linewidth=0.5, - label="Youngest", - ) - plt.xlabel("Time (s)") - plt.ylabel("Optical Flow Feature Age (s)") - plt.title("Optical Flow Feature Age") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - plt.figure() - plt.imshow( - np.transpose(self.of_heatmap), - cmap="hot", - interpolation="nearest", - vmin=0, - vmax=np.amax(self.vl_heatmap), - ) - plt.title("Optical Flow Density") - pdf.savefig() - plt.close() - - plt.figure() - plt.axis("off") - plt.text(0.0, 0.5, output_text) - pdf.savefig() - plt.close() - - -class RunEKFOptions(object): - def __init__( - self, - bag_file, - map_file, - ekf_output_file, - pdf_output_file, - results_csv_output_file, - ): - self.bag_file = bag_file - self.map_file = map_file - self.ekf_output_file = ekf_output_file - self.pdf_output_file = pdf_output_file - self.results_csv_output_file = results_csv_output_file - self.image_topic = None - self.job_id = 0 - self.gnc_config = "gnc.config" - self.robot_name = None - self.start_time = float("-inf") - self.end_time = float("inf") - self.ekf_in_bag = False - self.features_in_bag = False - self.cached = False - self.save_stats = False - self.make_plots = True - - def set_bag_sweep_params(self, args): - self.image_topic = args.image_topic - self.robot_name = args.robot_name - self.gnc_config = args.gnc_config - self.save_stats = args.save_stats - self.make_plots = args.make_plots - - def set_command_line_params(self, args): - self.image_topic = args.image_topic - self.robot_name = args.robot_name - self.start_time = args.start_time - self.end_time = args.end_time - self.ekf_in_bag = args.ekf_in_bag - self.features_in_bag = args.features_in_bag - self.cached = args.cached - self.save_stats = args.save_stats - self.make_plots = args.make_plots - - def set_param_sweep_params( - self, image_topic, job_id, gnc_config, save_stats=True, make_plots=True - ): - self.image_topic = image_topic - self.job_id = job_id - self.gnc_config = gnc_config - self.save_stats = save_stats - self.make_plots = make_plots - - -def run_ekf_and_save_stats(options): - (astrobee_map, astrobee_bag, robot_config) = environment.initialize_environment( - options.map_file, options.bag_file, options.robot_name - ) - - print(("Running EKF for job_id: " + str(options.job_id))) - - start = time.time() - if not options.cached: - run_ekf( - astrobee_map, - astrobee_bag, - options.ekf_output_file, - options.ekf_in_bag, - options.features_in_bag, - options.image_topic, - options.gnc_config, - ) - run_time = time.time() - start - output_text = "Run time: %g\n" % (run_time) - - print( - ( - "Finished running EKF for job_id " - + str(options.job_id) - + " in " - + str(run_time) - + " seconds" - ) - ) - - log = EkfLog(options.ekf_output_file, options.start_time, options.end_time) - - if options.save_stats: - log.write_results_to_csv( - options.job_id, options.results_csv_output_file, astrobee_bag - ) - print(("Printed csv reults for job_id: " + str(options.job_id))) - - if options.make_plots: - offset = log.correct_ground_truth() - (pos_err, angle_err) = log.evaluate_error() - (fps, max_gap) = log.evaluate_features() - output_text += "Ground Truth Time Shift: %g\n" % (offset) - output_text += "RMSE Position Error: %g Angular Error: %g\n" % ( - pos_err, - angle_err, - ) - output_text += "VL Feature Throughput: %g VLs / s Max Gap: %g s" % ( - fps, - max_gap, - ) - print(output_text) - log.plot(options.pdf_output_file, output_text) - print(("Saved results to %s" % (options.pdf_output_file))) diff --git a/tools/ekf_bag/scripts/environment.py b/tools/ekf_bag/scripts/environment.py deleted file mode 100755 index 634b2675d5..0000000000 --- a/tools/ekf_bag/scripts/environment.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import os -import os.path -import sys - -import rospkg - - -# sets all needed environment variables. returns map and bag specified -# by environment variables, if any -def initialize_environment(astrobee_map, astrobee_bag, astrobee_robot): - if astrobee_map == None and "ASTROBEE_MAP" in os.environ: - astrobee_map = os.environ["ASTROBEE_MAP"] - if astrobee_map == None: - print("ASTROBEE_MAP not set.", file=sys.stderr) - sys.exit(0) - if astrobee_bag == None and "ASTROBEE_BAG" in os.environ: - astrobee_bag = os.environ["ASTROBEE_BAG"] - if astrobee_bag == None: - print("ASTROBEE_BAG not set.", file=sys.stderr) - sys.exit(0) - - rospack = rospkg.RosPack() - astrobee_path = "" - try: - astrobee_path = rospack.get_path("astrobee") - except: - print("Failed to find package astrobee.", file=sys.stderr) - sys.exit(0) - - os.environ["ASTROBEE_RESOURCE_DIR"] = astrobee_path + "/resources" - os.environ["ASTROBEE_CONFIG_DIR"] = astrobee_path + "/config" - - if astrobee_robot == None: - robot_config = os.path.dirname(os.path.abspath(astrobee_bag)) - robot_config += "/robot.config" - else: - robot_config = astrobee_path + "/config/robots/" + astrobee_robot + ".config" - - if not os.path.isfile(robot_config): - print( - "Please create the robot config file %s." % (robot_config), file=sys.stderr - ) - sys.exit(0) - - os.environ["ASTROBEE_ROBOT"] = robot_config - os.environ["ASTROBEE_WORLD"] = "granite" - - return (astrobee_map, astrobee_bag, robot_config) diff --git a/tools/ekf_bag/scripts/parameter_sweep.py b/tools/ekf_bag/scripts/parameter_sweep.py deleted file mode 100644 index 75856d0352..0000000000 --- a/tools/ekf_bag/scripts/parameter_sweep.py +++ /dev/null @@ -1,219 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import argparse -import csv -import itertools -import math -import multiprocessing -import os - -import config_creator -import ekf_graph -import numpy as np -import utilities - - -# Run ekf with values. -# Add traceback so errors are forwarded, otherwise -# some errors are suppressed due to the multiprocessing -# library call -@utilities.full_traceback -def test_values( - values, job_id, value_names, output_dir, bag_file, map_file, image_topic, gnc_config -): - new_gnc_config_filepath = os.path.join(output_dir, str(job_id) + "_gnc.config") - config_creator.make_config(values, value_names, gnc_config, new_gnc_config_filepath) - - ekf_output_file = os.path.join(output_dir, str(job_id) + "_ekf_results.txt") - pdf_output_file = os.path.join(output_dir, str(job_id) + "_plots.pdf") - results_csv_output_file = os.path.join(output_dir, str(job_id) + "_results.csv") - - values_file_name = str(job_id) + "_values.csv" - with open(os.path.join(output_dir, values_file_name), "w") as values_file: - id_and_values = (job_id,) + values - id_name_and_value_names = ["job_id"] + value_names - writer = csv.writer(values_file) - writer.writerow(id_name_and_value_names) - writer.writerow(id_and_values) - - options = ekf_graph.RunEKFOptions( - bag_file, map_file, ekf_output_file, pdf_output_file, results_csv_output_file - ) - options.set_param_sweep_params(image_topic, job_id, new_gnc_config_filepath) - ekf_graph.run_ekf_and_save_stats(options) - - -# Helper that unpacks arguments and calls original function -# Aides running jobs in parallel as pool only supports -# passing a single argument to workers -def test_values_helper(zipped_vals): - return test_values(*zipped_vals) - - -def parameter_sweep( - all_value_combos, - value_names, - output_dir, - bag_file, - map_file, - image_topic, - gnc_config, -): - job_ids = list(range(len(all_value_combos))) - num_processes = 6 - pool = multiprocessing.Pool(num_processes) - # izip arguments so we can pass as one argument to pool worker - pool.map( - test_values_helper, - list( - zip( - all_value_combos, - job_ids, - itertools.repeat(value_names), - itertools.repeat(output_dir), - itertools.repeat(bag_file), - itertools.repeat(map_file), - itertools.repeat(image_topic), - itertools.repeat(gnc_config), - ) - ), - ) - - -def make_all_value_combinations(value_ranges): - return list(itertools.product(*value_ranges)) - - -def make_value_ranges(): - value_ranges = [] - value_names = [] - steps = 50 - - # tune_ase_Q_imu - - # q_gyro - # .001 -> 2 deg - # q_gyro_degrees_range = np.logspace(-3, .3, steps, endpoint=True) - # q_gyro_squared_rads_range = [math.radians(deg)**2 for deg in q_gyro_degrees_range] - # value_ranges.append(q_gyro_squared_rads_range) - # value_names.append('q_gyro') - - # q_gyro_bias - # q_gyro_bias_degrees_range = np.logspace(-8, 0, steps, endpoint=True) - # q_gyro_bias_squared_rads_range = [math.radians(bias)**2 for bias in q_gyro_bias_degrees_range] - # value_ranges.append(q_gyro_bias_squared_rads_range) - # value_names.append('q_gyro_bias') - - # q_accel (logspace) - # value_ranges.append(list(np.logspace(-9, -5, steps, endpoint=True))) - # value_names.append('q_accel') - - # q_accel_bias - # value_ranges.append(list(np.logspace(-11, -5, steps, endpoint=True))) - # value_names.append('q_accel_bias') - - # Tune Image Params - - # tun_ase_map_error - # value_ranges.append(list(np.linspace(0, 0.1, steps, endpoint=True))) - # value_names.append('tun_ase_map_error') - - # tun_ase_vis_r_mag - # value_ranges.append(list(np.linspace(2, 6, steps, endpoint=True))) - # value_names.append('tun_ase_vis_r_mag') - # tun_ase_of_r_mag - # value_ranges.append(list(np.linspace(0, 3, steps, endpoint=True))) - # value_names.append('tun_ase_of_r_mag') - - # tun_ase_dock_r_mag - # value_ranges.append(list(np.linspace(1, 3, steps, endpoint=True))) - # value_names.append('tun_ase_dock_r_mag') - - # Other Options - - # tun_ase_mahal_distance_max - value_ranges.append(list(np.linspace(0, 40, steps, endpoint=True))) - value_names.append("tun_ase_mahal_distance_max") - - return value_ranges, value_names - - -def save_values(value_names, values, filename, output_dir): - with open(os.path.join(output_dir, filename), "w") as values_file: - writer = csv.writer(values_file) - writer.writerow(value_names) - writer.writerows(values) - - -def make_values_and_parameter_sweep( - output_dir, bag_file, map_file, image_topic, gnc_config -): - output_dir = utilities.create_directory(output_dir) - print(("Output directory for results is {}".format(output_dir))) - - value_ranges, value_names = make_value_ranges() - save_values(value_names, value_ranges, "individual_value_ranges.csv", output_dir) - - all_value_combos = make_all_value_combinations(value_ranges) - save_values(value_names, all_value_combos, "all_value_combos.csv", output_dir) - - parameter_sweep( - all_value_combos, - value_names, - output_dir, - bag_file, - map_file, - image_topic, - gnc_config, - ) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("bag_file", help="Full path to bagfile.") - parser.add_argument("map_file", help="Full path to map file.") - - parser.add_argument( - "--directory", - default=None, - help="Full path to output directory where files will be saved. If not specified, timestamped directory will be created in current path.", - ) - parser.add_argument( - "--image_topic", - "-i", - default=None, - help="Image topic for images in bagfile. Default topic will be used if not specified.", - ) - - parser.add_argument( - "--gnc_config", - "-g", - default=None, - help="GNC config to use as a base for the parameter sweep. Parameters not swept over will be taken from this file. Default will be gnc.config in astrobee/config", - ) - # TODO(rsoussan): pass number of processes to run and parallel and parameter ranges as arguments - - args = parser.parse_args() - - gnc_config = utilities.get_gnc_config(args.gnc_config) - - make_values_and_parameter_sweep( - args.directory, args.bag_file, args.map_file, args.image_topic, gnc_config - ) diff --git a/tools/ekf_bag/scripts/plot_creator.py b/tools/ekf_bag/scripts/plot_creator.py deleted file mode 100644 index 44424dc58e..0000000000 --- a/tools/ekf_bag/scripts/plot_creator.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import matplotlib.pyplot as plt -import pandas as pd -from matplotlib.backends.backend_pdf import PdfPages - - -def create_pdf(dataframes, pdf_filename, write_values=False, directory="None"): - with PdfPages(pdf_filename) as pdf: - for dataframe in dataframes: - for index in range(len(dataframe.columns)): - plot = create_histogram(dataframe, index) - pdf.savefig(plot) - if write_values and dataframe.columns.name == "values": - pdf.savefig(create_table(dataframe)) - if directory != "None": - pdf.savefig(create_directory_page(directory)) - - -def create_directory_page(directory): - directory_page = plt.figure() - directory_page.text(0.1, 0.9, "Output directory: " + directory) - return directory_page - - -def create_table(dataframe): - sorted_dataframe = dataframe.sort_values("job_id") - figure = plt.figure() - ax = plt.subplot(111) - ax.axis("off") - ax.table( - cellText=sorted_dataframe.values, - colLabels=sorted_dataframe.columns, - bbox=[0, 0, 1, 1], - ) - return figure - - -def create_histogram(dataframe, index): - figure = plt.figure() - job_ids = dataframe["job_id"] - y = dataframe.iloc[:, index] - plt.scatter(job_ids, y, c=y, marker="+", s=150, linewidths=4, cmap=plt.cm.coolwarm) - column_name = dataframe.columns[index] - plt.title(column_name) - return figure - - -def load_dataframe(files): - dataframes = [pd.read_csv(file) for file in files] - dataframe = pd.concat(dataframes) - return dataframe diff --git a/tools/ekf_bag/scripts/rosbag_to_csv b/tools/ekf_bag/scripts/rosbag_to_csv deleted file mode 100755 index 1d5fff5463..0000000000 --- a/tools/ekf_bag/scripts/rosbag_to_csv +++ /dev/null @@ -1,34 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import os -import sys - -import environment - -astrobee_map = None -astrobee_bag = None -if len(sys.argv) >= 2: - astrobee_bag = sys.argv[1] -if len(sys.argv) >= 3: - astrobee_map = sys.argv[2] - -(astrobee_map, astrobee_bag, astrobee_robot) = environment.initialize_environment(astrobee_map, astrobee_bag, None) - -os.system('rosrun ekf_bag ekf_bag %s %s --save_inputs_file=true' % (astrobee_map, astrobee_bag)) diff --git a/tools/ekf_bag/scripts/sparse_map_eval b/tools/ekf_bag/scripts/sparse_map_eval deleted file mode 100755 index 1a42cf1f23..0000000000 --- a/tools/ekf_bag/scripts/sparse_map_eval +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import os -import sys -import time - -import environment - - -def run_sparse_map_bag(astrobee_map, astrobee_bag): - os.system('rosrun ekf_bag sparse_map_bag %s %s' % (astrobee_map, astrobee_bag)) - -astrobee_map = None -astrobee_bag = None - -if len(sys.argv) < 3: - print("Usage: " + sys.argv[0] + " mybag.bag mymap.map") - sys.exit(1) - -astrobee_bag = sys.argv[1] -astrobee_map = sys.argv[2] - -(astrobee_map, astrobee_bag, astrobee_robot) = environment.initialize_environment(astrobee_map, astrobee_bag, None) -name_prefix = astrobee_bag[:astrobee_bag.rfind('.')] - -start = time.time() -run_sparse_map_bag(astrobee_map, astrobee_bag) -run_time = time.time() - start -print 'Run time: %g' % (run_time) - diff --git a/tools/ekf_bag/scripts/streamlit_server.py b/tools/ekf_bag/scripts/streamlit_server.py deleted file mode 100644 index ddb63fbb83..0000000000 --- a/tools/ekf_bag/scripts/streamlit_server.py +++ /dev/null @@ -1,229 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - - -import glob -import os -import subprocess - -import numpy as np -import pandas as pd -import streamlit as st -from pdf2image import convert_from_path - - -def PDFtoJPEG(filename, folder, start=0): - pages = convert_from_path(filename + ".pdf", 500, size=1000) - pageList = [] - for i in range(len(pages)): - pageList.append(folder + "/" + str(start + i) + ".jpg") - pages[i].save(pageList[-1], "JPEG") - os.system("rm " + filename + '".pdf"') - return pageList - - -@st.cache -def ekf_diffJPEGs(files, flags, folder): - oldFiles = glob.glob(folder + "/*") - for f in oldFiles: - os.remove(f) - cmd = "rosrun ekf_bag ekf_diff " - cmd += " ".join(files) - cmd += flags - os.system(cmd) - pageList = [] - for i in range(len(files)): - for j in range(i + 1, len(files)): - filename = files[i][:-4] + "_" + files[j][:-4] - pageList.extend(PDFtoJPEG(filename, folder)) - if "-m" in flags: - filename = files[0][:-4] + "_heatmap" - pageList.extend(PDFtoJPEG(filename, folder, 3)) - return pageList - - -@st.cache -def ekf_graphJPEGs(mapFile, bagFile, flags, folder): - oldFiles = glob.glob(folder + "/*") - for f in oldFiles: - os.remove(f) - cmd = "rosrun ekf_bag ekf_graph " + bagFile + " " + mapFile + flags - os.system(cmd) - return PDFtoJPEG(bagFile[:-4], folder) - - -@st.cache -def ekf_video(mapFile, bagFile, flags, folder): - oldFiles = glob.glob(folder + "/*") - for f in oldFiles: - os.remove(f) - cmd = "rosrun ekf_video ekf_video " + bagFile + " " + mapFile + flags - os.system(cmd) - os.system( - "ffmpeg -i " - + bagFile[:-4] - + ".mkv -c copy " - + folder - + "/" - + bagFile.split("/")[-1][:-4] - + ".mp4" - ) - os.system("rm " + bagFile[:-4] + ".mkv") - return folder + "/" + bagFile.split("/")[-1][:-4] + ".mp4" - - -st.title("Astrobee Pose Estimation Evaluation") -script = st.selectbox( - "Script to Run:", ("ekf_diff", "ekf_graph", "ekf_video", "change branch") -) - -if script == "ekf_diff": - flags = "" - - textFiles = glob.glob("cache/*/*.txt") - - numFiles = st.slider("Number of Input Files", min_value=2, value=2, step=1) - - ekfFiles = [] - for i in range(numFiles): - ekfFiles.append(st.selectbox("File " + str(i), textFiles)) - - if st.sidebar.checkbox("Enable Resampling"): - flags += " -i" - if st.sidebar.checkbox("Use Relative Poses"): - flags += " -r" - if st.sidebar.checkbox("Generate Heatmap"): - flags += " -m" - if st.sidebar.checkbox("Use Sparse Map Pose Ground Truth"): - flags += " -sm" - if st.sidebar.checkbox("Define Start Time"): - flags += " -s " + str(st.sidebar.number_input("", key=1, format="%f")) - if st.sidebar.checkbox("Define Stop Time"): - flags += " -e " + str(st.sidebar.number_input("", key=2, format="%f")) - if st.sidebar.checkbox("Define Period"): - flags += " -p " + str(st.sidebar.number_input("", key=3, format="%f")) - - if st.button("Run"): - fileNames = [] - for ekfFile in ekfFiles: - fileNames.append(ekfFile.split("/")[-1]) - os.system("cp " + ekfFile + " " + fileNames[-1]) - folder = "cache/ekf_diff__" + "__".join(fileNames + flags.split()) - os.system("mkdir -p cache") - os.system("mkdir -p " + folder) - - imageList = ekf_diffJPEGs(fileNames, flags, folder) - - for ekfFile in fileNames: - os.system("rm " + ekfFile) - - for image in imageList: - st.image(image) -elif script == "ekf_graph": - flags = "" - - bagFiles = glob.glob("bags/*.bag") - mapFiles = glob.glob("maps/*.map") - - mapFile = st.selectbox("Map File", mapFiles) - bagFile = st.selectbox("Bag File", bagFiles) - - if st.sidebar.checkbox("Use EKF Messages From Bag"): - flags += " -e" - if st.sidebar.checkbox("Use Feature Messages From Bag"): - flags += " -f" - if st.sidebar.checkbox("Do Not Rerun EKF"): - flags += " --cached" - if st.sidebar.checkbox("Define Start Time"): - flags += " --start " + str(st.sidebar.number_input("", key=1, format="%f")) - if st.sidebar.checkbox("Define Stop Time"): - flags += " --end " + str(st.sidebar.number_input("", key=2, format="%f")) - if st.sidebar.checkbox("Specify Robot"): - flags += " -r " + st.sidebar.text_input( - "Enter Robot Name Without Extension/Path", key=1 - ) - gitHash = str( - subprocess.check_output(["git", "rev-parse", "origin/master"])[:-1], "utf-8" - ) - folder = "cache/ekf_graph__" + "__".join( - [gitHash, mapFile[5:], bagFile[5:]] + flags.split() - ) - if st.sidebar.checkbox("Specify Image Topic", value=True): - flags += " -i " + st.sidebar.text_input( - "", value="/mgt/img_sampler/nav_cam/image_record", key=2 - ) - - if st.button("Run"): - os.system("mkdir -p cache") - os.system("mkdir -p " + folder) - - imageList = ekf_graphJPEGs(mapFile, bagFile, flags, folder) - - os.system( - "mv bags/" + bagFile[5:-4] + ".txt " + folder + "/" + bagFile[5:-4] + ".txt" - ) - - for image in imageList: - st.image(image) -elif script == "ekf_video": - flags = "" - - bagFiles = glob.glob("bags/*.bag") - mapFiles = glob.glob("maps/*.map") - - mapFile = st.selectbox("Map File", mapFiles) - bagFile = st.selectbox("Bag File", bagFiles) - - if st.sidebar.checkbox("Use EKF Messages From Bag"): - flags += " -e" - if st.sidebar.checkbox("Use Feature Messages From Bag"): - flags += " -f" - if st.sidebar.checkbox("Use JEM Mode: Top and Side View"): - flags += " -j" - if st.sidebar.checkbox("Specify Robot"): - flags += " -r " + st.sidebar.text_input( - "Enter Robot Name Without Extension/Path" - ) - folder = "cache/ekf_video__" + "__".join( - [bagFile.split("/")[-1], mapFile.split("/")[-1]] + flags.split() - ) - os.system("mkdir -p " + folder) - if st.sidebar.checkbox("Specify Image Topic", value=True): - flags += " -i " + st.sidebar.text_input( - "", value="/mgt/img_sampler/nav_cam/image_record" - ) - - if st.button("Run"): - videoFile = ekf_video(mapFile, bagFile, flags, folder) - - video_file = open(videoFile, "rb") - video_bytes = video_file.read() - st.video(video_bytes) -elif script == "change branch": - allBranches = str(subprocess.check_output(["git", "branch", "-a"]), "utf-8").split( - "\n" - ) - branches = [] - for branch in allBranches: - if branch[2:17] == "remotes/origin/": - branches.append(branch[17:]) - branch = st.selectbox("Branch", branches) - currBranch = str(subprocess.check_output(["git", "branch"]), "utf-8").split()[1] - if branch != currBranch: - os.system("git checkout " + branch) - if st.button("Build"): - os.system("make -C ../../../../../astrobee_build/native") diff --git a/tools/ekf_bag/scripts/test_ekf_graph.py b/tools/ekf_bag/scripts/test_ekf_graph.py deleted file mode 100644 index 257b681585..0000000000 --- a/tools/ekf_bag/scripts/test_ekf_graph.py +++ /dev/null @@ -1,100 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import copy -import math -import unittest - -import ekf_graph -import numpy as np - - -class TestRMSESequence(unittest.TestCase): - def test_prune_missing_timestamps_beginning_set(self): - a_times = np.arange(10.0) - a_xs = np.arange(10.0) - a_ys = np.arange(10.0) + 1.0 - a_zs = np.arange(10.0) + 2.0 - b_times = np.arange(5.0) - pruned_a = ekf_graph.prune_missing_timestamps( - a_xs, a_ys, a_zs, a_times, b_times - ) - self.assertEqual(len(pruned_a), len(b_times)) - self.assertTrue(np.allclose(pruned_a[:, 0], b_times, rtol=0)) - self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0, rtol=0)) - self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0, rtol=0)) - - def test_prune_missing_timestamps_middle_set(self): - a_times = np.arange(10.0) - a_xs = np.arange(10.0) - a_ys = np.arange(10.0) + 1.0 - a_zs = np.arange(10.0) + 2.0 - b_times = np.arange(3.0, 7.0) - pruned_a = ekf_graph.prune_missing_timestamps( - a_xs, a_ys, a_zs, a_times, b_times - ) - self.assertEqual(len(pruned_a), len(b_times)) - self.assertTrue(np.allclose(pruned_a[:, 0], b_times, rtol=0)) - self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0, rtol=0)) - self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0, rtol=0)) - - def test_prune_missing_timestamps_end_set(self): - a_times = np.arange(10.0) - a_xs = np.arange(10.0) - a_ys = np.arange(10.0) + 1.0 - a_zs = np.arange(10.0) + 2.0 - b_times = np.arange(7.0, 10.0) - pruned_a = ekf_graph.prune_missing_timestamps( - a_xs, a_ys, a_zs, a_times, b_times - ) - self.assertEqual(len(pruned_a), len(b_times)) - self.assertTrue(np.allclose(pruned_a[:, 0], b_times, rtol=0)) - self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0, rtol=0)) - self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0, rtol=0)) - - def test_prune_missing_timestamps_scattered_set(self): - a_times = np.arange(10.0) - a_xs = np.arange(10.0) - a_ys = np.arange(10.0) + 1.0 - a_zs = np.arange(10.0) + 2.0 - b_times = np.array([1.0, 5.0, 6.0, 9.0]) - pruned_a = ekf_graph.prune_missing_timestamps( - a_xs, a_ys, a_zs, a_times, b_times - ) - self.assertEqual(len(pruned_a), len(b_times)) - self.assertTrue(np.allclose(pruned_a[:, 0], b_times, rtol=0)) - self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0, rtol=0)) - self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0, rtol=0)) - - def test_rmse_matrix_ones(self): - ones_mat = np.ones(shape=(5, 3)) - self.assertTrue( - np.isclose(ekf_graph.rmse_matrix(ones_mat), math.sqrt(3.0), rtol=0) - ) - - def test_rmse_matrix_1_2_2(self): - col_0 = np.ones(5) - col_1 = np.full(5, 2.0) - col_2 = np.full(5, 2.0) - mat = np.column_stack((col_0, col_1, col_2)) - self.assertTrue(np.isclose(ekf_graph.rmse_matrix(mat), 3.0, rtol=0)) - - -if __name__ == "__main__": - unittest.main() diff --git a/tools/ekf_bag/scripts/utilities.py b/tools/ekf_bag/scripts/utilities.py deleted file mode 100644 index ccbccab013..0000000000 --- a/tools/ekf_bag/scripts/utilities.py +++ /dev/null @@ -1,69 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import datetime -import glob -import os - -import rospkg - - -# Forward errors so we can recover failures -# even when running commands through multiprocessing -# pooling -def full_traceback(func): - import functools - import traceback - - @functools.wraps(func) - def wrapper(*args, **kwargs): - try: - return func(*args, **kwargs) - except Exception as e: - msg = "{}\n\nOriginal {}".format(e, traceback.format_exc()) - raise type(e)(msg) - - return wrapper - - -def get_files(directory, file_string): - return glob.glob(os.path.join(directory, file_string)) - - -def create_directory(directory=None): - if directory == None: - directory = os.path.join( - os.getcwd(), datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") - ) - if os.path.exists(directory): - print((directory + " already exists!")) - exit() - os.makedirs(directory) - return directory - - -def get_gnc_config(gnc_config): - if gnc_config == None: - # Load from astrobee/config/gnc.confg - astrobee_path = rospkg.RosPack().get_path("astrobee") - gnc_config = os.path.join(astrobee_path, "config/gnc.config") - else: - if not os.path.exists(gnc_config): - print(("GNC config does not exists! {}".format(gnc_config))) - exit() - - return gnc_config diff --git a/tools/ekf_bag/src/ekf_bag.cc b/tools/ekf_bag/src/ekf_bag.cc deleted file mode 100644 index 27f57e3230..0000000000 --- a/tools/ekf_bag/src/ekf_bag.cc +++ /dev/null @@ -1,292 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include - -#include -#include -#include -#include - -namespace ekf_bag { - -EkfBag::EkfBag(const char* bagfile, const char* mapfile, bool run_ekf, bool gen_features, const char* biasfile, - std::string image_topic, const std::string& gnc_config) - : map_(mapfile, true), - loc_(&map_), - run_ekf_(run_ekf), - gen_features_(gen_features), - bias_file_(biasfile), - image_topic_(image_topic), - gnc_config_(gnc_config) { - bag_.open(bagfile, rosbag::bagmode::Read); - // Don't gen features if they are already in the bag - rosbag::View view(bag_); - std::vector connection_infos = view.getConnections(); - gen_features_ = true; - for (const auto info : connection_infos) { - std::string of_features = "/" + std::string(TOPIC_LOCALIZATION_OF_FEATURES); - if (info->topic == of_features) { - gen_features_ = false; - break; - } - } -} - -EkfBag::~EkfBag(void) { bag_.close(); } - -void EkfBag::ReadParams(config_reader::ConfigReader* config) { - config->AddFile("tools/ekf_bag.config"); - config->AddFile(gnc_config_.c_str()); - config->AddFile("cameras.config"); - config->AddFile("geometry.config"); - config->AddFile("localization.config"); - config->AddFile("optical_flow.config"); - - if (!config->ReadFiles()) { - ROS_FATAL("Failed to read config files."); - return; - } - - if (!config->GetReal("sparse_map_delay", &sparse_map_delay_)) ROS_FATAL("sparse_map_delay not specified."); - if (!config->GetReal("of_delay", &of_delay_)) ROS_FATAL("of_delay not specified."); - - ekf_.ReadParams(config); - of_.ReadParams(config); - loc_.ReadParams(config); -} - -void EkfBag::EstimateBias(void) { - Eigen::Vector3f gyro(0, 0, 0), accel(0, 0, 0); - - if (bias_file_ != NULL) { - printf("Using bias file: %s\n", bias_file_); - FILE* f = fopen(bias_file_, "r"); - if (f == NULL) { - fprintf(stderr, "Failed to open bias file %s.\n", bias_file_); - exit(1); - } - int ret = fscanf(f, "%g %g %g\n", &gyro[0], &gyro[1], &gyro[2]); - if (ret != 3) { - fprintf(stderr, "Failed to read bias.\n"); - exit(1); - } - ret = fscanf(f, "%g %g %g\n", &accel[0], &accel[1], &accel[2]); - if (ret != 3) { - fprintf(stderr, "Failed to read bias.\n"); - exit(1); - } - fclose(f); - } else { - std::vector topics; - topics.push_back(std::string("/") + TOPIC_HARDWARE_IMU); - topics.push_back(TOPIC_HARDWARE_IMU); - - // get the start time - rosbag::View temp(bag_, rosbag::TopicQuery(topics)); - ros::Time start = temp.getBeginTime(); - - // look in only the first few seconds - rosbag::View early_imu(bag_, rosbag::TopicQuery(topics), start, start + ros::Duration(5.0)); - - int count = 0; - for (rosbag::MessageInstance const m : early_imu) { - if (m.isType()) { - sensor_msgs::ImuConstPtr imu = m.instantiate(); - accel.x() += imu->linear_acceleration.x; - accel.y() += imu->linear_acceleration.y; - accel.z() += imu->linear_acceleration.z; - gyro.x() += imu->angular_velocity.x; - gyro.y() += imu->angular_velocity.y; - gyro.z() += imu->angular_velocity.z; - count++; - } - } - accel = accel / count; - gyro = gyro / count; - printf("Using Bias:\n"); - printf("%g %g %g\n", gyro[0], gyro[1], gyro[2]); - printf("%g %g %g\n", accel[0], accel[1], accel[2]); - } - ekf_.SetBias(gyro, accel); - ekf_.Reset(); -} - -void EkfBag::UpdateImu(const ros::Time& time, const sensor_msgs::Imu& imu) { - if (!run_ekf_) return; - // send the visual messages when it's time - if (processing_of_ && time >= of_send_time_) { - processing_of_ = false; - UpdateOpticalFlow(of_features_); - } - if (processing_sparse_map_ && time >= vl_send_time_) { - processing_sparse_map_ = false; - UpdateSparseMap(vl_features_); - } - - // Pass a quaternion in to do MGTF gravity correction if needed - ekf_.PrepareStep(imu, ground_truth_.orientation); - ff_msgs::EkfState state; - int ret = ekf_.Step(&state); - if (ret) UpdateEKF(state); -} - -void EkfBag::UpdateImage(const ros::Time& time, const sensor_msgs::ImageConstPtr& image_msg) { - if (!run_ekf_) return; - if (!gen_features_) return; - if (!processing_of_) { - // send of registration - ff_msgs::CameraRegistration r; - r.header = std_msgs::Header(); - r.header.stamp = time; - r.camera_id = ++of_id_; - UpdateOpticalFlowReg(r); - // do the processing now, but send it later - of_features_.feature_array.clear(); - of_.OpticalFlow(image_msg, &of_features_); - of_features_.camera_id = of_id_; - processing_of_ = true; - of_send_time_ = time + ros::Duration(of_delay_); - } - // now do sparse map - if (!processing_sparse_map_) { - // send registration - ff_msgs::CameraRegistration r; - r.header = std_msgs::Header(); - r.header.stamp = time; - r.camera_id = ++vl_id_; - UpdateSparseMapReg(r); - // do processing now, but send it later - cv_bridge::CvImageConstPtr image; - try { - image = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8); - } catch (cv_bridge::Exception& e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - vl_features_.landmarks.clear(); - loc_.Localize(image, &vl_features_); - vl_features_.camera_id = vl_id_; - processing_sparse_map_ = true; - vl_send_time_ = time + ros::Duration(sparse_map_delay_); - } -} - -void EkfBag::UpdateGroundTruth(const geometry_msgs::PoseStamped& pose) { - ground_truth_ = pose.pose; // Cache the pose for MGTF gravity correction -} - -void EkfBag::UpdateOpticalFlow(const ff_msgs::Feature2dArray& of) { - if (run_ekf_) ekf_.OpticalFlowUpdate(of); -} - -void EkfBag::UpdateSparseMap(const ff_msgs::VisualLandmarks& vl) { - if (run_ekf_) ekf_.SparseMapUpdate(vl); -} - -void EkfBag::UpdateOpticalFlowReg(const ff_msgs::CameraRegistration& reg) { - if (run_ekf_) ekf_.OpticalFlowRegister(reg); -} - -void EkfBag::UpdateSparseMapReg(const ff_msgs::CameraRegistration& reg) { - if (run_ekf_) ekf_.SparseMapRegister(reg); -} - -bool string_ends_with(const std::string& str, const std::string& ending) { - if (str.length() >= ending.length()) { - return (0 == str.compare(str.length() - ending.length(), ending.length(), ending)); - } else { - return false; - } -} - -void EkfBag::Run(void) { - if (run_ekf_) EstimateBias(); - - std::vector topics; - topics.push_back(std::string("/") + TOPIC_HARDWARE_IMU); - topics.push_back(TOPIC_HARDWARE_IMU); - topics.push_back(std::string("/") + TOPIC_LOCALIZATION_TRUTH); - topics.push_back(TOPIC_LOCALIZATION_TRUTH); - topics.push_back(std::string("/") + image_topic_); - topics.push_back(image_topic_); - if (!gen_features_) { - topics.push_back(std::string("/") + TOPIC_LOCALIZATION_ML_REGISTRATION); - topics.push_back(TOPIC_LOCALIZATION_ML_REGISTRATION); - topics.push_back(std::string("/") + TOPIC_LOCALIZATION_ML_FEATURES); - topics.push_back(TOPIC_LOCALIZATION_ML_FEATURES); - topics.push_back(std::string("/") + TOPIC_LOCALIZATION_AR_REGISTRATION); - topics.push_back(TOPIC_LOCALIZATION_AR_REGISTRATION); - topics.push_back(std::string("/") + TOPIC_LOCALIZATION_AR_FEATURES); - topics.push_back(TOPIC_LOCALIZATION_AR_FEATURES); - topics.push_back(std::string("/") + TOPIC_LOCALIZATION_OF_REGISTRATION); - topics.push_back(TOPIC_LOCALIZATION_OF_REGISTRATION); - topics.push_back(std::string("/") + TOPIC_LOCALIZATION_OF_FEATURES); - topics.push_back(TOPIC_LOCALIZATION_OF_FEATURES); - } - if (!run_ekf_) { - topics.push_back(std::string("/") + TOPIC_GNC_EKF); - topics.push_back(TOPIC_GNC_EKF); - } - - rosbag::View view(bag_, rosbag::TopicQuery(topics)); - bag_start_time_ = view.getBeginTime(); - - processing_of_ = processing_sparse_map_ = false; - of_id_ = vl_id_ = 0; - - int progress = 0; - for (rosbag::MessageInstance const m : view) { - progress++; - - if (string_ends_with(m.getTopic(), image_topic_)) { - sensor_msgs::ImageConstPtr image_msg = m.instantiate(); - UpdateImage(m.getTime(), image_msg); - // TODO(rsoussan): making print this optional - /*ff_common::PrintProgressBar(stdout, - static_cast(progress) / view.size());*/ - } else if (string_ends_with(m.getTopic(), TOPIC_HARDWARE_IMU)) { - sensor_msgs::ImuConstPtr imu_msg = m.instantiate(); - UpdateImu(m.getTime(), *imu_msg.get()); - } else if (string_ends_with(m.getTopic(), TOPIC_LOCALIZATION_TRUTH)) { - geometry_msgs::PoseStampedConstPtr gt_msg = m.instantiate(); - UpdateGroundTruth(*gt_msg.get()); - } else if (string_ends_with(m.getTopic(), TOPIC_LOCALIZATION_ML_FEATURES)) { - ff_msgs::VisualLandmarksConstPtr vl_msg = m.instantiate(); - UpdateSparseMap(*vl_msg.get()); - } else if (string_ends_with(m.getTopic(), TOPIC_LOCALIZATION_ML_REGISTRATION)) { - ff_msgs::CameraRegistrationConstPtr reg_msg = m.instantiate(); - UpdateSparseMapReg(*reg_msg.get()); - } else if (string_ends_with(m.getTopic(), TOPIC_LOCALIZATION_OF_FEATURES)) { - ff_msgs::Feature2dArrayConstPtr of_msg = m.instantiate(); - UpdateOpticalFlow(*of_msg.get()); - } else if (string_ends_with(m.getTopic(), TOPIC_LOCALIZATION_OF_REGISTRATION)) { - ff_msgs::CameraRegistrationConstPtr reg_msg = m.instantiate(); - UpdateOpticalFlowReg(*reg_msg.get()); - } else if (string_ends_with(m.getTopic(), TOPIC_GNC_EKF)) { - ff_msgs::EkfStateConstPtr ekf_msg = m.instantiate(); - UpdateEKF(*ekf_msg.get()); - } - } - printf("\n"); -} - -} // namespace ekf_bag diff --git a/tools/ekf_bag/src/ekf_bag_csv.cc b/tools/ekf_bag/src/ekf_bag_csv.cc deleted file mode 100644 index af323ab575..0000000000 --- a/tools/ekf_bag/src/ekf_bag_csv.cc +++ /dev/null @@ -1,176 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include - -namespace ekf_bag { - -Eigen::Vector3f QuatToEuler(const geometry_msgs::Quaternion & q) { - Eigen::Vector3f euler; - float q2q2 = q.y * q.y; - euler.x() = atan2(2 * (q.x * q.w + q.y * q.z), 1 - 2 * (q.x * q.x + q2q2)); - float arg = std::max(-1.0, std::min(1.0, 2 * (q.y * q.w - q.x * q.z))); - euler.y() = asin(arg); - euler.z() = atan2(2 * (q.x * q.y + q.z * q.w), 1 - 2 * (q2q2 + q.z * q.z)); - return euler; -} - -Eigen::Vector3f QuatToEuler(const Eigen::Quaternionf & q) { - Eigen::Vector3f euler; - float q2q2 = q.y() * q.y(); - euler.x() = atan2(2 * (q.x() * q.w() + q.y() * q.z()), 1 - 2 * (q.x() * q.x() + q2q2)); - float arg = std::max(-1.0f, std::min(1.0f, 2 * (q.y() * q.w() - q.x() * q.z()))); - euler.y() = asin(arg); - euler.z() = atan2(2 * (q.x() * q.y() + q.z() * q.w()), 1 - 2 * (q2q2 + q.z() * q.z())); - return euler; -} - -EkfBagCsv::EkfBagCsv(const char* bagfile, const char* mapfile, const char* csvfile, - bool run_ekf, bool gen_features, const char* biasfile, - std::string image_topic, const std::string& gnc_config) : - EkfBag(bagfile, mapfile, run_ekf, gen_features, biasfile, image_topic, gnc_config), - start_time_set_(false) { - // virtual function has to be called in subclass since not initialized in superclass - config_reader::ConfigReader config; - ReadParams(&config); - - f_ = fopen(csvfile, "w"); - if (f_ == NULL) { - fprintf(stderr, "Failed to open file %s.", csvfile); - exit(0); - } -} - -EkfBagCsv::~EkfBagCsv(void) { - fclose(f_); -} - -void EkfBagCsv::ReadParams(config_reader::ConfigReader* config) { - EkfBag::ReadParams(config); - - camera::CameraParameters cam_params(config, "nav_cam"); - tracked_of_.SetCameraParameters(cam_params); -} - -void EkfBagCsv::UpdateEKF(const ff_msgs::EkfState & s) { - EkfBag::UpdateEKF(s); - - if (!start_time_set_) { - start_time_ = s.header.stamp; - start_time_set_ = true; - } - - // Only print ekf results if it has been initialized with a sparse mapping measurement - if (!ekf_.HasPoseEstimate()) return; - - fprintf(f_, "EKF %g ", (s.header.stamp - start_time_).toSec()); - fprintf(f_, "%g %g %g ", s.pose.position.x, s.pose.position.y, s.pose.position.z); - Eigen::Vector3f euler = QuatToEuler(s.pose.orientation); - fprintf(f_, "%g %g %g ", euler.x(), euler.y(), euler.z()); - fprintf(f_, "%g %g %g ", s.velocity.x, s.velocity.y, s.velocity.z); - fprintf(f_, "%g %g %g ", s.omega.x, s.omega.y, s.omega.z); - fprintf(f_, "%g %g %g ", s.accel.x, s.accel.y, s.accel.z); - fprintf(f_, "%g %g %g ", s.accel_bias.x, s.accel_bias.y, s.accel_bias.z); - fprintf(f_, "%g %g %g ", s.gyro_bias.x, s.gyro_bias.y, s.gyro_bias.z); - fprintf(f_, "%d ", s.confidence); - fprintf(f_, "%d ", s.status); - fprintf(f_, "%d %d", s.ml_count, s.of_count); - for (int i = 0; i < 15; i++) - fprintf(f_, " %g", s.cov_diag[i]); - for (int i = 0; i < 50; i++) - fprintf(f_, " %g", s.ml_mahal_dists[i]); - fprintf(f_, "\n"); -} - -void EkfBagCsv::UpdateOpticalFlowReg(const ff_msgs::CameraRegistration & reg) { - EkfBag::UpdateOpticalFlowReg(reg); - - of_reg_time_ = reg.header.stamp; -} - -void EkfBagCsv::UpdateSparseMapReg(const ff_msgs::CameraRegistration & reg) { - EkfBag::UpdateSparseMapReg(reg); - - ml_reg_time_ = reg.header.stamp; -} - -void EkfBagCsv::UpdateOpticalFlow(const ff_msgs::Feature2dArray & of) { - if (!start_time_set_) - return; - - EkfBag::UpdateOpticalFlow(of); - - float t = (of.header.stamp - start_time_).toSec(); - tracked_of_.UpdateFeatures(of, t); - fprintf(f_, "OF %g ", (of_reg_time_ - start_time_).toSec()); - fprintf(f_, "%d ", static_cast(of.feature_array.size())); - for (auto it = tracked_of_.begin(); it != tracked_of_.end(); it++) { - const auto & a = (*it).second; - fprintf(f_, "%d %g %g %g ", a.id, a.time, a.x, a.y); - } - fprintf(f_, "\n"); -} - -void EkfBagCsv::UpdateSparseMap(const ff_msgs::VisualLandmarks & vl) { - if (!start_time_set_) - return; - - EkfBag::UpdateSparseMap(vl); - - const camera::CameraParameters & params = map_.GetCameraParameters(); - fprintf(f_, "VL %g ", (ml_reg_time_ - start_time_).toSec()); - fprintf(f_, "%d ", static_cast(vl.landmarks.size())); - if (vl.landmarks.size() >= 5) { - Eigen::Vector3f trans = ekf_.GetNavCamToBody().translation().cast(); - Eigen::Quaternionf q1(0, trans.x(), trans.y(), trans.z()); - Eigen::Quaternionf q2(vl.pose.orientation.w, vl.pose.orientation.x, vl.pose.orientation.y, vl.pose.orientation.z); - Eigen::Quaternionf cam_to_body_q(ekf_.GetNavCamToBody().rotation().cast()); - cam_to_body_q.w() = -cam_to_body_q.w(); - q2 = q2 * cam_to_body_q; - Eigen::Quaternionf t = (q2 * q1) * q2.conjugate(); - Eigen::Vector3f r(vl.pose.position.x, vl.pose.position.y, vl.pose.position.z); - r -= Eigen::Vector3f(t.x(), t.y(), t.z()); - fprintf(f_, "%g %g %g ", r.x(), r.y(), r.z()); - Eigen::Vector3f euler = QuatToEuler(q2); - fprintf(f_, "%g %g %g ", euler.x(), euler.y(), euler.z()); - } - for (unsigned int i = 0; i < vl.landmarks.size(); i++) { - Eigen::Vector2d input(vl.landmarks[i].u, vl.landmarks[i].v); - Eigen::Vector2d output; - params.Convert(input, &output); - fprintf(f_, "%g %g %g %g %g ", output.x(), output.y(), vl.landmarks[i].x, vl.landmarks[i].y, vl.landmarks[i].z); - } - fprintf(f_, "\n"); -} - -void EkfBagCsv::UpdateGroundTruth(const geometry_msgs::PoseStamped & gt) { - EkfBag::UpdateGroundTruth(gt); - - if (f_ == NULL || !start_time_set_) - return; - fprintf(f_, "GT %g ", (gt.header.stamp - start_time_).toSec()); - fprintf(f_, "%g %g %g ", gt.pose.position.x, gt.pose.position.y, gt.pose.position.z); - Eigen::Vector3f euler = QuatToEuler(gt.pose.orientation); - fprintf(f_, "%g %g %g\n", euler.x(), euler.y(), euler.z()); -} - -} // namespace ekf_bag - diff --git a/tools/ekf_bag/src/tracked_features.cc b/tools/ekf_bag/src/tracked_features.cc deleted file mode 100644 index 4c0409e920..0000000000 --- a/tools/ekf_bag/src/tracked_features.cc +++ /dev/null @@ -1,83 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include - -#include - -namespace ekf_bag { - -void TrackedOFFeatures::UpdateFeatures(const ff_msgs::Feature2dArray & of, float time) { - std::map cur_features; - for (unsigned int i = 0; i < of.feature_array.size(); i++) { - cur_features.insert(std::pair(of.feature_array[i].id, of.feature_array[i])); - } - - for (std::map::iterator it = f_.begin(); it != f_.end(); it++) { - int count = cur_features.count(it->first); - if (count == 0) { - it = f_.erase(it); - it--; // erase returns the next element, so we would skip it - } - } - for (std::map::const_iterator it = cur_features.begin(); it != cur_features.end(); it++) { - Eigen::Vector2d input(it->second.x, it->second.y); - Eigen::Vector2d output; - params_.Convert(input, &output); - struct OFFeature f = {it->second.id, time, static_cast(output.x()), static_cast(output.y())}; - auto t = f_.emplace(f.id, f); - // if is already present - t.first->second.x = output.x(); - t.first->second.y = output.y(); - } -} - -void TrackedSMFeatures::UpdateFeatures(const ff_msgs::VisualLandmarks & vl, float time) { - f_.clear(); - - for (unsigned int i = 0; i < vl.landmarks.size(); i++) { - auto & l = vl.landmarks[i]; - struct SMFeature f; - f.pos.x() = l.x; f.pos.y() = l.y; f.pos.z() = l.z; - Eigen::Vector2d input(l.u, l.v), output; - params_.Convert(input, &output); - f.pixel.x() = output.x(); f.pixel.y() = output.y(); - f.time = time; - f_.push_back(f); - } -} - -void TrackedSMFeatures::UpdatePose(const geometry_msgs::Pose & pose) { - pose_transform_ = Eigen::Affine3d( - Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z)) * - Eigen::Affine3d(Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, - pose.orientation.y, pose.orientation.z)); -} - -Eigen::Vector2d TrackedSMFeatures::FeatureToCurrentPixel(const struct SMFeature & f) { - Eigen::Vector3d rel = f.pos.cast(); - camera::CameraModel c(body_to_camera_ * pose_transform_.inverse(), params_); - Eigen::Vector2d output; - params_.Convert(c.ImageCoordinates(rel), &output); - return output; -} - -} // namespace ekf_bag - diff --git a/tools/ekf_bag/tools/bag_to_csv.cc b/tools/ekf_bag/tools/bag_to_csv.cc deleted file mode 100644 index 282cd3c73d..0000000000 --- a/tools/ekf_bag/tools/bag_to_csv.cc +++ /dev/null @@ -1,45 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include - -DEFINE_bool(gen_features, false, - "If true, generate features from image, otherwise use from bag."); -DEFINE_bool(run_ekf, false, - "If true, run EKF, otherwise read messages from bag."); -DEFINE_string(image_topic, TOPIC_HARDWARE_NAV_CAM, - "The topic to get images from.."); - -int main(int argc, char** argv) { - ff_common::InitFreeFlyerApplication(&argc, &argv); - - ros::Time::init(); - - if (argc < 3) { - LOG(INFO) << "Usage: " << argv[0] << " map.map bag.bag output.txt"; - exit(0); - } - - const char* biasfile = NULL; - if (argc >= 6) biasfile = argv[5]; - - ekf_bag::EkfBagCsv bag(argv[2], argv[1], argv[3], FLAGS_run_ekf, - FLAGS_gen_features, biasfile, FLAGS_image_topic, argv[4]); - bag.Run(); -} diff --git a/tools/ekf_bag/tools/sparse_map_bag.cc b/tools/ekf_bag/tools/sparse_map_bag.cc deleted file mode 100644 index a190ded437..0000000000 --- a/tools/ekf_bag/tools/sparse_map_bag.cc +++ /dev/null @@ -1,159 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -Eigen::Vector3f QuatToEuler(const geometry_msgs::Quaternion & q) { - Eigen::Vector3f euler; - float q2q2 = q.y * q.y; - euler.x() = atan2(2 * (q.x * q.w + q.y * q.z), 1 - 2 * (q.x * q.x + q2q2)); - float arg = std::max(-1.0, std::min(1.0, 2 * (q.y * q.w - q.x * q.z))); - euler.y() = asin(arg); - euler.z() = atan2(2 * (q.x * q.y + q.z * q.w), 1 - 2 * (q2q2 + q.z * q.z)); - return euler; -} - -Eigen::Vector3f QuatToEuler(const Eigen::Quaternionf & q) { - Eigen::Vector3f euler; - float q2q2 = q.y() * q.y(); - euler.x() = atan2(2 * (q.x() * q.w() + q.y() * q.z()), 1 - 2 * (q.x() * q.x() + q2q2)); - float arg = std::max(-1.0f, std::min(1.0f, 2 * (q.y() * q.w() - q.x() * q.z()))); - euler.y() = asin(arg); - euler.z() = atan2(2 * (q.x() * q.y() + q.z() * q.w()), 1 - 2 * (q2q2 + q.z() * q.z())); - return euler; -} - -void ReadParams(localization_node::Localizer* loc) { - config_reader::ConfigReader config; - config.AddFile("cameras.config"); - config.AddFile("geometry.config"); - config.AddFile("localization.config"); - if (!config.ReadFiles()) { - ROS_ERROR("Failed to read config files."); - return; - } - - loc->ReadParams(&config); -} - -int main(int argc, char ** argv) { - ff_common::InitFreeFlyerApplication(&argc, &argv); - - struct timespec tstart={0, 0}, tend={0, 0}; - struct timespec tstart2={0, 0}, tend2={0, 0}; - clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tstart); - clock_gettime(CLOCK_MONOTONIC, &tstart2); - - if (argc < 3) { - LOG(INFO) << "Usage: " << argv[0] << " map.map bag.bag"; - exit(0); - } - - rosbag::Bag bag; - bag.open(argv[2], rosbag::bagmode::Read); - - sparse_mapping::SparseMap map(argv[1], true); - localization_node::Localizer loc(&map); - - ReadParams(&loc); - - std::vector topics; - topics.push_back(std::string("/") + std::string(TOPIC_MANAGEMENT_IMG_SAMPLER_NAV_CAM_RECORD)); - topics.push_back(std::string(TOPIC_MANAGEMENT_IMG_SAMPLER_NAV_CAM_RECORD)); - topics.push_back(std::string("/") + std::string(TOPIC_HARDWARE_NAV_CAM)); - topics.push_back(std::string(TOPIC_HARDWARE_NAV_CAM)); - topics.push_back(std::string("/") + std::string(TOPIC_LOCALIZATION_TRUTH)); - topics.push_back(std::string(TOPIC_LOCALIZATION_TRUTH)); - rosbag::View view(bag, rosbag::TopicQuery(topics)); - - int progress = 0; - int images = 0; - int failures = 0; - int total_features = 0; - - // If the user provided an existing directory in which to save the - // failed images, do so. - char * failed_images_dir = getenv("SAVE_FAILED_IMAGES_DIR"); - char filename_buffer[256]; - - for (rosbag::MessageInstance const m : view) { - progress++; - - if (m.isType()) { - sensor_msgs::ImageConstPtr image_msg = m.instantiate(); - cv_bridge::CvImageConstPtr image; - try { - image = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8); - } catch (cv_bridge::Exception& e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); - break; - } - ff_msgs::VisualLandmarks vl_features; - loc.Localize(image, &vl_features); - if (vl_features.landmarks.size() == 0) { - failures++; - - if (failed_images_dir != NULL) { - double curr_image_time = image_msg->header.stamp.toSec(); - snprintf(filename_buffer, sizeof(filename_buffer), - "%s/%10.7f.jpg", failed_images_dir, curr_image_time); - std::cout << "Writing image with failed localization: " << filename_buffer << std::endl; - cv::imwrite(filename_buffer, image->image); - } - } - - images++; - total_features += vl_features.landmarks.size(); - } else if (m.isType()) { - geometry_msgs::PoseStampedConstPtr gt_msg = m.instantiate(); - } - ff_common::PrintProgressBar(stdout, static_cast(progress) / view.size()); - } - - bag.close(); - if (images - failures == 0) { - printf("Failed to localize any images.\n"); - } else { - printf("Localized %d / %d images with mean of %d features.\n", - images - failures, images, total_features / (images - failures)); - clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tend); - clock_gettime(CLOCK_MONOTONIC, &tend2); - printf("Time per Frame: %.5f s\n", ((static_cast(tend2.tv_sec) + 1.0e-9*tend2.tv_nsec) - - (static_cast(tstart2.tv_sec) + 1.0e-9*tstart2.tv_nsec)) / images); - printf("CPU Time per Frame: %.5f s\n", ((static_cast(tend.tv_sec) + 1.0e-9*tend.tv_nsec) - - (static_cast(tstart.tv_sec) + 1.0e-9*tstart.tv_nsec)) / images); - } -} - diff --git a/tools/ekf_video/CMakeLists.txt b/tools/ekf_video/CMakeLists.txt deleted file mode 100644 index 58fbf025ca..0000000000 --- a/tools/ekf_video/CMakeLists.txt +++ /dev/null @@ -1,65 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -cmake_minimum_required(VERSION 3.0) -project(ekf_video) - -## Compile as C++14, supported in ROS Kinetic and newer -add_compile_options(-std=c++14) - -## Find catkin macros and libraries -find_package(catkin2 REQUIRED COMPONENTS - roscpp - ekf_bag -) - -# System dependencies are found with CMake's conventions -find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Gui) -list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../../cmake") -find_package(FFMPEG REQUIRED ) - -catkin_package( - CATKIN_DEPENDS roscpp ekf_bag -) - -########### -## Build ## -########### - -# Specify additional locations of header files -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Qt5Gui_INCLUDE_DIRS} - ${FFMPEG_INCLUDE_DIRS} -) - -# Declare C++ libraries -add_library(ekf_video - src/ekf_bag_video.cc - src/video_writer.cc -) -add_dependencies(ekf_video ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ekf_video ${catkin_LIBRARIES} ${Qt5Gui_LIBRARIES} ${FFMPEG_LIBRARIES} -) - - -## Declare a C++ executable: inspection_tool -add_executable(ekf_to_video tools/ekf_to_video.cc) -add_dependencies(ekf_to_video ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ekf_to_video - ekf_video gflags glog ${catkin_LIBRARIES}) diff --git a/tools/ekf_video/include/ekf_video/ekf_bag_video.h b/tools/ekf_video/include/ekf_video/ekf_bag_video.h deleted file mode 100644 index 01405d1988..0000000000 --- a/tools/ekf_video/include/ekf_video/ekf_bag_video.h +++ /dev/null @@ -1,107 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef EKF_VIDEO_EKF_BAG_VIDEO_H_ -#define EKF_VIDEO_EKF_BAG_VIDEO_H_ - -#include -#include -#include - -#include -#include - -#include -#include - -class QPainter; -class QRect; - -#define MAHAL_NUM_BINS 20 -#define MAHAL_BIN_SIZE 0.5 - -namespace ekf_video { - -class EkfBagVideo : public ekf_bag::EkfBag { - public: - EkfBagVideo(const char* bagfile, const char* mapfile, const char* videofile, - bool run_ekf, bool gen_features, bool use_jem, - const char* biasfile, std::string image_topic); - virtual ~EkfBagVideo(void); - - protected: - virtual void UpdateImage(const ros::Time& time, - const sensor_msgs::ImageConstPtr& image); - - virtual void UpdateEKF(const ff_msgs::EkfState& state); - virtual void UpdateOpticalFlow(const ff_msgs::Feature2dArray& of); - virtual void UpdateSparseMap(const ff_msgs::VisualLandmarks& vl); - - virtual void ReadParams(config_reader::ConfigReader* config); - - private: - void DrawFrame(void); - void DrawImage(QPainter& p, const sensor_msgs::ImageConstPtr& ros_image, - const QRect& rect); - void DrawPose(QPainter& p, const QRect& rect); - - void DrawVelocity(QPainter& p, const QRect& rect); - void DrawOmega(QPainter& p, const QRect& rect); - void DrawAccelBias(QPainter& p, const QRect& rect); - void DrawGyroBias(QPainter& p, const QRect& rect); - void DrawPositionCov(QPainter& p, const QRect& rect); - void DrawVelocityCov(QPainter& p, const QRect& rect); - void DrawAngleCov(QPainter& p, const QRect& rect); - - void DrawStatus(QPainter& p, const QRect& rect); - void DrawTimestamp(QPainter& p, const QRect& rect); - void DrawSMFeatureCount(QPainter& p, const QRect& rect); - void DrawOFFeatureCount(QPainter& p, const QRect& rect); - - void DrawMahalanobis(QPainter& p, const QRect& rect); - - void DrawBarGraph(QPainter& p, const QRect& rect, int data_count, - const float* data, float y_min, float y_max, - const char* title, const char* xlabel, const char* ylabel, - const std::string* data_labels, bool data_labels_on_bars); - - VideoWriter video_; - - sensor_msgs::ImageConstPtr last_image_; - ekf_bag::TrackedOFFeatures tracked_of_; - ekf_bag::TrackedSMFeatures tracked_sm_; - - std::list pose_history_; - int pose_count_; - - bool start_time_set_; - bool use_jem_; - ros::Time start_time_; - ros::Time last_draw_time_; - ros::Time last_image_time_; - - ff_msgs::EkfState state_; - int of_count_; - int ml_count_; - - int mahal_bins_[MAHAL_NUM_BINS]; -}; - -} // end namespace ekf_video - -#endif // EKF_VIDEO_EKF_BAG_VIDEO_H_ diff --git a/tools/ekf_video/include/ekf_video/video_writer.h b/tools/ekf_video/include/ekf_video/video_writer.h deleted file mode 100644 index a96d8a9a2c..0000000000 --- a/tools/ekf_video/include/ekf_video/video_writer.h +++ /dev/null @@ -1,55 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef EKF_VIDEO_VIDEO_WRITER_H_ -#define EKF_VIDEO_VIDEO_WRITER_H_ - -#include - -struct AVCodecContext; -struct AVFormatContext; -struct AVFrame; - -class QImage; - -namespace ekf_video { - -class VideoWriter { - public: - VideoWriter(const char* videofile, int width, int height); - virtual ~VideoWriter(void); - - virtual void AddFrame(const QImage & image); - - private: - void OpenVideo(const char* videofile); - void CloseVideo(); - - int EncodeFrame(AVFrame* frame, int* output_received); - - int width_, height_; - - struct AVFormatContext* format_context_; - struct AVCodecContext* codec_context_; - struct AVFrame* frame_; -}; - -} // end namespace ekf_video - -#endif // EKF_VIDEO_VIDEO_WRITER_H_ - diff --git a/tools/ekf_video/package.xml b/tools/ekf_video/package.xml deleted file mode 100644 index 9cf98e0fee..0000000000 --- a/tools/ekf_video/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - ekf_video - 0.0.0 - - The ekf_bag package runs the EKF on an entire bag and creates a video. - - - Apache License, Version 2.0 - - - Astrobee Flight Software - - - Astrobee Flight Software - - catkin - roscpp - ekf_bag - roscpp - ekf_bag - - diff --git a/tools/ekf_video/readme.md b/tools/ekf_video/readme.md deleted file mode 100644 index a01d71841b..0000000000 --- a/tools/ekf_video/readme.md +++ /dev/null @@ -1,4 +0,0 @@ -\page ekfvideo EKF Video - -This package creates a video from a bag file. It builds -on the `ekf_bag` package. diff --git a/tools/ekf_video/scripts/ekf_video b/tools/ekf_video/scripts/ekf_video deleted file mode 100755 index 9445ab6465..0000000000 --- a/tools/ekf_video/scripts/ekf_video +++ /dev/null @@ -1,84 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import argparse -import math -import os -import os.path -import sys -import time - -sys.path.insert(0, os.path.dirname(os.path.realpath(__file__)) + '/../../ekf_bag/scripts') -import environment - - -def run_ekf(astrobee_map, astrobee_bag, output_file, ekf_in_bag = False, features_in_bag=False, image_topic=None, use_jem=False): - cmd = '' if ekf_in_bag else '--run_ekf %s' % ('' if features_in_bag else '--gen_features') - - robot_bias = os.path.dirname(os.path.abspath(astrobee_bag)) + '/imu_bias.config' - cmd = 'rosrun ekf_video ekf_to_video %s %s %s %s' % (cmd, astrobee_map, astrobee_bag, output_file) - print('Not running EKF.' if ekf_in_bag else 'Running EKF.') - if not ekf_in_bag: - print( 'Using features from bag.' if features_in_bag else 'Generating features from image.') - if os.path.isfile(robot_bias): - cmd += ' ' + robot_bias - if image_topic is not None: - cmd += ' --image_topic ' + image_topic - if use_jem: - cmd += ' --use_jem' - os.system(cmd) - -epilog_msg=""" -The arguments BAG or MAP could be ommitted if specificed with the following -environement variables: - - ASTROBEE_BAG instead of positional argument 1 BAG - - ASTROBEE_MAP instead of positional argument 2 MAP - -If [-r ROBOT] in not used, then the robot configuration file should be copied -locally as 'robot.config'. -""" - -parser = argparse.ArgumentParser(description='Graph the EKF from a ros bag file.', epilog=epilog_msg, formatter_class=argparse.RawDescriptionHelpFormatter) - -parser.add_argument('-e', '--ekf_in_bag', dest='ekf_in_bag', action='store_true', - help='Use EKF message from bag instead of running EKF on images.') -parser.add_argument('-f', '--features_in_bag', dest='features_in_bag', action='store_true', - help='Use feature message from bag in EKF instead of generating from images.') -parser.add_argument('-j', '--use_jem_extends', dest='use_jem', action='store_true', - help='Use JEM mode: draw both a top and side view of the robot pose in the JEM. By default, the program draws a top view of the granite table.') -parser.add_argument('-r', '--robot_name', dest='robot_name', metavar='ROBOT', - help='Specify the robot to use (just name, not path).') -parser.add_argument('-i', '--image_topic', dest='image_topic', default=None, - help='Use specified image topic.') -parser.add_argument('bag_file', metavar='BAG', nargs='?', - help='bag to process') -parser.add_argument('map_file', metavar='MAP', nargs='?', - help='map to use') - -args, args_unknown = parser.parse_known_args() - -(astrobee_map, astrobee_bag, robot_config) = environment.initialize_environment(args.map_file, args.bag_file, args.robot_name) - -video_name = astrobee_bag[:astrobee_bag.rfind('.')] + '.mkv' - -start = time.time() -run_ekf(astrobee_map, astrobee_bag, video_name, args.ekf_in_bag, args.features_in_bag, args.image_topic, args.use_jem) -run_time = time.time() - start -print 'Complete! Video output to %s. Run time: %g\n' % (video_name, run_time) - diff --git a/tools/ekf_video/src/ekf_bag_video.cc b/tools/ekf_video/src/ekf_bag_video.cc deleted file mode 100644 index 30b131723a..0000000000 --- a/tools/ekf_video/src/ekf_bag_video.cc +++ /dev/null @@ -1,601 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include -#include -#include - -#include -#include - -#include -#include - -namespace ekf_video { - -EkfBagVideo::EkfBagVideo(const char* bagfile, const char* mapfile, - const char* videofile, bool run_ekf, bool gen_features, - bool use_jem, const char* biasfile, - std::string image_topic) - : EkfBag(bagfile, mapfile, run_ekf, gen_features, biasfile, image_topic), - video_(videofile, 1920, 1080), - use_jem_(use_jem) { - // virtual function has to be called in subclass since not initialized in - // superclass - config_reader::ConfigReader config; - ReadParams(&config); - - start_time_set_ = false; - last_draw_time_ = ros::Time(0, 0); - pose_count_ = 0; -} - -EkfBagVideo::~EkfBagVideo(void) {} - -void EkfBagVideo::ReadParams(config_reader::ConfigReader* config) { - EkfBag::ReadParams(config); - - Eigen::Vector3d trans; - Eigen::Quaterniond rot; - if (!msg_conversions::config_read_transform(config, "nav_cam_transform", - &trans, &rot)) - ROS_FATAL("Unspecified nav_cam_transform."); - Eigen::Affine3d nav_cam_to_body = - Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * - Eigen::Affine3d(rot); - camera::CameraParameters cam_params(config, "nav_cam"); - tracked_of_.SetCameraParameters(cam_params); - tracked_sm_.SetCameraParameters(cam_params); - tracked_sm_.SetCameraToBody(nav_cam_to_body); -} - -void EkfBagVideo::DrawImage(QPainter& p, - const sensor_msgs::ImageConstPtr& ros_image, - const QRect& rect) { - if (ros_image) { - const QImage gray(ros_image->data.data(), ros_image->width, - ros_image->height, ros_image->step, - QImage::Format_Grayscale8); - // draw on image - p.drawImage(rect, gray); - } - p.setRenderHint(QPainter::Antialiasing); - p.setRenderHint(QPainter::HighQualityAntialiasing); - - p.translate(rect.center().x(), rect.center().y()); - - // draw optical flow - p.setBrush(QColor::fromRgb(0x99, 0x99, 0xFF, 0xA0)); - for (auto it = tracked_of_.begin(); it != tracked_of_.end(); it++) { - const auto& a = (*it).second; - p.drawEllipse(QPointF(a.x, a.y), 4, 4); - } - - // draw sparse mapping - p.setBrush(QColor::fromRgb(0xFF, 0x00, 0x66, 0xA0)); - for (auto it = tracked_sm_.begin(); it != tracked_sm_.end(); it++) { - Eigen::Vector2d pixel = tracked_sm_.FeatureToCurrentPixel(*it); - if (pixel.x() < rect.width() / 2 && pixel.y() < rect.height() / 2) - p.drawEllipse(QPointF(pixel.x(), pixel.y()), 4, 4); - } - - p.resetTransform(); -} - -void EkfBagVideo::DrawPose(QPainter& p, const QRect& rect) { - if (use_jem_) { - p.fillRect(QRectF(rect.x(), rect.y(), rect.width(), rect.height()), - QBrush(QColor::fromRgb(0xDD, 0xDD, 0xDD))); - - // Bounding volume of the JEM - float y_range[2] = {-11.5, -2.5}; // 9.0m - float x_range[2] = {9.85, 12.0}; // 2.15m - float z_range[2] = {3.8, 5.95}; // 2.15m - // screen-X = world-Y and screen-Y = -world-X - // Limitation dimmension is along Y axis - float margin = 4.0; - float scale = (rect.width() - 2.0 * margin) / (y_range[1] - y_range[0]); - - // Top View - p.resetTransform(); - p.translate(rect.x() + margin, rect.y() + margin); - p.scale(scale, scale); - p.translate(-y_range[0], x_range[1]); - - // Draw axes (X=red, Y=green) - p.setPen(QPen(Qt::red, 0.03)); - p.drawLine(QLineF(y_range[0], -x_range[0], y_range[0], -x_range[1])); - p.setPen(QPen(Qt::green, 0.03)); - p.drawLine(QLineF(y_range[0], -x_range[0], y_range[1], -x_range[0])); - p.setPen(QPen(Qt::gray, 0.03)); - p.drawLine(QLineF(y_range[1], -x_range[1], y_range[1], -x_range[0])); - p.drawLine(QLineF(y_range[1], -x_range[1], y_range[0], -x_range[1])); - - if (pose_history_.size() > 0) { - // Draw robot - auto& pose = pose_history_.back(); - Eigen::Quaternionf o(pose.orientation.w, pose.orientation.x, - pose.orientation.y, pose.orientation.z); - auto rot = o.toRotationMatrix(); - auto euler = rot.eulerAngles(0, 1, 2); - float zrot = (euler[2] - M_PI / 2.0) * 180.0 / M_PI; - p.translate(pose.position.y, -pose.position.x); - p.rotate(zrot); - p.fillRect(QRectF(-0.1, -0.1, 0.2, 0.2), - QColor::fromRgb(0xBB, 0xBB, 0x00)); - p.rotate(-zrot); - p.setPen(QPen(Qt::black, 0.02)); - // Draw X robot axis - p.drawLine(QLineF(0.0, 0.0, 0.5 * rot(1, 0), -0.5 * rot(0, 0))); - p.translate(-pose.position.y, pose.position.x); - // Draw pose history - p.setPen(QPen(Qt::cyan, 0.01)); - for (auto it = pose_history_.begin(); it != pose_history_.end(); it++) { - auto second = std::next(it); - if (second == pose_history_.end()) break; - p.drawLine(QLineF(it->position.y, -it->position.x, second->position.y, - -second->position.x)); - } - } - // Side View - p.resetTransform(); - p.translate(rect.x() + margin, rect.y() + margin + rect.height() / 3.0); - p.scale(scale, scale); - p.translate(-y_range[0], -z_range[0]); - // Draw axes (Z=blue, Y=green) - p.setPen(QPen(Qt::blue, 0.03)); - p.drawLine(QLineF(y_range[0], z_range[0], y_range[0], z_range[1])); - p.setPen(QPen(Qt::green, 0.03)); - p.drawLine(QLineF(y_range[0], z_range[0], y_range[1], z_range[0])); - p.setPen(QPen(Qt::gray, 0.03)); - p.drawLine(QLineF(y_range[1], z_range[0], y_range[1], z_range[1])); - p.drawLine(QLineF(y_range[1], z_range[1], y_range[0], z_range[1])); - - if (pose_history_.size() > 0) { - // Draw robot - auto& pose = pose_history_.back(); - Eigen::Quaternionf o(pose.orientation.w, pose.orientation.x, - pose.orientation.y, pose.orientation.z); - auto rot = o.toRotationMatrix(); - auto euler = rot.eulerAngles(2, 0, 1); - float yrot = -euler[2] * 180.0 / M_PI; - p.translate(pose.position.y, pose.position.z); - p.rotate(yrot); - p.fillRect(QRectF(-0.1, -0.1, 0.2, 0.2), - QColor::fromRgb(0xBB, 0xBB, 0x00)); - p.rotate(-yrot); - p.setPen(QPen(Qt::black, 0.02)); - // Draw X robot axis - p.drawLine(QLineF(0.0, 0.0, 0.5 * rot(1, 0), 0.5 * rot(2, 0))); - p.translate(-pose.position.y, -pose.position.z); - // Draw pose history - p.setPen(QPen(Qt::magenta, 0.01)); - for (auto it = pose_history_.begin(); it != pose_history_.end(); it++) { - auto second = std::next(it); - if (second == pose_history_.end()) break; - p.drawLine(QLineF(it->position.y, it->position.z, second->position.y, - second->position.z)); - } - } - } else { - // draw robot pose - p.translate(rect.center().x(), rect.center().y()); - p.scale(rect.width() / 2.13333, rect.height() / 2.13333); - - if (pose_history_.size() > 0) { - p.fillRect(QRectF(-1.0, -1.0, 2.0, 2.0), - QBrush(QColor::fromRgb(0xDD, 0xDD, 0xDD))); - auto& pose = pose_history_.back(); - Eigen::Quaternionf o(pose.orientation.w, pose.orientation.x, - pose.orientation.y, pose.orientation.z); - auto euler = o.toRotationMatrix().eulerAngles(0, 1, 2); - float zrot = (euler[2] + M_PI) * 180.0 / M_PI; - p.translate(pose.position.x, pose.position.y); - p.rotate(zrot); - p.fillRect(QRectF(-0.1, -0.1, 0.2, 0.2), - QColor::fromRgb(0xBB, 0xBB, 0x00)); - QPen pen(QColor::fromRgb(0, 0, 0)); - pen.setWidth(0.2); - p.setPen(pen); - p.drawLine(QLineF(0.0, 0.0, 0.25, 0.0)); - p.rotate(-zrot); - p.translate(-pose.position.x, -pose.position.y); - - int i = 0; - for (auto it = pose_history_.begin(); it != pose_history_.end(); it++) { - pen.setColor(QColor::fromRgb(0x10, 0xDD, 0x10, - (unsigned char)(255 * (i / 200.0)))); - p.setPen(pen); - auto second = std::next(it); - if (second == pose_history_.end()) break; - p.drawLine(QLineF(it->position.x, it->position.y, second->position.x, - second->position.y)); - i++; - } - } - } - p.resetTransform(); -} - -void EkfBagVideo::DrawBarGraph(QPainter& p, const QRect& rect, int data_count, - const float* data, float y_min, float y_max, - const char* title, const char* xlabel, - const char* ylabel, - const std::string* data_labels, - bool data_labels_on_bars) { - int TITLE_HEIGHT = title == NULL ? 0 : 25; - int LABELS_HEIGHT = data_labels == NULL ? 0 : 20; - int YLABEL_WIDTH = ylabel == NULL ? 0 : 20; - int XLABEL_HEIGHT = xlabel == NULL ? 0 : 30; - int TICKS_WIDTH = 25; - int GRAPH_HEIGHT = - rect.height() - LABELS_HEIGHT - TITLE_HEIGHT - XLABEL_HEIGHT - 5; - int GRAPH_WIDTH = rect.width() - TICKS_WIDTH - YLABEL_WIDTH - 10; - float BIN_WIDTH = GRAPH_WIDTH / static_cast(data_count); - - QPen white(QColor::fromRgb(0xFF, 0xFF, 0xFF)); - p.setPen(white); - QFont font("Arial"); - - p.translate(rect.x(), rect.y() + 5); - - // draw title - font.setPointSize(8); - p.setFont(font); - p.drawText(QRectF(0, 0, rect.width(), TITLE_HEIGHT), Qt::AlignCenter, title); - p.translate(0, TITLE_HEIGHT); - - float ZERO_HEIGHT = (std::max(std::min(0.0f, y_max), y_min) - y_min) / - (y_max - y_min) * GRAPH_HEIGHT; - - // draw left side - // ylabel - if (ylabel != NULL) { - p.save(); - p.translate(YLABEL_WIDTH / 2, GRAPH_HEIGHT / 2); - p.rotate(-90); - font.setPointSize(6); - p.setFont(font); - p.drawText(QRectF(-GRAPH_HEIGHT / 2, -YLABEL_WIDTH / 2, GRAPH_HEIGHT, - YLABEL_WIDTH), - Qt::AlignCenter, ylabel); - p.restore(); - } - - p.translate(YLABEL_WIDTH, 0); - font.setPointSize(4); - int font_height = p.fontMetrics().height() / 2; - p.setFont(font); - // axis labels - { - std::stringstream stream; - stream << std::fixed << std::setprecision(1) << y_max; - p.drawText(QRectF(0, -font_height / 2, TICKS_WIDTH - 6, font_height), - Qt::AlignRight, stream.str().c_str()); - } - p.drawLine(QLineF(TICKS_WIDTH - 5, 0, TICKS_WIDTH, 0)); - { - std::stringstream stream; - stream << std::fixed << std::setprecision(1) << y_min; - p.drawText( - QRectF(0, GRAPH_HEIGHT - font_height / 2, TICKS_WIDTH - 6, font_height), - Qt::AlignRight, stream.str().c_str()); - } - p.drawLine(QLineF(TICKS_WIDTH - 5, GRAPH_HEIGHT, TICKS_WIDTH, GRAPH_HEIGHT)); - if (y_max > 0.0 && y_min < 0.0) { - p.drawText( - QRectF(0, ZERO_HEIGHT - font_height / 2, TICKS_WIDTH - 6, font_height), - Qt::AlignRight, "0"); - p.drawLine(QLineF(TICKS_WIDTH - 5, ZERO_HEIGHT, TICKS_WIDTH, ZERO_HEIGHT)); - } - - // draw content of graph - p.translate(TICKS_WIDTH, 0); - for (int i = 0; i < data_count; i++) { - float height = (std::max(std::min(data[i], y_max), y_min) - y_min) / - (y_max - y_min) * GRAPH_HEIGHT; - p.fillRect(QRectF(i * BIN_WIDTH, GRAPH_HEIGHT - height, BIN_WIDTH, - height - ZERO_HEIGHT), - QBrush(QColor::fromRgb(0x00, 0xFF, 0x00))); - } - // draw bounding box - white.setWidth(1.0); - p.setPen(white); - p.setBrush(Qt::NoBrush); - p.drawRect(QRectF(0, 0, GRAPH_WIDTH, GRAPH_HEIGHT)); - - // draw bottom tick marks - if (data_labels != NULL) { - font.setPointSize(4); - p.setFont(font); - for (int i = 0; i < data_count + (data_labels_on_bars ? 0 : 1); i++) { - p.drawText( - QRectF(i * BIN_WIDTH - (data_labels_on_bars ? 0 : BIN_WIDTH / 2), - GRAPH_HEIGHT, BIN_WIDTH, LABELS_HEIGHT), - Qt::AlignCenter, data_labels[i].c_str()); - } - for (int i = 0; i < data_count + 1; i++) - p.drawLine(QLineF(i * BIN_WIDTH, 0, i * BIN_WIDTH, GRAPH_HEIGHT + 5)); - } - - font.setPointSize(6); - p.setFont(font); - p.translate(0, GRAPH_HEIGHT + LABELS_HEIGHT); - if (xlabel != NULL) - p.drawText(QRectF(0, 0, GRAPH_WIDTH, LABELS_HEIGHT), Qt::AlignCenter, - xlabel); - - p.resetTransform(); -} - -void EkfBagVideo::DrawMahalanobis(QPainter& p, const QRect& rect) { - float data[MAHAL_NUM_BINS]; - std::string data_labels[MAHAL_NUM_BINS + 1]; - for (int i = 0; i < MAHAL_NUM_BINS; i++) { - data[i] = static_cast(mahal_bins_[i]); - std::stringstream stream; - stream << std::fixed << std::setprecision(1) << (i * MAHAL_BIN_SIZE); - data_labels[i] = stream.str(); - } - data_labels[MAHAL_NUM_BINS] = std::string("\u221E"); - DrawBarGraph(p, rect, MAHAL_NUM_BINS, data, 0.0, 10.0, - "Visual Landmarks Mahalanobis Distance Histogram", NULL, NULL, - data_labels, false); -} - -void EkfBagVideo::DrawSMFeatureCount(QPainter& p, const QRect& rect) { - float count = static_cast(ml_count_); - std::string data_label("Map Features"); - DrawBarGraph(p, rect, 1, &count, 0.0, 50.0, NULL, NULL, NULL, &data_label, - true); -} - -void EkfBagVideo::DrawOFFeatureCount(QPainter& p, const QRect& rect) { - float count = static_cast(of_count_); - std::string data_label("VO Features"); - DrawBarGraph(p, rect, 1, &count, 0.0, 50.0, NULL, NULL, NULL, &data_label, - true); -} - -void EkfBagVideo::DrawStatus(QPainter& p, const QRect& rect) { - QColor color = QColor::fromRgb(0xFF, 0x00, 0x00); - std::string text("Lost"); - if (state_.confidence == ff_msgs::EkfState::CONFIDENCE_GOOD) { - color = QColor::fromRgb(0x00, 0xFF, 0x00); - text = std::string("Good"); - } else if (state_.confidence == ff_msgs::EkfState::CONFIDENCE_POOR) { - color = QColor::fromRgb(0xFF, 0xFF, 0x00); - text = std::string("Poor"); - } - - QPen white(QColor::fromRgb(0xFF, 0xFF, 0xFF)); - QPen black(QColor::fromRgb(0xFF, 0xFF, 0xFF)); - white.setWidth(1.0); - p.setPen(white); - p.setBrush(QBrush(color)); - QFont font("Arial"); - font.setPointSize(10); - p.setFont(font); - - p.drawRect(rect); - - p.setPen(black); - p.drawText(rect, Qt::AlignCenter, text.c_str()); -} - -void EkfBagVideo::DrawVelocity(QPainter& p, const QRect& rect) { - std::string data_labels[] = {std::string("x"), std::string("y"), - std::string("z")}; - float data[] = {static_cast(state_.velocity.x), - static_cast(state_.velocity.y), - static_cast(state_.velocity.z)}; - DrawBarGraph(p, rect, 3, data, -0.25, 0.25, "Velocity", NULL, "m/s", - data_labels, true); -} - -void EkfBagVideo::DrawAccelBias(QPainter& p, const QRect& rect) { - std::string data_labels[] = {std::string("x"), std::string("y"), - std::string("z")}; - float data[] = {static_cast(state_.accel_bias.x), - static_cast(state_.accel_bias.y), - static_cast(state_.accel_bias.z)}; - DrawBarGraph(p, rect, 3, data, -0.01, 0.01, "Accel Bias", NULL, "m/s^2", - data_labels, true); -} - -void EkfBagVideo::DrawOmega(QPainter& p, const QRect& rect) { - std::string data_labels[] = {std::string("\u03D1"), std::string("\u03C6"), - std::string("\u03C8")}; - float data[] = {static_cast(state_.omega.x * 180.0 / M_PI), - static_cast(state_.omega.y * 180.0 / M_PI), - static_cast(state_.omega.z * 180.0 / M_PI)}; - DrawBarGraph(p, rect, 3, data, -45.0, 45.0, "Angular Velocity", NULL, - "\u00B0/s", data_labels, true); -} - -void EkfBagVideo::DrawGyroBias(QPainter& p, const QRect& rect) { - std::string data_labels[] = {std::string("\u03D1"), std::string("\u03C6"), - std::string("\u03C8")}; - float data[] = {static_cast(state_.gyro_bias.x * 180.0 / M_PI), - static_cast(state_.gyro_bias.y * 180.0 / M_PI), - static_cast(state_.gyro_bias.z * 180.0 / M_PI)}; - DrawBarGraph(p, rect, 3, data, -0.1, 0.1, "Gyro Bias", NULL, "\u00B0/s", - data_labels, true); -} - -void EkfBagVideo::DrawPositionCov(QPainter& p, const QRect& rect) { - std::string data_labels[] = {std::string("x"), std::string("y"), - std::string("z")}; - float data[] = {static_cast(sqrt(state_.cov_diag[12]) * 100.0), - static_cast(sqrt(state_.cov_diag[13]) * 100.0), - static_cast(sqrt(state_.cov_diag[14]) * 100.0)}; - DrawBarGraph(p, rect, 3, data, 0.0, 2.0, "Position Std. Dev.", NULL, "cm", - data_labels, true); -} - -void EkfBagVideo::DrawVelocityCov(QPainter& p, const QRect& rect) { - std::string data_labels[] = {std::string("x"), std::string("y"), - std::string("z")}; - float data[] = {static_cast(sqrt(state_.cov_diag[7]) * 100.0), - static_cast(sqrt(state_.cov_diag[8]) * 100.0), - static_cast(sqrt(state_.cov_diag[9]) * 100.0)}; - DrawBarGraph(p, rect, 3, data, 0.0, 0.2, "Velocity Std. Dev.", NULL, "cm/s", - data_labels, true); -} - -void EkfBagVideo::DrawAngleCov(QPainter& p, const QRect& rect) { - std::string data_labels[] = {std::string("\u03D1"), std::string("\u03C6"), - std::string("\u03C8")}; - float data[] = {static_cast(sqrt(state_.cov_diag[0]) * 180.0 / M_PI), - static_cast(sqrt(state_.cov_diag[1]) * 180.0 / M_PI), - static_cast(sqrt(state_.cov_diag[2]) * 180.0 / M_PI)}; - DrawBarGraph(p, rect, 3, data, 0.0, 0.1, "Orientation Std. Dev.", NULL, - "\u00B0", data_labels, true); -} - -void EkfBagVideo::DrawTimestamp(QPainter& p, const QRect& rect) { - QPen white(QColor::fromRgb(0xFF, 0xFF, 0xFF)); - p.setPen(white); - QFont font("Arial"); - - p.translate(rect.x() + 20, rect.y() + 20); - font.setPointSize(28); - p.setFont(font); - std::stringstream ros_ts; - std::stringstream bag_ts; - int millis = last_image_time_.nsec / 1E6; - ros::Duration ellapsed = last_image_time_ - bag_start_time_; - ros_ts << last_image_time_.sec << "." << std::setfill('0') << std::setw(3) - << millis; - int hours = ellapsed.sec / 3600; - int total_minutes = ellapsed.sec / 60; - int minutes = total_minutes % 60; - int seconds = ellapsed.sec % 60; - bag_ts << std::setfill('0') << std::setw(2) << hours << ":" - << std::setfill('0') << std::setw(2) << minutes << ":" - << std::setfill('0') << std::setw(2) << seconds << "." << millis / 100; - p.drawText(QRectF(0, 0, rect.width(), 60), Qt::AlignLeft, - ros_ts.str().c_str()); - p.translate(0, 40); - p.drawText(QRectF(0, 0, rect.width(), 60), Qt::AlignLeft, - bag_ts.str().c_str()); - p.resetTransform(); -} - -void EkfBagVideo::UpdateImage(const ros::Time& time, - const sensor_msgs::ImageConstPtr& ros_image) { - EkfBag::UpdateImage(time, ros_image); - - last_image_ = ros_image; - last_image_time_ = time; -} - -void EkfBagVideo::DrawFrame(void) { - QImage image(1920, 1080, QImage::Format_ARGB32); - image.fill(QColor::fromRgb(0, 0, 0)); - QPainter p(&image); - - DrawImage(p, last_image_, QRect(0, 0, 1280, 960)); - - // draw side widgets - // starts at pixel 1280, width is 640 - DrawPose(p, QRect(1280, 0, 640, 640)); - - DrawVelocity(p, QRect(1280, 640, 320, 110)); - DrawAccelBias(p, QRect(1280, 750, 320, 110)); - DrawPositionCov(p, QRect(1280, 860, 320, 110)); - DrawVelocityCov(p, QRect(1280, 970, 320, 110)); - - DrawOmega(p, QRect(1600, 640, 320, 110)); - DrawGyroBias(p, QRect(1600, 750, 320, 110)); - DrawAngleCov(p, QRect(1600, 860, 320, 110)); - // gyro_bias - // accel - // accel_bias - - DrawTimestamp(p, QRect(1600, 970, 320, 110)); - - // bottom - DrawStatus(p, QRect(0, 960, 120, 120)); - DrawSMFeatureCount(p, QRect(120, 960, 120, 120)); - DrawOFFeatureCount(p, QRect(240, 960, 120, 120)); - DrawMahalanobis(p, QRect(360, 960, 920, 120)); - - p.end(); - video_.AddFrame(image); -} - -void EkfBagVideo::UpdateEKF(const ff_msgs::EkfState& s) { - EkfBag::UpdateEKF(s); - - state_ = s; - - pose_count_++; - if (pose_count_ == 10) { - pose_count_ = 0; - if (pose_history_.size() > 200) pose_history_.pop_front(); - pose_history_.push_back(s.pose); - } - - tracked_sm_.UpdatePose(s.pose); - - if (state_.ml_count > 0) { - memset(mahal_bins_, 0, sizeof(int) * MAHAL_NUM_BINS); - for (unsigned int i = 0; i < state_.ml_mahal_dists.size(); i++) { - float m = state_.ml_mahal_dists[i]; - if (std::isnan(m)) continue; - mahal_bins_[std::min(MAHAL_NUM_BINS - 1, - static_cast(m / MAHAL_BIN_SIZE))]++; - } - ml_count_ = state_.ml_count; - } - - if (state_.of_count > 0) of_count_ = state_.of_count; - - if (!start_time_set_) { - start_time_ = s.header.stamp; - start_time_set_ = true; - } - - if ((s.header.stamp - last_draw_time_).toSec() >= 1.0 / 15) { - last_draw_time_ = s.header.stamp; - DrawFrame(); - } -} - -void EkfBagVideo::UpdateOpticalFlow(const ff_msgs::Feature2dArray& of) { - EkfBag::UpdateOpticalFlow(of); - - if (!start_time_set_) return; - float t = (of.header.stamp - start_time_).toSec(); - - tracked_of_.UpdateFeatures(of, t); -} - -void EkfBagVideo::UpdateSparseMap(const ff_msgs::VisualLandmarks& vl) { - EkfBag::UpdateSparseMap(vl); - - if (!start_time_set_) return; - float t = (vl.header.stamp - start_time_).toSec(); - - tracked_sm_.UpdateFeatures(vl, t); -} - -} // namespace ekf_video diff --git a/tools/ekf_video/src/video_writer.cc b/tools/ekf_video/src/video_writer.cc deleted file mode 100644 index 911a54a9c8..0000000000 --- a/tools/ekf_video/src/video_writer.cc +++ /dev/null @@ -1,222 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include - -#include - -extern "C" { -#include -#include -#include -} - -#include - -namespace ekf_video { - -bool libav_initialized = false; - -void InitializeLibAv(void) { - if (libav_initialized) - return; - libav_initialized = true; - - av_register_all(); - avcodec_register_all(); -} - -VideoWriter::VideoWriter(const char* videofile, int width, int height) : - width_(width), height_(height) { - InitializeLibAv(); - OpenVideo(videofile); -} - -VideoWriter::~VideoWriter(void) { - CloseVideo(); -} - -void VideoWriter::OpenVideo(const char* videofile) { - // open container - avformat_alloc_output_context2(&format_context_, NULL, NULL, videofile); - if (format_context_ == NULL) { - fprintf(stderr, "Could not allocate format context.\n"); - exit(1); - } - assert(format_context_->oformat->video_codec != AV_CODEC_ID_NONE); - - // open codec - AVCodec* codec = avcodec_find_encoder(AV_CODEC_ID_H264); - if (codec == NULL) { - fprintf(stderr, "Could not open encoder.\n"); - exit(1); - } - assert(codec->type == AVMEDIA_TYPE_VIDEO); - AVStream* stream = avformat_new_stream(format_context_, codec); - if (stream == NULL) { - fprintf(stderr, "Could not open stream.\n"); - exit(1); - } - stream->id = 0; - stream->time_base.num = 1; - stream->time_base.den = 15; - codec_context_ = stream->codec; - - avcodec_get_context_defaults3(codec_context_, codec); - codec_context_->codec_id = AV_CODEC_ID_H264; - codec_context_->bit_rate = 400000; - codec_context_->width = width_; - codec_context_->height = height_; - codec_context_->gop_size = 15; - codec_context_->time_base.num = 1; - codec_context_->time_base.den = 15; - codec_context_->pix_fmt = AV_PIX_FMT_YUV420P; - if (format_context_->oformat->flags & AVFMT_GLOBALHEADER) - codec_context_->flags |= AV_CODEC_FLAG_GLOBAL_HEADER; - - AVDictionary* dict = NULL; - av_dict_set(&dict, "preset", "veryslow", 0); - av_dict_set_int(&dict, "crf", 10, 0); - int result = avcodec_open2(codec_context_, codec, &dict); - if (result < 0) { - fprintf(stderr, "Failed to open codec with error %d.\n", result); - exit(1); - } - av_dict_free(&dict); - - // allocate frame - frame_ = av_frame_alloc(); - if (frame_ == NULL) { - fprintf(stderr, "Failed to allocate frame.\n"); - exit(1); - } - frame_->format = codec_context_->pix_fmt; - frame_->width = codec_context_->width; - frame_->height = codec_context_->height; - result = av_frame_get_buffer(frame_, 32); - if (result < 0) { - fprintf(stderr, "Failed to allocate picture.\n"); - exit(1); - } - - av_dump_format(format_context_, 0, videofile, 1); - - if (!(format_context_->oformat->flags & AVFMT_NOFILE)) { - result = avio_open(&format_context_->pb, videofile, AVIO_FLAG_WRITE); - if (result < 0) { - fprintf(stderr, "Failed to open file %s with error %d.\n", videofile, result); - exit(1); - } - } - result = avformat_write_header(format_context_, NULL); - if (result < 0) { - fprintf(stderr, "Failed to write header.\n"); - exit(1); - } - - frame_->pts = 0; -} - -void VideoWriter::CloseVideo(void) { - // flush encoder - int output_received = 0; - do { - int result = EncodeFrame(NULL, &output_received); - if (result < 0) { - fprintf(stderr, "Error flushing codec.\n"); - break; - } - } while (output_received); - - // close everything - av_write_trailer(format_context_); - for (unsigned int i = 0; i < format_context_->nb_streams; i++) - if (format_context_->streams[i]->codec) - avcodec_close(format_context_->streams[i]->codec); - av_frame_free(&frame_); - if (!(format_context_->oformat->flags & AVFMT_NOFILE)) - avio_close(format_context_->pb); - avformat_free_context(format_context_); - format_context_ = NULL; -} - -int VideoWriter::EncodeFrame(AVFrame* frame, int* output_received) { - AVPacket packet; - av_init_packet(&packet); - packet.data = NULL; - packet.size = 0; - - int update = 0; - int result = avcodec_encode_video2(codec_context_, &packet, frame, &update); - if (output_received) - *output_received = update; - if (result < 0) { - fprintf(stderr, "Failed to encode frame with error %d.\n", result); - return result; - } - if (update) { - if (frame && frame->key_frame) - packet.flags |= AV_PKT_FLAG_KEY; - packet.pts = av_rescale_q_rnd(packet.pts, codec_context_->time_base, - format_context_->streams[0]->time_base, AV_ROUND_NEAR_INF); - packet.dts = av_rescale_q_rnd(packet.dts, codec_context_->time_base, - format_context_->streams[0]->time_base, AV_ROUND_NEAR_INF); - packet.duration = av_rescale_q(packet.duration, codec_context_->time_base, format_context_->streams[0]->time_base); - packet.stream_index = 0; - result = av_interleaved_write_frame(format_context_, &packet); - if (result < 0) { - fprintf(stderr, "Failed to write frame with error %d.\n", result); - return result; - } - } - av_free_packet(&packet); - - return 0; -} - -void VideoWriter::AddFrame(const QImage & image) { - struct SwsContext* context = sws_getContext(image.width(), image.height(), AV_PIX_FMT_BGRA, - width_, height_, AV_PIX_FMT_YUV420P, SWS_BICUBIC, NULL, NULL, NULL); - int result = av_frame_make_writable(frame_); - if (result < 0) { - fprintf(stderr, "Failed to make frame writable.\n"); - exit(1); - } - - const unsigned char* source_image = image.constBits(); - int image_size = image.bytesPerLine(); - - sws_scale(context, &source_image, &image_size, 0, image.height(), frame_->data, frame_->linesize); - sws_freeContext(context); - - if (frame_->pts % 15 == 0) - frame_->key_frame = 1; - else - frame_->key_frame = 0; - - result = EncodeFrame(frame_, NULL); - if (result < 0) { - fprintf(stderr, "Failed to encode frame.\n"); - exit(1); - } - - frame_->pts++; -} - -} // namespace ekf_video - diff --git a/tools/ekf_video/tools/ekf_to_video.cc b/tools/ekf_video/tools/ekf_to_video.cc deleted file mode 100644 index 88b0fb9688..0000000000 --- a/tools/ekf_video/tools/ekf_to_video.cc +++ /dev/null @@ -1,59 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include - -#include -#include - -DEFINE_bool(gen_features, false, - "If true, generate features from image, otherwise use from bag."); -DEFINE_bool(run_ekf, false, - "If true, run EKF, otherwise read messages from bag."); -DEFINE_bool( - use_jem, false, - "If true, use JEM mode: draw both a top view and side view of poses."); -DEFINE_string(image_topic, "/hw/cam_nav", "The topic to get images from.."); - -int main(int argc, char* argv[]) { - ff_common::InitFreeFlyerApplication(&argc, &argv); - - ros::Time::init(); - - QGuiApplication a(argc, argv); - if (argc < 4) { - LOG(INFO) << "Usage: " << argv[0] << " map.map bag.bag output.mp4"; - exit(0); - } - - const char* biasfile = NULL; - if (argc >= 5) biasfile = argv[4]; - - // have to run in this strange way because Qt requires a QApplication - // This will run the task from the application event loop. - QTimer::singleShot(0, [argv, biasfile]() { - ekf_video::EkfBagVideo bag(argv[2], argv[1], argv[3], FLAGS_run_ekf, - FLAGS_gen_features, FLAGS_use_jem, biasfile, - FLAGS_image_topic); - bag.Run(); - exit(0); - }); - - return a.exec(); -}