diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider index 61fee4dc23..27f6648911 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider @@ -7,4 +7,3 @@ param set-default FW_THR_CRUISE 0.0 param set-default RWTO_TKOFF 0 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc b/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc index 3759c229e6..7b77ebca99 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc @@ -56,4 +56,3 @@ param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4072_draco b/ROMFS/px4fmu_common/init.d/airframes/4072_draco index 525bc56514..4181f9f03f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4072_draco +++ b/ROMFS/px4fmu_common/init.d/airframes/4072_draco @@ -77,4 +77,3 @@ param set-default IMU_DGYRO_CUTOFF 100 # enable to use high-rate logging for better rate tracking analysis param set-default SDLOG_PROFILE 27 - diff --git a/Tools/compress.py b/Tools/compress.py index 1cee672746..e4511e293f 100755 --- a/Tools/compress.py +++ b/Tools/compress.py @@ -17,4 +17,3 @@ def save_compressed(filename): f.write(content_file.read()) save_compressed(filename) - diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py index f9c9b5d487..467a911c58 100755 --- a/Tools/module_config/generate_params.py +++ b/Tools/module_config/generate_params.py @@ -342,4 +342,3 @@ for yaml_file in args.config_files: if verbose: print("Generating {:}".format(params_output_file)) with open(params_output_file, 'w') as fid: fid.write(all_params) - diff --git a/Tools/module_config/output_groups_from_timer_config.py b/Tools/module_config/output_groups_from_timer_config.py index 2520aa1cb2..998fd2ea5e 100755 --- a/Tools/module_config/output_groups_from_timer_config.py +++ b/Tools/module_config/output_groups_from_timer_config.py @@ -199,4 +199,3 @@ if __name__ == '__main__': output_groups, timer_params = get_output_groups(timer_groups, verbose=verbose) print('output groups: {:}'.format(output_groups)) print('timer params: {:}'.format(timer_params)) - diff --git a/Tools/px4airframes/markdownout.py b/Tools/px4airframes/markdownout.py index 8907e3efb8..d8ac5aeb33 100644 --- a/Tools/px4airframes/markdownout.py +++ b/Tools/px4airframes/markdownout.py @@ -25,7 +25,7 @@ div.frame_common table, div.frame_common table { } div.frame_common table { - float: right; + float: right; width: 70%; } diff --git a/Tools/px4events/jsonout.py b/Tools/px4events/jsonout.py index 456f3a2e1a..7e5affb08a 100644 --- a/Tools/px4events/jsonout.py +++ b/Tools/px4events/jsonout.py @@ -55,4 +55,3 @@ class JsonOutput(): if need_to_write: with codecs.open(filename, 'w', 'utf-8') as f: f.write(json.dumps(self.json,indent=2)) - diff --git a/Tools/test_keys/key0.pub b/Tools/test_keys/key0.pub index 7eb9984de1..86153ac965 100644 --- a/Tools/test_keys/key0.pub +++ b/Tools/test_keys/key0.pub @@ -1,5 +1,5 @@ //Public key to verify signed binaries -0x4d, 0xb0, 0xc2, 0x1, 0x5, 0x55, 0x2a, 0x3c, -0xd7, 0xfb, 0xaf, 0x5c, 0xba, 0x7a, 0xb0, 0x81, -0x1b, 0x36, 0x63, 0xdb, 0x28, 0x52, 0x5e, 0xdb, -0x14, 0x36, 0xf2, 0x57, 0x8d, 0x2, 0xb7, 0xfd, +0x4d, 0xb0, 0xc2, 0x1, 0x5, 0x55, 0x2a, 0x3c, +0xd7, 0xfb, 0xaf, 0x5c, 0xba, 0x7a, 0xb0, 0x81, +0x1b, 0x36, 0x63, 0xdb, 0x28, 0x52, 0x5e, 0xdb, +0x14, 0x36, 0xf2, 0x57, 0x8d, 0x2, 0xb7, 0xfd, diff --git a/Tools/test_keys/rsa2048.pub b/Tools/test_keys/rsa2048.pub index 0fa4872a05..57dfa81ce3 100644 --- a/Tools/test_keys/rsa2048.pub +++ b/Tools/test_keys/rsa2048.pub @@ -1,38 +1,38 @@ -0x30, 0x82, 0x1, 0x22, 0x30, 0xd, 0x6, 0x9, -0x2a, 0x86, 0x48, 0x86, 0xf7, 0xd, 0x1, 0x1, -0x1, 0x5, 0x0, 0x3, 0x82, 0x1, 0xf, 0x0, -0x30, 0x82, 0x1, 0xa, 0x2, 0x82, 0x1, 0x1, -0x0, 0xcf, 0xa3, 0xb8, 0xec, 0x4f, 0xac, 0x37, -0xe6, 0x1b, 0xfa, 0x1e, 0xb3, 0x92, 0xe9, 0x1a, -0x83, 0x36, 0x2d, 0x27, 0xa6, 0x4a, 0xab, 0x7e, -0x38, 0x6f, 0x93, 0x7a, 0x1f, 0x60, 0x83, 0x83, -0xac, 0xfd, 0x2e, 0x61, 0xbf, 0x19, 0x10, 0x1f, -0x39, 0xe3, 0x6c, 0xb6, 0xcc, 0xf5, 0x85, 0x7c, -0xb2, 0x38, 0x27, 0x14, 0x43, 0xd7, 0xfe, 0xbc, -0xa1, 0xe8, 0x7d, 0x33, 0x74, 0x93, 0xe7, 0xd6, -0x2b, 0x2f, 0x79, 0x87, 0x59, 0xb0, 0x63, 0x95, -0x5c, 0xe7, 0x4f, 0x60, 0x46, 0xc9, 0xa9, 0xf, -0xde, 0xa9, 0x36, 0x9e, 0x3e, 0x34, 0xba, 0x77, -0x18, 0x50, 0xe3, 0x6b, 0x3, 0x93, 0xaa, 0xc7, -0xf2, 0x2d, 0xe, 0xf3, 0x2b, 0x5, 0x34, 0x60, -0xa4, 0xf7, 0xa9, 0xe2, 0x37, 0xa3, 0x6d, 0x27, -0xd8, 0xa3, 0x71, 0x9c, 0x6b, 0xb2, 0x5e, 0xdd, -0x12, 0x2b, 0x73, 0xe1, 0xdb, 0x9d, 0xf4, 0xc3, -0xc, 0xd6, 0x5, 0x1d, 0xac, 0xa4, 0xdc, 0x69, -0x2a, 0xf5, 0x7c, 0x30, 0x24, 0x7b, 0x2b, 0x80, -0x1b, 0x7a, 0xec, 0x68, 0x77, 0x12, 0x97, 0x2a, -0xb2, 0xe5, 0xd2, 0xff, 0x41, 0x1d, 0xf5, 0x6, -0xa4, 0xb6, 0x47, 0xa5, 0xfa, 0x5, 0x7e, 0xf0, -0xa1, 0xb8, 0xad, 0xde, 0x5c, 0xae, 0x2c, 0x8b, -0x4e, 0xad, 0xaf, 0xac, 0x6d, 0x46, 0x62, 0xe1, -0xe9, 0xfe, 0xa1, 0xc5, 0xc9, 0xf4, 0x48, 0xd2, -0x80, 0x2b, 0xe5, 0xa9, 0x3e, 0xc1, 0xfc, 0x98, -0xde, 0x3, 0xa9, 0xac, 0x86, 0xb7, 0xb5, 0x19, -0xe5, 0x75, 0x6a, 0x63, 0x83, 0x3f, 0x27, 0x7a, -0x5e, 0xf8, 0xf1, 0x6d, 0xb2, 0xd2, 0x36, 0xa1, -0xc0, 0xd2, 0x5f, 0x93, 0x9c, 0x90, 0xe8, 0x3e, -0xca, 0x89, 0xd7, 0xca, 0xd0, 0xc7, 0xe8, 0x8f, -0x4b, 0xb7, 0x60, 0x92, 0x5e, 0x36, 0x43, 0x70, -0x23, 0x36, 0xfb, 0xb5, 0xa5, 0x6, 0x9a, 0xc0, -0xcb, 0x2, 0x3, 0x1, 0x0, 0x1, \ No newline at end of file +0x30, 0x82, 0x1, 0x22, 0x30, 0xd, 0x6, 0x9, +0x2a, 0x86, 0x48, 0x86, 0xf7, 0xd, 0x1, 0x1, +0x1, 0x5, 0x0, 0x3, 0x82, 0x1, 0xf, 0x0, +0x30, 0x82, 0x1, 0xa, 0x2, 0x82, 0x1, 0x1, +0x0, 0xcf, 0xa3, 0xb8, 0xec, 0x4f, 0xac, 0x37, +0xe6, 0x1b, 0xfa, 0x1e, 0xb3, 0x92, 0xe9, 0x1a, +0x83, 0x36, 0x2d, 0x27, 0xa6, 0x4a, 0xab, 0x7e, +0x38, 0x6f, 0x93, 0x7a, 0x1f, 0x60, 0x83, 0x83, +0xac, 0xfd, 0x2e, 0x61, 0xbf, 0x19, 0x10, 0x1f, +0x39, 0xe3, 0x6c, 0xb6, 0xcc, 0xf5, 0x85, 0x7c, +0xb2, 0x38, 0x27, 0x14, 0x43, 0xd7, 0xfe, 0xbc, +0xa1, 0xe8, 0x7d, 0x33, 0x74, 0x93, 0xe7, 0xd6, +0x2b, 0x2f, 0x79, 0x87, 0x59, 0xb0, 0x63, 0x95, +0x5c, 0xe7, 0x4f, 0x60, 0x46, 0xc9, 0xa9, 0xf, +0xde, 0xa9, 0x36, 0x9e, 0x3e, 0x34, 0xba, 0x77, +0x18, 0x50, 0xe3, 0x6b, 0x3, 0x93, 0xaa, 0xc7, +0xf2, 0x2d, 0xe, 0xf3, 0x2b, 0x5, 0x34, 0x60, +0xa4, 0xf7, 0xa9, 0xe2, 0x37, 0xa3, 0x6d, 0x27, +0xd8, 0xa3, 0x71, 0x9c, 0x6b, 0xb2, 0x5e, 0xdd, +0x12, 0x2b, 0x73, 0xe1, 0xdb, 0x9d, 0xf4, 0xc3, +0xc, 0xd6, 0x5, 0x1d, 0xac, 0xa4, 0xdc, 0x69, +0x2a, 0xf5, 0x7c, 0x30, 0x24, 0x7b, 0x2b, 0x80, +0x1b, 0x7a, 0xec, 0x68, 0x77, 0x12, 0x97, 0x2a, +0xb2, 0xe5, 0xd2, 0xff, 0x41, 0x1d, 0xf5, 0x6, +0xa4, 0xb6, 0x47, 0xa5, 0xfa, 0x5, 0x7e, 0xf0, +0xa1, 0xb8, 0xad, 0xde, 0x5c, 0xae, 0x2c, 0x8b, +0x4e, 0xad, 0xaf, 0xac, 0x6d, 0x46, 0x62, 0xe1, +0xe9, 0xfe, 0xa1, 0xc5, 0xc9, 0xf4, 0x48, 0xd2, +0x80, 0x2b, 0xe5, 0xa9, 0x3e, 0xc1, 0xfc, 0x98, +0xde, 0x3, 0xa9, 0xac, 0x86, 0xb7, 0xb5, 0x19, +0xe5, 0x75, 0x6a, 0x63, 0x83, 0x3f, 0x27, 0x7a, +0x5e, 0xf8, 0xf1, 0x6d, 0xb2, 0xd2, 0x36, 0xa1, +0xc0, 0xd2, 0x5f, 0x93, 0x9c, 0x90, 0xe8, 0x3e, +0xca, 0x89, 0xd7, 0xca, 0xd0, 0xc7, 0xe8, 0x8f, +0x4b, 0xb7, 0x60, 0x92, 0x5e, 0x36, 0x43, 0x70, +0x23, 0x36, 0xfb, 0xb5, 0xa5, 0x6, 0x9a, 0xc0, +0xcb, 0x2, 0x3, 0x1, 0x0, 0x1, diff --git a/boards/modalai/fc-v2/init/rc.board_defaults b/boards/modalai/fc-v2/init/rc.board_defaults index 73266f3af2..118e22528d 100644 --- a/boards/modalai/fc-v2/init/rc.board_defaults +++ b/boards/modalai/fc-v2/init/rc.board_defaults @@ -2,4 +2,3 @@ # # ModalAI FC-v2 specific board defaults #------------------------------------------------------------------------------ - diff --git a/cmake/px4_list_make_absolute.cmake b/cmake/px4_list_make_absolute.cmake index e087040719..3964b7bd4e 100644 --- a/cmake/px4_list_make_absolute.cmake +++ b/cmake/px4_list_make_absolute.cmake @@ -55,4 +55,3 @@ function(px4_list_make_absolute var prefix) endforeach(f) set(${var} "${list_var}" PARENT_SCOPE) endfunction() - diff --git a/msg/actuator_motors.msg b/msg/actuator_motors.msg index b4a6ed85cf..9e09111309 100644 --- a/msg/actuator_motors.msg +++ b/msg/actuator_motors.msg @@ -6,4 +6,3 @@ uint8 NUM_CONTROLS = 8 float32[8] control # range: [-1, 1], where 1 means maximum positive thrust, # -1 maximum negative (if not supported by the output, <0 maps to NaN), # and NaN maps to disarmed (stop the motors) - diff --git a/msg/actuator_servos.msg b/msg/actuator_servos.msg index 6600d0d2e3..2c7900e811 100644 --- a/msg/actuator_servos.msg +++ b/msg/actuator_servos.msg @@ -6,4 +6,3 @@ uint8 NUM_CONTROLS = 8 float32[8] control # range: [-1, 1], where 1 means maximum positive position, # -1 maximum negative, # and NaN maps to disarmed - diff --git a/msg/tools/generate_msg_docs.py b/msg/tools/generate_msg_docs.py index 64718987f8..c3f542a8e6 100755 --- a/msg/tools/generate_msg_docs.py +++ b/msg/tools/generate_msg_docs.py @@ -28,24 +28,24 @@ if __name__ == "__main__": msg_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),"..") msg_files = get_msgs_list(msg_path) msg_files.sort() - + filelist_in_markdown='' - + for msg_file in msg_files: msg_name = os.path.splitext(msg_file)[0] output_file = os.path.join(output_dir, msg_name+'.md') msg_filename = os.path.join(msg_path, msg_file) print("{:} -> {:}".format(msg_filename, output_file)) - + #Format msg url msg_url="[source file](https://github.com/PX4/PX4-Autopilot/blob/master/msg/%s)" % msg_file - + msg_description = "" summary_description = "" #Get msg description (first non-empty comment line from top of msg) with open(msg_filename, 'r') as lineparser: - line = lineparser.readline() + line = lineparser.readline() while line.startswith('#') or (line.strip() == ''): print('DEBUG: line: %s' % line) line=line[1:].strip()+'\n' @@ -65,10 +65,10 @@ if __name__ == "__main__": #Get msg contents (read the file) with open(msg_filename, 'r') as source_file: msg_contents = source_file.read() - + #Format markdown using msg name, comment, url, contents. markdown_output="""# %s (UORB message) - + %s %s @@ -78,27 +78,26 @@ if __name__ == "__main__": ``` """ % (msg_name, msg_description, msg_url, msg_contents) - with open(output_file, 'w') as content_file: content_file.write(markdown_output) - + readme_markdown_file_link='- [%s](%s.md)' % (msg_name,msg_name) if summary_description: readme_markdown_file_link+=" — %s" % summary_description filelist_in_markdown+=readme_markdown_file_link+"\n" - + # Write out the README.md file readme_text="""# uORB Message Reference :::note This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/master/msg/tools/generate_msg_docs.py) from the source code. ::: - + This topic lists the UORB messages available in PX4 (some of which may be may be shared by the [PX4-ROS 2 Bridge](../ros/ros2_comm.md)). Graphs showing how these are used [can be found here](../middleware/uorb_graph.md). - + %s """ % (filelist_in_markdown) readme_file = os.path.join(output_dir, 'README.md') with open(readme_file, 'w') as content_file: - content_file.write(readme_text) + content_file.write(readme_text) diff --git a/platforms/nuttx/src/bootloader/stm/CMakeLists.txt b/platforms/nuttx/src/bootloader/stm/CMakeLists.txt index d92823f010..36843c5694 100644 --- a/platforms/nuttx/src/bootloader/stm/CMakeLists.txt +++ b/platforms/nuttx/src/bootloader/stm/CMakeLists.txt @@ -32,4 +32,3 @@ ############################################################################ add_subdirectory(${PX4_CHIP}) - diff --git a/src/drivers/barometer/CMakeLists.txt b/src/drivers/barometer/CMakeLists.txt index 9ea21af6d3..037daaf269 100644 --- a/src/drivers/barometer/CMakeLists.txt +++ b/src/drivers/barometer/CMakeLists.txt @@ -41,4 +41,3 @@ add_subdirectory(lps33hw) add_subdirectory(maiertek) add_subdirectory(ms5611) #add_subdirectory(tcbp001ta) # only for users who really need this - diff --git a/src/drivers/linux_pwm_out/module.yaml b/src/drivers/linux_pwm_out/module.yaml index 8869e7b2f9..3c35e962ee 100644 --- a/src/drivers/linux_pwm_out/module.yaml +++ b/src/drivers/linux_pwm_out/module.yaml @@ -9,4 +9,3 @@ actuator_output: max: { min: 1600, max: 2200, default: 2000 } failsafe: { min: 800, max: 2200 } num_channels: 8 - diff --git a/src/drivers/pca9685_pwm_out/module.yaml b/src/drivers/pca9685_pwm_out/module.yaml index 20b955cf78..09c5aab015 100644 --- a/src/drivers/pca9685_pwm_out/module.yaml +++ b/src/drivers/pca9685_pwm_out/module.yaml @@ -9,4 +9,3 @@ actuator_output: max: { min: 1600, max: 2200, default: 2000 } failsafe: { min: 800, max: 2200 } num_channels: 16 - diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml index bec86df77e..ea0a3910b3 100644 --- a/src/drivers/pwm_out/module.yaml +++ b/src/drivers/pwm_out/module.yaml @@ -4,7 +4,7 @@ actuator_output: - generator: pwm param_prefix: '${PWM_MAIN_OR_AUX}' channel_labels: ['${PWM_MAIN_OR_AUX}', 'PWM Capture'] - standard_params: + standard_params: disarmed: { min: 800, max: 2200, default: 900 } min: { min: 800, max: 1400, default: 1000 } max: { min: 1600, max: 2200, default: 2000 } @@ -30,4 +30,3 @@ actuator_output: 200: PWM200 400: PWM400 reboot_required: true - diff --git a/src/drivers/pwm_out_sim/module.yaml b/src/drivers/pwm_out_sim/module.yaml index 4a74becf49..947e977de2 100644 --- a/src/drivers/pwm_out_sim/module.yaml +++ b/src/drivers/pwm_out_sim/module.yaml @@ -5,4 +5,3 @@ actuator_output: - param_prefix: ${PWM_MAIN_OR_HIL} channel_label: ${PWM_MAIN_OR_HIL} num_channels: 16 - diff --git a/src/drivers/px4io/module.yaml b/src/drivers/px4io/module.yaml index 9d0ec8fcbd..e4d3d8ad43 100644 --- a/src/drivers/px4io/module.yaml +++ b/src/drivers/px4io/module.yaml @@ -26,4 +26,3 @@ actuator_output: 200: PWM200 400: PWM400 reboot_required: true - diff --git a/src/drivers/tap_esc/CMakeLists.txt b/src/drivers/tap_esc/CMakeLists.txt index 46bf745bde..7dcdfa6e71 100644 --- a/src/drivers/tap_esc/CMakeLists.txt +++ b/src/drivers/tap_esc/CMakeLists.txt @@ -50,4 +50,3 @@ px4_add_module( output_limit tunes ) - diff --git a/src/drivers/tap_esc/module.yaml b/src/drivers/tap_esc/module.yaml index 8bf79b85aa..3f9ce23ed3 100644 --- a/src/drivers/tap_esc/module.yaml +++ b/src/drivers/tap_esc/module.yaml @@ -4,4 +4,3 @@ actuator_output: - param_prefix: TAP_ESC channel_label: 'TAP ESC' num_channels: 8 - diff --git a/src/drivers/uavcan/module.yaml b/src/drivers/uavcan/module.yaml index c738c38387..96e446d8f9 100644 --- a/src/drivers/uavcan/module.yaml +++ b/src/drivers/uavcan/module.yaml @@ -17,4 +17,3 @@ actuator_output: max: { min: 0, max: 1000, default: 1000 } failsafe: { min: 0, max: 1000 } num_channels: 8 - diff --git a/src/lib/component_information/CMakeLists.txt b/src/lib/component_information/CMakeLists.txt index f39ea55207..9e40d72909 100644 --- a/src/lib/component_information/CMakeLists.txt +++ b/src/lib/component_information/CMakeLists.txt @@ -89,6 +89,3 @@ add_custom_command(OUTPUT ${component_general_json} ${component_general_json}.xz COMMENT "Generating component_general.json and checksums.h" ) add_custom_target(component_general_json DEPENDS ${component_general_json}) - - - diff --git a/src/lib/events/CMakeLists.txt b/src/lib/events/CMakeLists.txt index 8c3287a328..b3540ffb3d 100644 --- a/src/lib/events/CMakeLists.txt +++ b/src/lib/events/CMakeLists.txt @@ -110,4 +110,3 @@ add_custom_command(OUTPUT ${generated_events_header} ) add_custom_target(events_header DEPENDS ${generated_events_header}) add_dependencies(prebuild_targets events_header) - diff --git a/src/lib/mixer_module/functions.hpp b/src/lib/mixer_module/functions.hpp index 1285fd75f1..4dd51c2058 100644 --- a/src/lib/mixer_module/functions.hpp +++ b/src/lib/mixer_module/functions.hpp @@ -240,4 +240,3 @@ private: uORB::Subscription _topic{ORB_ID(actuator_controls_2)}; float _data[3] { NAN, NAN, NAN }; }; - diff --git a/src/lib/mixer_module/generate_function_header.py b/src/lib/mixer_module/generate_function_header.py index a08a4d7524..56e299378e 100755 --- a/src/lib/mixer_module/generate_function_header.py +++ b/src/lib/mixer_module/generate_function_header.py @@ -72,4 +72,3 @@ enum class OutputFunction : int32_t {{ {2} }}; '''.format(os.path.basename(__file__), yaml_file, function_defs)) - diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index a2972dbdab..43245b2d80 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -281,4 +281,3 @@ PARAM_DEFINE_INT32(SYS_FAILURE_EN, 0); * @group System */ PARAM_DEFINE_INT32(SYS_CTRL_ALLOC, 0); - diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/EulToQuat.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/EulToQuat.m index 9b57a20b5d..89d1ca5092 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/EulToQuat.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/EulToQuat.m @@ -19,5 +19,3 @@ quaterion(3,1) = (cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi); quaterion(4,1) = (cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi); return; - - diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/Quat2Tbn.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/Quat2Tbn.m index 663d19cde8..3748f965a6 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/Quat2Tbn.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/Quat2Tbn.m @@ -8,7 +8,7 @@ q0 = quat(1); q1 = quat(2); q2 = quat(3); q3 = quat(4); - + Tbn = [q0^2 + q1^2 - q2^2 - q3^2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2); ... 2*(q1*q2 + q0*q3), q0^2 - q1^2 + q2^2 - q3^2, 2*(q2*q3 - q0*q1); ... - 2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2]; \ No newline at end of file + 2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatMult.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatMult.m index 357c545d22..8572529091 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatMult.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatMult.m @@ -1,5 +1,5 @@ function quatOut = QuatMult(quatA,quatB) -% Calculate the following quaternion product quatA * quatB using the +% Calculate the following quaternion product quatA * quatB using the % standard identity quatOut = [quatA(1)*quatB(1)-quatA(2:4)'*quatB(2:4); quatA(1)*quatB(2:4) + quatB(1)*quatA(2:4) + cross(quatA(2:4),quatB(2:4))]; \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/AlignHeading.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/AlignHeading.m index 3b3862b6e5..1e817e1fcc 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/AlignHeading.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/AlignHeading.m @@ -1,6 +1,6 @@ function quat = AlignHeading( ... quat, ... % quaternion state vector - magMea, ... % body frame magnetic flux measurements + magMea, ... % body frame magnetic flux measurements declination) % Estimated magnetic field delination at current location (rad) % Get the Euler angles and set yaw to zero @@ -20,4 +20,4 @@ euler(3) = declination - atan2(magMeasNED(2),magMeasNED(1)); % convert to a quaternion quat = EulToQuat(euler); -end \ No newline at end of file +end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBodyVel.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBodyVel.m index 4a1e37e24d..798b89a499 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBodyVel.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBodyVel.m @@ -28,7 +28,7 @@ relVelBodyPred = transpose(Tbn)*[vn;ve;vd]; % calculate the observation jacobian, innovation variance and innovation for obsIndex = 1:3 - + % Calculate corrections using X component if (obsIndex == 1) H(1,:) = calcH_VELX(q0,q1,q2,q3,vd,ve,vn); @@ -53,30 +53,30 @@ end for obsIndex = 1:3 Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex); - + % correct the state vector states = states - Kfusion * innovation(obsIndex); - + % normalise the updated quaternion states quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2); if (quatMag > 1e-12) states(1:4) = states(1:4) / quatMag; end - + % correct the covariance P = P - K*H*P P = P - Kfusion*H(obsIndex,:)*P; - + % Force symmetry on the covariance matrix to prevent ill-conditioning % of the matrix which would cause the filter to blow-up P = 0.5*(P + transpose(P)); - + % ensure diagonals are positive for i=1:24 if P(i,i) < 0 P(i,i) = 0; end end - + end -end \ No newline at end of file +end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m index 58281fd624..6e66bc5962 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m @@ -33,7 +33,7 @@ magPred = transpose(Tbn)*[magN;magE;magD] + [magXbias;magYbias;magZbias]; % calculate the observation jacobian, innovation variance and innovation for obsIndex = 1:3 - + % Calculate corrections using X component if (obsIndex == 1) H(1,:) = calcH_MAGX(magD,magE,magN,q0,q1,q2,q3); @@ -58,30 +58,30 @@ end for obsIndex = 1:3 Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex); - + % correct the state vector states = states - Kfusion * innovation(obsIndex); - + % normalise the updated quaternion states quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2); if (quatMag > 1e-12) states(1:4) = states(1:4) / quatMag; end - + % correct the covariance P = P - K*H*P P = P - Kfusion*H(obsIndex,:)*P; - + % Force symmetry on the covariance matrix to prevent ill-conditioning % of the matrix which would cause the filter to blow-up P = 0.5*(P + transpose(P)); - + % ensure diagonals are positive for i=1:24 if P(i,i) < 0 P(i,i) = 0; end end - + end -end \ No newline at end of file +end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m index 2b18c552d0..0c9e0e7f14 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m @@ -36,7 +36,7 @@ losRateMea = - flowRate + bodyRate; % calculate the observation jacobian, innovation variance and innovation for obsIndex = 1:2 - + % Calculate corrections using X component if (obsIndex == 1) H(1,:) = calcH_LOSX(q0,q1,q2,q3,range,vd,ve,vn); @@ -59,30 +59,30 @@ end for obsIndex = 1:2 Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex); - + % correct the state vector states = states - Kfusion * innovation(obsIndex); - + % normalise the updated quaternion states quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2); if (quatMag > 1e-12) states(1:4) = states(1:4) / quatMag; end - + % correct the covariance P = P - K*H*P P = P - Kfusion*H(obsIndex,:)*P; - + % Force symmetry on the covariance matrix to prevent ill-conditioning % of the matrix which would cause the filter to blow-up P = 0.5*(P + transpose(P)); - + % ensure diagonals are positive for i=1:24 if P(i,i) < 0 P(i,i) = 0; end end - + end -end \ No newline at end of file +end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FusePosition.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FusePosition.m index d1a31c1fce..c25f6dd770 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FusePosition.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FusePosition.m @@ -15,54 +15,54 @@ varInnov = zeros(1,2); H = zeros(2,24); for obsIndex = 1:2 - + % velocity states start at index 8 stateIndex = 7 + obsIndex; % Calculate the velocity measurement innovation innovation(obsIndex) = states(stateIndex) - measPos(obsIndex); - + % Calculate the observation Jacobian H(obsIndex,stateIndex) = 1; - + varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS); - + end % Apply an innovation consistency check for obsIndex = 1:2 - + if (innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0 return; end - + end % Calculate Kalman gains and update states and covariances for obsIndex = 1:2 - + % Calculate the Kalman gains K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex); - + % Calculate state corrections xk = K * innovation(obsIndex); - + % Apply the state corrections states = states - xk; - + % Update the covariance P = P - K*H(obsIndex,:)*P; - + % Force symmetry on the covariance matrix to prevent ill-conditioning P = 0.5*(P + transpose(P)); - + % ensure diagonals are positive for i=1:24 if P(i,i) < 0 P(i,i) = 0; end end - + end -end \ No newline at end of file +end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseVelocity.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseVelocity.m index 432470aaaa..bc533beb44 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseVelocity.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseVelocity.m @@ -15,54 +15,54 @@ varInnov = zeros(1,3); H = zeros(3,24); for obsIndex = 1:3 - + % velocity states start at index 5 stateIndex = 4 + obsIndex; % Calculate the velocity measurement innovation innovation(obsIndex) = states(stateIndex) - measVel(obsIndex); - + % Calculate the observation Jacobian H(obsIndex,stateIndex) = 1; - + varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS); - + end % Apply an innovation consistency check for obsIndex = 1:3 - + if (innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0 return; end - + end % Calculate Kalman gains and update states and covariances for obsIndex = 1:3 - + % Calculate the Kalman gains K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex); - + % Calculate state corrections xk = K * innovation(obsIndex); - + % Apply the state corrections states = states - xk; - + % Update the covariance P = P - K*H(obsIndex,:)*P; - + % Force symmetry on the covariance matrix to prevent ill-conditioning P = 0.5*(P + transpose(P)); - + % ensure diagonals are positive for i=1:24 if P(i,i) < 0 P(i,i) = 0; end end - + end -end \ No newline at end of file +end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/GenerateEquations24.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/GenerateEquations24.m index a33a53d6f6..90f9259893 100755 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/GenerateEquations24.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/GenerateEquations24.m @@ -32,7 +32,7 @@ quat = [q0;q1;q2;q3]; Tbn = Quat2Tbn(quat); % define the truth delta angle -% ignore coning compensation as these effects are negligible in terms of +% ignore coning compensation as these effects are negligible in terms of % covariance growth for our application and grade of sensor dAngTruth = dAngMeas - dAngBias; @@ -137,7 +137,7 @@ f = matlabFunction(H_MAGD,'file','calcH_MAGD.m'); %% derive equations for fusion of a single magneic compass heading measurement % rotate body measured field into earth axes -magMeasNED = Tbn*[magX;magY;magZ]; +magMeasNED = Tbn*[magX;magY;magZ]; % the predicted measurement is the angle wrt true north of the horizontal % component of the measured field diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitCovariance.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitCovariance.m index 6695498493..dccefb852c 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitCovariance.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitCovariance.m @@ -33,4 +33,3 @@ Sigma_wind = param.alignment.windErrNE * [1;1]; % Convert to variances and write to covariance matrix diagonals covariance = diag([Sigma_quat;Sigma_velocity;Sigma_position;Sigma_dAngBias;Sigma_dVelBias;Sigma_magNED;Sigma_magXYZ;Sigma_wind].^2); end - diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PlotData.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PlotData.m index 4ba5d5ab04..8bc9bc357b 100755 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PlotData.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PlotData.m @@ -215,7 +215,7 @@ if (output.magFuseMethod <= 1) set(h,'PaperOrientation','portrait'); set(h,'PaperUnits','normalized'); set(h,'PaperPosition', [0 0 1 1]); - + subplot(3,1,1); plot(output.time_lapsed',[output.mag_XYZ(:,1),output.mag_XYZ(:,1)+2*sqrt(output.state_variances(:,20)),output.mag_XYZ(:,1)-2*sqrt(output.state_variances(:,20))]); grid on; @@ -224,21 +224,21 @@ if (output.magFuseMethod <= 1) ylabel('X bias (gauss)'); xlabel('time (sec)'); legend('estimate','upper 95% bound','lower 95% bound'); - + subplot(3,1,2); plot(output.time_lapsed',[output.mag_XYZ(:,2),output.mag_XYZ(:,2)+2*sqrt(output.state_variances(:,21)),output.mag_XYZ(:,2)-2*sqrt(output.state_variances(:,21))]); grid on; ylabel('Y bias (gauss)'); xlabel('time (sec)'); legend('estimate','upper 95% bound','lower 95% bound'); - + subplot(3,1,3); plot(output.time_lapsed',[output.mag_XYZ(:,3),output.mag_XYZ(:,3)+2*sqrt(output.state_variances(:,22)),output.mag_XYZ(:,3)-2*sqrt(output.state_variances(:,22))]); grid on; ylabel('Z bias (gauss)'); xlabel('time (sec)'); legend('estimate','upper 95% bound','lower 95% bound'); - + fileName='body_field_estimates.png'; fullFileName = fullfile(folder, fileName); saveas(h,fullFileName); @@ -251,9 +251,9 @@ if (output.magFuseMethod <= 1) set(h,'PaperOrientation','portrait'); set(h,'PaperUnits','normalized'); set(h,'PaperPosition', [0 0 1 1]); - + margin = 0.1; - + subplot(4,1,1); plot(output.time_lapsed',[output.mag_NED(:,1),output.mag_NED(:,1)+2*sqrt(output.state_variances(:,17)),output.mag_NED(:,1)-2*sqrt(output.state_variances(:,17))]); minVal = min(output.mag_NED(:,1))-margin; @@ -265,7 +265,7 @@ if (output.magFuseMethod <= 1) ylabel('North (gauss)'); xlabel('time (sec)'); legend('estimate','upper 95% bound','lower 95% bound'); - + subplot(4,1,2); plot(output.time_lapsed',[output.mag_NED(:,2),output.mag_NED(:,2)+2*sqrt(output.state_variances(:,18)),output.mag_NED(:,2)-2*sqrt(output.state_variances(:,18))]); minVal = min(output.mag_NED(:,2))-margin; @@ -275,7 +275,7 @@ if (output.magFuseMethod <= 1) ylabel('East (gauss)'); xlabel('time (sec)'); legend('estimate','upper 95% bound','lower 95% bound'); - + subplot(4,1,3); plot(output.time_lapsed',[output.mag_NED(:,3),output.mag_NED(:,3)+2*sqrt(output.state_variances(:,19)),output.mag_NED(:,3)-2*sqrt(output.state_variances(:,19))]); grid on; @@ -285,7 +285,7 @@ if (output.magFuseMethod <= 1) ylabel('Down (gauss)'); xlabel('time (sec)'); legend('estimate','upper 95% bound','lower 95% bound'); - + subplot(4,1,4); plot(output.time_lapsed',rad2deg*atan2(output.mag_NED(:,2),output.mag_NED(:,1))); grid on; @@ -293,7 +293,7 @@ if (output.magFuseMethod <= 1) title(titleText); ylabel('declination (deg)'); xlabel('time (sec)'); - + fileName='earth_field_estimates.png'; fullFileName = fullfile(folder, fileName); saveas(h,fullFileName); @@ -301,13 +301,13 @@ end %% plot velocity innovations if isfield(output.innovations,'vel_innov') - + figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); h=gcf; set(h,'PaperOrientation','portrait'); set(h,'PaperUnits','normalized'); set(h,'PaperPosition', [0 0 1 1]); - + subplot(3,1,1); plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,1),sqrt(output.innovations.vel_innov_var(:,1)),-sqrt(output.innovations.vel_innov_var(:,1))]); grid on; @@ -316,21 +316,21 @@ if isfield(output.innovations,'vel_innov') ylabel('North (m/s)'); xlabel('time (sec)'); legend('innovation','variance sqrt','variance sqrt'); - + subplot(3,1,2); plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,2),sqrt(output.innovations.vel_innov_var(:,2)),-sqrt(output.innovations.vel_innov_var(:,2))]); grid on; ylabel('East (m/s)'); xlabel('time (sec)'); legend('innovation','variance sqrt','variance sqrt'); - + subplot(3,1,3); plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,3),sqrt(output.innovations.vel_innov_var(:,3)),-sqrt(output.innovations.vel_innov_var(:,3))]); grid on; ylabel('Down (m/s)'); xlabel('time (sec)'); legend('innovation','variance sqrt','variance sqrt'); - + fileName='velocity_fusion.png'; fullFileName = fullfile(folder, fileName); saveas(h,fullFileName); @@ -343,7 +343,7 @@ if isfield(output.innovations,'posInnov') set(h,'PaperOrientation','portrait'); set(h,'PaperUnits','normalized'); set(h,'PaperPosition', [0 0 1 1]); - + subplot(3,1,1); plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,1),sqrt(output.innovations.posInnovVar(:,1)),-sqrt(output.innovations.posInnovVar(:,1))]); grid on; @@ -352,21 +352,21 @@ if isfield(output.innovations,'posInnov') ylabel('North (m)'); xlabel('time (sec)'); legend('innovation','variance sqrt','variance sqrt'); - + subplot(3,1,2); plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,2),sqrt(output.innovations.posInnovVar(:,2)),-sqrt(output.innovations.posInnovVar(:,2))]); grid on; ylabel('East (m)'); xlabel('time (sec)'); legend('innovation','variance sqrt','variance sqrt'); - + subplot(3,1,3); plot(output.innovations.hgt_time_lapsed',[output.innovations.hgtInnov(:),sqrt(output.innovations.hgtInnovVar(:)),-sqrt(output.innovations.hgtInnovVar(:))]); grid on; ylabel('Up (m)'); xlabel('time (sec)'); legend('innovation','variance sqrt','variance sqrt'); - + fileName='position_fusion.png'; fullFileName = fullfile(folder, fileName); saveas(h,fullFileName); @@ -374,7 +374,7 @@ end %% plot magnetometer innovations if isfield(output.innovations,'magInnov') - + figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); h=gcf; set(h,'PaperOrientation','portrait'); @@ -412,18 +412,18 @@ if isfield(output.innovations,'magInnov') fileName='magnetometer_fusion.png'; fullFileName = fullfile(folder, fileName); saveas(h,fullFileName); - + end %% plot magnetic yaw innovations if isfield(output.innovations,'hdgInnov') - + figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); h=gcf; set(h,'PaperOrientation','portrait'); set(h,'PaperUnits','normalized'); set(h,'PaperPosition', [0 0 1 1]); - + subplot(2,1,1); plot(output.innovations.mag_time_lapsed,[output.innovations.hdgInnov*rad2deg;sqrt(output.innovations.hdgInnovVar)*rad2deg;-sqrt(output.innovations.hdgInnovVar)*rad2deg]); ylim([-30 30]); @@ -442,12 +442,12 @@ if isfield(output.innovations,'hdgInnov') fileName='magnetometer_fusion.png'; fullFileName = fullfile(folder, fileName); saveas(h,fullFileName); - + end %% plot optical flow innovations if isfield(output.innovations,'flowInnov') - + figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); h=gcf; set(h,'PaperOrientation','portrait'); @@ -471,17 +471,18 @@ if isfield(output.innovations,'flowInnov') fileName='optical_flow_fusion.png'; fullFileName = fullfile(folder, fileName); saveas(h,fullFileName); - + end + %% plot ZED camera innovations if isfield(output.innovations,'bodyVelInnov') - + figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); h=gcf; set(h,'PaperOrientation','portrait'); set(h,'PaperUnits','normalized'); set(h,'PaperPosition', [0 0 1 1]); - + subplot(3,1,1); plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,1)';sqrt(output.innovations.bodyVelInnovVar(:,1))';-sqrt(output.innovations.bodyVelInnovVar(:,1))']); grid on; @@ -489,23 +490,23 @@ if isfield(output.innovations,'bodyVelInnov') ylabel('X (m/sec)'); xlabel('time (sec)'); legend('innovation','innovation variance sqrt','innovation variance sqrt'); - + subplot(3,1,2); plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,2)';sqrt(output.innovations.bodyVelInnovVar(:,2))';-sqrt(output.innovations.bodyVelInnovVar(:,2))']); grid on; ylabel('Y (m/sec)'); xlabel('time (sec)'); legend('innovation','innovation variance sqrt','innovation variance sqrt'); - + subplot(3,1,3); plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,3)';sqrt(output.innovations.bodyVelInnovVar(:,3))';-sqrt(output.innovations.bodyVelInnovVar(:,3))']); grid on; ylabel('Z (m/sec)'); xlabel('time (sec)'); legend('innovation','innovation variance sqrt','innovation variance sqrt'); - + fileName='zed_camera_fusion.png'; fullFileName = fullfile(folder, fileName); saveas(h,fullFileName); - + end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictCovariance.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictCovariance.m index 89e445d4bf..6f6a57a853 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictCovariance.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictCovariance.m @@ -3,8 +3,8 @@ function P = PredictCovariance(deltaAngle, ... states,... P, ... % Previous state covariance matrix dt, ... % IMU and prediction time step - param) % tuning parameters - + param) % tuning parameters + % Set filter state process noise other than IMU errors, which are already % built into the derived covariance prediction equations. % This process noise determines the rate of estimation of IMU bias errors @@ -63,4 +63,4 @@ for i=1:24 end end -end \ No newline at end of file +end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictStates.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictStates.m index 88f3d64ff9..bf6096008a 100755 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictStates.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictStates.m @@ -44,10 +44,10 @@ delAngEarth_NED(3,1) = -0.000072921 * sin(latitude) * dt; % Apply corrections for coning errors and earth spin rate % Coning correction from : -% "A new strapdown attitude algorithm", -% R. B. MILLER, +% "A new strapdown attitude algorithm", +% R. B. MILLER, % Journal of Guidance, Control, and Dynamics -% July, Vol. 6, No. 4, pp. 287-291, Eqn 11 +% July, Vol. 6, No. 4, pp. 287-291, Eqn 11 % correctedDelAng = delAng - 1/12*cross(prevDelAng , delAng) - transpose(Tbn_prev)*delAngEarth_NED; correctedDelAng = delAng - transpose(Tbn_prev)*delAngEarth_NED; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/RunFilter.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/RunFilter.m index 6ff8f377e8..33259f94c7 100755 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/RunFilter.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/RunFilter.m @@ -106,20 +106,20 @@ last_synthetic_velocity_fusion_time = 0; last_valid_range_time = - param.fusion.rngTimeout; for index = indexStart:indexStop - + % read IMU measurements local_time=imu_data.time_us(imuIndex)*1e-6; delta_angle(:,1) = imu_data.del_ang(imuIndex,:); delta_velocity(:,1) = imu_data.del_vel(imuIndex,:); dt_imu = 0.5 * (imu_data.accel_dt(imuIndex) + imu_data.gyro_dt(imuIndex)); imuIndex = imuIndex+1; - + % predict states [states, delAngCorrected, delVelCorrected] = PredictStates(states,delta_angle,delta_velocity,imu_data.accel_dt(imuIndex),gravity,gps_data.refLLH(1,1)*deg2rad); - + % constrain states [states] = ConstrainStates(states,dt_imu_avg); - + dtCov = dtCov + dt_imu; delAngCov = delAngCov + delAngCorrected; delVelCov = delVelCov + delVelCorrected; @@ -131,7 +131,7 @@ for index = indexStart:indexStop dtCovInt = dtCovInt + dtCov; dtCov = 0; covIndex = covIndex + 1; - + % output state data output.time_lapsed(covIndex) = local_time; output.euler_angles(covIndex,:) = QuatToEul(states(1:4)')'; @@ -142,22 +142,22 @@ for index = indexStart:indexStop output.mag_NED(covIndex,:) = states(17:19); output.mag_XYZ(covIndex,:) = states(20:22); output.wind_NE(covIndex,:) = states(23:24); - + % output covariance data for i=1:24 output.state_variances(covIndex,i) = covariance(i,i); end - + % output equivalent euler angle variances error_transfer_matrix = quat_to_euler_error_transfer_matrix(states(1),states(2),states(3),states(4)); euler_covariance_matrix = error_transfer_matrix * covariance(1:4,1:4) * transpose(error_transfer_matrix); for i=1:3 output.euler_variances(covIndex,i) = euler_covariance_matrix(i,i); end - + % Get most recent GPS data that had fallen behind the fusion time horizon latest_gps_index = find((gps_data.time_us - 1e6 * param.fusion.gpsTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); - + if ~isempty(latest_gps_index) % Check if GPS use is being blocked by the user if ((local_time < param.control.gpsOnTime) && (local_time > param.control.gpsOffTime)) @@ -166,7 +166,7 @@ for index = indexStart:indexStop else gps_use_blocked = false; end - + % If we haven't started using GPS, check that the quality is sufficient before aligning the position and velocity states to GPS if (~gps_use_started && ~gps_use_blocked) if ((gps_data.spd_error(latest_gps_index) < param.control.gpsSpdErrLim) && (gps_data.pos_error(latest_gps_index) < param.control.gpsPosErrLim)) @@ -176,24 +176,24 @@ for index = indexStart:indexStop last_drift_constrain_time = local_time; end end - + % Fuse GPS data when available if GPS use has started if (gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index)) last_gps_index = latest_gps_index; gps_fuse_index = gps_fuse_index + 1; last_drift_constrain_time = local_time; - + % fuse NED GPS velocity [states,covariance,velInnov,velInnovVar] = FuseVelocity(states,covariance,gps_data.vel_ned(latest_gps_index,:),param.fusion.gpsVelGate,gps_data.spd_error(latest_gps_index)); - + % data logging output.innovations.vel_time_lapsed(gps_fuse_index) = local_time; output.innovations.vel_innov(gps_fuse_index,:) = velInnov'; output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar'; - + % fuse NE GPS position [states,covariance,posInnov,posInnovVar] = FusePosition(states,covariance,gps_data.pos_ned(latest_gps_index,:),param.fusion.gpsPosGate,gps_data.pos_error(latest_gps_index)); - + % data logging output.innovations.pos_time_lapsed(gps_fuse_index) = local_time; output.innovations.posInnov(gps_fuse_index,:) = posInnov'; @@ -208,13 +208,13 @@ for index = indexStart:indexStop end end end - + % Fuse new Baro data that has fallen beind the fusion time horizon latest_baro_index = find((baro_data.time_us - 1e6 * param.fusion.baroTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); if (latest_baro_index > last_baro_index) last_baro_index = latest_baro_index; baro_fuse_index = baro_fuse_index + 1; - + % fuse baro height [states,covariance,hgtInnov,hgtInnovVar] = FuseBaroHeight(states,covariance,baro_data.height(latest_baro_index),param.fusion.baroHgtGate,param.fusion.baroHgtNoise); @@ -223,125 +223,125 @@ for index = indexStart:indexStop output.innovations.hgtInnov(baro_fuse_index) = hgtInnov; output.innovations.hgtInnovVar(baro_fuse_index) = hgtInnovVar; end - + % Fuse new mag data that has fallen behind the fusion time horizon latest_mag_index = find((mag_data.time_us - 1e6 * param.fusion.magTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); if (latest_mag_index > last_mag_index) last_mag_index = latest_mag_index; mag_fuse_index = mag_fuse_index + 1; - + % output magnetic field length to help with diagnostics output.innovations.magLength(mag_fuse_index) = sqrt(dot(mag_data.field_ga(latest_mag_index,:),mag_data.field_ga(latest_mag_index,:))); - + % fuse magnetometer data if (param.fusion.magFuseMethod == 0 || param.fusion.magFuseMethod == 1) [states,covariance,magInnov,magInnovVar] = FuseMagnetometer(states,covariance,mag_data.field_ga(latest_mag_index,:),param.fusion.magFieldGate, param.fusion.magFieldError^2); - + % data logging output.innovations.mag_time_lapsed(mag_fuse_index) = local_time; output.innovations.magInnov(mag_fuse_index,:) = magInnov; output.innovations.magInnovVar(mag_fuse_index,:) = magInnovVar; - + if (param.fusion.magFuseMethod == 1) % fuse in the local declination value [states, covariance] = FuseMagDeclination(states, covariance, param.fusion.magDeclDeg*deg2rad); - + end elseif (param.fusion.magFuseMethod == 2) % fuse magnetometer data as a single magnetic heading measurement [states, covariance, hdgInnov, hdgInnovVar] = FuseMagHeading(states, covariance, mag_data.field_ga(latest_mag_index,:), param.fusion.magDeclDeg*deg2rad, param.fusion.magHdgGate, param.fusion.magHdgError^2); - + % log data output.innovations.mag_time_lapsed(mag_fuse_index) = local_time; output.innovations.hdgInnov(mag_fuse_index) = hdgInnov; output.innovations.hdgInnovVar(mag_fuse_index) = hdgInnovVar; - + end - + end - + % Check if optical flow use is being blocked by the user if ((local_time < param.control.flowOnTime) && (local_time > param.control.flowOffTime)) flow_use_blocked = true; else flow_use_blocked = false; end - + % Attempt to use optical flow and range finder data if available and not blocked if (flowDataPresent && ~flow_use_blocked) - + % Get latest range finder data and gate to remove dropouts last_range_index = find((rng_data.time_us - 1e6 * param.fusion.rangeTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); if (rng_data.dist(last_range_index) < param.fusion.rngValidMax) range = max(rng_data.dist(last_range_index) , param.fusion.rngValidMin); last_valid_range_time = local_time; end - + % Fuse optical flow data that has fallen behind the fusion time horizon if we have a valid range measurement latest_flow_index = find((flow_data.time_us - 1e6 * param.fusion.flowTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); - + if (~isempty(latest_flow_index) && (latest_flow_index > last_flow_index) && ((local_time - last_valid_range_time) < param.fusion.rngTimeout)) last_flow_index = latest_flow_index; flow_fuse_index = flow_fuse_index + 1; last_drift_constrain_time = local_time; - + % fuse flow data flowRate = [flow_data.flowX(latest_flow_index);flow_data.flowY(latest_flow_index)]; bodyRate = [flow_data.bodyX(latest_flow_index);flow_data.bodyY(latest_flow_index)]; [states,covariance,flowInnov,flowInnovVar] = FuseOpticalFlow(states, covariance, flowRate, bodyRate, range, param.fusion.flowRateError^2, param.fusion.flowGate); - + % data logging output.innovations.flow_time_lapsed(flow_fuse_index) = local_time; output.innovations.flowInnov(flow_fuse_index,:) = flowInnov; output.innovations.flowInnovVar(flow_fuse_index,:) = flowInnovVar; - + end - + end - + % Check if optical flow use is being blocked by the user if ((local_time < param.control.visoOnTime) && (local_time > param.control.visoOffTime)) viso_use_blocked = true; else viso_use_blocked = false; end - + % attempt to use ZED camera visual odmetry data if available and not blocked if (visoDataPresent && ~viso_use_blocked) - + % Fuse ZED camera body frame odmometry data that has fallen behind the fusion time horizon latest_viso_index = find((viso_data.time_us - 1e6 * param.fusion.bodyVelTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); if (latest_viso_index > last_viso_index) last_viso_index = latest_viso_index; viso_fuse_index = viso_fuse_index + 1; last_drift_constrain_time = local_time; - + % convert delta position measurements to velocity relVelBodyMea = [viso_data.dPosX(latest_viso_index);viso_data.dPosY(latest_viso_index);viso_data.dPosZ(latest_viso_index)]./viso_data.dt(latest_viso_index); - + % convert quality metric to equivalent observation error % (this is a guess) quality = viso_data.qual(latest_viso_index); bodyVelError = param.fusion.bodyVelErrorMin * quality + param.fusion.bodyVelErrorMax * (1 - quality); - + % fuse measurements [states,covariance,bodyVelInnov,bodyVelInnovVar] = FuseBodyVel(states, covariance, relVelBodyMea, bodyVelError^2, param.fusion.bodyVelGate); - + % data logging output.innovations.bodyVel_time_lapsed(viso_fuse_index) = local_time; output.innovations.bodyVelInnov(viso_fuse_index,:) = bodyVelInnov; output.innovations.bodyVelInnovVar(viso_fuse_index,:) = bodyVelInnovVar; - + end - + end - + end - + % update average delta time estimate output.dt = dtCovInt / covIndex; - + end end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_apm_data.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_apm_data.m index af76d37d62..2fe352c5cd 100755 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_apm_data.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_apm_data.m @@ -14,7 +14,7 @@ load '../TestData/APM/mag_data.mat'; if exist('../TestData/APM/rng_data.mat','file') && exist('../TestData/APM/flow_data.mat','file') load '../TestData/APM/rng_data.mat'; load '../TestData/APM/flow_data.mat'; -else +else rng_data = []; flow_data = []; end @@ -29,7 +29,7 @@ end % load default parameters run('SetParameters.m'); -% run the filter replay +% run the filter replay output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data,viso_data); % generate and save output plots @@ -43,4 +43,4 @@ fileName = '../OutputData/APM/ekf_replay_output.mat'; if ~exist(folder,'dir') mkdir(folder); end -save(fileName,'output'); \ No newline at end of file +save(fileName,'output'); diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m b/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m index eae994aacf..3eba488511 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m @@ -76,16 +76,16 @@ mag_strength = zeros(length(mag_meas),1); for i = 1:length(mag_meas) % subtract the offsets mag_corrected_5(:,i) = mag_meas(:,i) - offset; - + % correct the rotation mag_corrected_5(:,i) = rotation_correction * mag_corrected_5(:,i); - + % correct the scale factor mag_corrected_5(:,i) = mag_corrected_5(:,i) .* scale_correction; - + % calculate the residual mag_strength(i) = sqrt(dot(mag_corrected_5(:,i),mag_corrected_5(:,i))); - + end % calculate the fit residual for fit option 5 @@ -104,19 +104,19 @@ angle_change_1 = zeros(length(mag_meas),1); for i = 1:length(mag_meas) % subtract the offsets mag_corrected_1(:,i) = mag_meas(:,i) - offset; - + % correct the rotation mag_corrected_1(:,i) = rotation_correction * mag_corrected_1(:,i); - + % correct the scale factor mag_corrected_1(:,i) = mag_corrected_1(:,i) .* scale_correction; - + % calculate the residual mag_strength(i) = sqrt(dot(mag_corrected_1(:,i),mag_corrected_1(:,i))); - + % calculate the angular change due to the fit angle_change_1(i) = atan2(norm(cross(mag_corrected_1(:,i),mag_meas(:,i))),dot(mag_corrected_1(:,i),mag_meas(:,i))); - + end % calculate the fit residual for fit option 1 @@ -135,19 +135,19 @@ angle_change_0 = zeros(length(mag_meas),1); for i = 1:length(mag_meas) % subtract the offsets mag_corrected_0(:,i) = mag_meas(:,i) - offset; - + % correct the rotation mag_corrected_0(:,i) = rotation_correction * mag_corrected_0(:,i); - + % correct the scale factor mag_corrected_0(:,i) = mag_corrected_0(:,i) .* scale_correction; - + % calculate the residual mag_strength(i) = sqrt(dot(mag_corrected_0(:,i),mag_corrected_0(:,i))); - + % calculate the angular change due to the fit angle_change_0(i) = atan2(norm(cross(mag_corrected_0(:,i),mag_meas(:,i))),dot(mag_corrected_0(:,i),mag_meas(:,i))); - + end % calculate the fit residual for fit option 0 @@ -155,7 +155,7 @@ fit_residual_0 = mag_strength - mean(mag_strength); %% calculate the residual for uncorrected data for i = 1:length(mag_meas) - mag_strength(i) = sqrt(dot(mag_meas(:,i),mag_meas(:,i))); + mag_strength(i) = sqrt(dot(mag_meas(:,i),mag_meas(:,i))); end uncorrected_residual = mag_strength - mean(mag_strength); diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m b/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m index 3269577689..73819b21e2 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m @@ -1,61 +1,61 @@ -function [ofs,gain,rotM]=ellipsoid_fit(XYZ,varargin) +function [ofs,gain,rotM]=ellipsoid_fit(XYZ,varargin) -% Fit an (non)rotated ellipsoid or sphere to a set of xyz data points +% Fit an (non)rotated ellipsoid or sphere to a set of xyz data points -% XYZ: N(rows) x 3(cols), matrix of N data points (x,y,z) +% XYZ: N(rows) x 3(cols), matrix of N data points (x,y,z) -% optional flag f, default to 0 (fitting of rotated ellipsoid) +% optional flag f, default to 0 (fitting of rotated ellipsoid) -x=XYZ(:,1); y=XYZ(:,2); z=XYZ(:,3); if nargin>1, f=varargin{1}; else f=0; end; +x=XYZ(:,1); y=XYZ(:,2); z=XYZ(:,3); if nargin>1, f=varargin{1}; else f=0; end; -if f==0, D=[x.*x, y.*y, z.*z, 2*x.*y,2*x.*z,2*y.*z, 2*x,2*y,2*z]; % any axes (rotated ellipsoid) +if f==0, D=[x.*x, y.*y, z.*z, 2*x.*y,2*x.*z,2*y.*z, 2*x,2*y,2*z]; % any axes (rotated ellipsoid) -elseif f==1, D=[x.*x, y.*y, z.*z, 2*x,2*y,2*z]; % XYZ axes (non-rotated ellipsoid) +elseif f==1, D=[x.*x, y.*y, z.*z, 2*x,2*y,2*z]; % XYZ axes (non-rotated ellipsoid) -elseif f==2, D=[x.*x+y.*y, z.*z, 2*x,2*y,2*z]; % and radius x=y +elseif f==2, D=[x.*x+y.*y, z.*z, 2*x,2*y,2*z]; % and radius x=y -elseif f==3, D=[x.*x+z.*z, y.*y, 2*x,2*y,2*z]; % and radius x=z +elseif f==3, D=[x.*x+z.*z, y.*y, 2*x,2*y,2*z]; % and radius x=z -elseif f==4, D=[y.*y+z.*z, x.*x, 2*x,2*y,2*z]; % and radius y=z +elseif f==4, D=[y.*y+z.*z, x.*x, 2*x,2*y,2*z]; % and radius y=z -elseif f==5, D=[x.*x+y.*y+z.*z, 2*x,2*y,2*z]; % and radius x=y=z (sphere) +elseif f==5, D=[x.*x+y.*y+z.*z, 2*x,2*y,2*z]; % and radius x=y=z (sphere) end; -v = (D'*D)\(D'*ones(length(x),1)); % least square fitting +v = (D'*D)\(D'*ones(length(x),1)); % least square fitting -if f==0, % rotated ellipsoid +if f==0, % rotated ellipsoid - A = [ v(1) v(4) v(5) v(7); v(4) v(2) v(6) v(8); v(5) v(6) v(3) v(9); v(7) v(8) v(9) -1 ]; + A = [ v(1) v(4) v(5) v(7); v(4) v(2) v(6) v(8); v(5) v(6) v(3) v(9); v(7) v(8) v(9) -1 ]; - ofs=-A(1:3,1:3)\[v(7);v(8);v(9)]; % offset is center of ellipsoid + ofs=-A(1:3,1:3)\[v(7);v(8);v(9)]; % offset is center of ellipsoid - Tmtx=eye(4); Tmtx(4,1:3)=ofs'; AT=Tmtx*A*Tmtx'; % ellipsoid translated to (0,0,0) + Tmtx=eye(4); Tmtx(4,1:3)=ofs'; AT=Tmtx*A*Tmtx'; % ellipsoid translated to (0,0,0) - [rotM ev]=eig(AT(1:3,1:3)/-AT(4,4)); % eigenvectors (rotation) and eigenvalues (gain) + [rotM ev]=eig(AT(1:3,1:3)/-AT(4,4)); % eigenvectors (rotation) and eigenvalues (gain) - gain=sqrt(1./diag(ev)); % gain is radius of the ellipsoid + gain=sqrt(1./diag(ev)); % gain is radius of the ellipsoid -else % non-rotated ellipsoid +else % non-rotated ellipsoid - if f==1, v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ]; + if f==1, v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ]; - elseif f==2, v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ]; + elseif f==2, v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ]; - elseif f==3, v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ]; + elseif f==3, v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ]; - elseif f==4, v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ]; + elseif f==4, v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ]; - elseif f==5, v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; % sphere + elseif f==5, v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; % sphere - end; + end; - ofs=-(v(1:3).\v(7:9))'; % offset is center of ellipsoid + ofs=-(v(1:3).\v(7:9))'; % offset is center of ellipsoid - rotM=eye(3); % eigenvectors (rotation), identity = no rotation + rotM=eye(3); % eigenvectors (rotation), identity = no rotation - g=1+(v(7)^2/v(1)+v(8)^2/v(2)+v(9)^2/v(3)); + g=1+(v(7)^2/v(1)+v(8)^2/v(2)+v(9)^2/v(3)); - gain=(sqrt(g./v(1:3)))'; % find radii of the ellipsoid (scale) + gain=(sqrt(g./v(1:3)))'; % find radii of the ellipsoid (scale) end; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m b/src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m index 6d6644f7cd..f5e1c3c2a9 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m @@ -166,7 +166,7 @@ cd ../; %% ------ SECTION 3: Ground Truth Data (STIL only, optional) ------ if exist('attitude_file','var') && exist('localpos_file','var') && exist('globalpos_file','var') - + %- Import Attitude data from text file opts = delimitedTextImportOptions("NumVariables", 13); opts.DataLines = [2, Inf]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/readme.txt b/src/modules/ekf2/EKF/matlab/EKF_replay/readme.txt index fc0d5f6b97..0a2bd490b2 100644 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/readme.txt +++ b/src/modules/ekf2/EKF/matlab/EKF_replay/readme.txt @@ -101,8 +101,8 @@ Finally copy the generated .mat files into the /EKF_replay/TestData/PX4 director 5) Execute ‘SetParameterDefaults’ at the command prompt to load the default filter tuning parameter struct ‘param’ into the workspace. The defaults have been set to provide robust estimation across the entire data set, not optimised for accuracy. -6) Replay the data by running either the replay_apm_data.m, replay_px4_data.m or if you have px4 optical flow data, the replay_px4_optflow_data.m script file. +6) Replay the data by running either the replay_apm_data.m, replay_px4_data.m or if you have px4 optical flow data, the replay_px4_optflow_data.m script file. Output plots are saved as .png files in the ‘…/EKF_replay/OutputPlots/‘ directory. -Output data is written to the Matlab workspace in the ‘output’ struct. \ No newline at end of file +Output data is written to the Matlab workspace in the ‘output’ struct. diff --git a/src/modules/ekf2/EKF/matlab/analysis/estimatorLogViewerPX4.m b/src/modules/ekf2/EKF/matlab/analysis/estimatorLogViewerPX4.m index 643a636bd3..efc3e8adde 100644 --- a/src/modules/ekf2/EKF/matlab/analysis/estimatorLogViewerPX4.m +++ b/src/modules/ekf2/EKF/matlab/analysis/estimatorLogViewerPX4.m @@ -96,20 +96,20 @@ else end centerLineTimeFull = get(lines(k),'XData'); centerLineDataFull = get(lines(k),'YData'); - + startTime = max(centerLineTimeFull(1),dataSrc.Tsec(1)); endTime = min(centerLineTimeFull(end),dataSrc.Tsec(end)); plotTimeFull = dataSrc.Tsec(dataSrc.Tsec >= startTime & dataSrc.Tsec <= endTime); plotDataFull = dataSrc.(varNames{k})(dataSrc.Tsec >= startTime & dataSrc.Tsec <= endTime); - + centerLineTime = centerLineTimeFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime); centerLineData = centerLineDataFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime); - + plotTime = linspace(startTime,endTime,250); plotData = sqrt(interp1(plotTimeFull,plotDataFull,plotTime)); centerLineData = interp1(centerLineTime,centerLineData,plotTime); - -% plotTime = downsample(centerLineTime,round(numel(plotDataT)/350),0); + + % plotTime = downsample(centerLineTime,round(numel(plotDataT)/350),0); if strcmp('IV',varNames{k}(end-1:end)) plotDataL = -plotData; plotDataU = plotData; diff --git a/src/modules/ekf2/EKF/matlab/analysis/importPX4log.m b/src/modules/ekf2/EKF/matlab/analysis/importPX4log.m index 335836058e..42b17658a8 100644 --- a/src/modules/ekf2/EKF/matlab/analysis/importPX4log.m +++ b/src/modules/ekf2/EKF/matlab/analysis/importPX4log.m @@ -59,7 +59,7 @@ while 1 fprintf('%.0f%%\n',percentDone); nextPrint = nextPrint + 5; end - + chunk = fread(fid,BLOCK_SIZE,'uint8'); if numel(chunk) == 0; break @@ -74,7 +74,7 @@ while 1 continue; end msg_type = buffer(ptr+2); - + if msg_type == MSG_TYPE_FORMAT if numel(buffer) - ptr <= MSG_FORMAT_PACKET_LEN break; @@ -144,7 +144,7 @@ msg_type = thisBlock(1); msg_labels = strsplit(LOCAL_parse_string(thisBlock(23:end)),','); msg_struct = cell(1,numel(msg_format)); msg_mults = zeros(1,numel(msg_format)); - + for k = 1:numel(msg_format) info = FORMAT_TO_STRUCT{strcmp(msg_format(k),FORMAT_TO_STRUCT(:,1)),2}; msg_struct{k} = info{1}; diff --git a/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m b/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m index 4e071ecf01..47c78ae335 100644 --- a/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m +++ b/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m @@ -14,11 +14,11 @@ % the X and Y sensor axes. These rates are motion compensated. % A positive LOS X rate is a RH rotation of the image about the X sensor -% axis, and is produced by either a positive ground relative velocity in +% axis, and is produced by either a positive ground relative velocity in % the direction of the Y axis. % A positive LOS Y rate is a RH rotation of the image about the Y sensor -% axis, and is produced by either a negative ground relative velocity in +% axis, and is produced by either a negative ground relative velocity in % the direction of the X axis. % Range measurement aligned with the Z body axis (flat earth model assumed) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/__init__.py b/src/modules/ekf2/EKF/python/ekf_derivation/__init__.py index d4412772f4..1d2b6d584d 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/__init__.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/__init__.py @@ -5,4 +5,3 @@ Created on Sat Mar 14 13:02:26 2020 @author: roman """ - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp index 10136b2698..a6fa3d2484 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp @@ -14,7 +14,7 @@ int main() { // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations - float Hfusion[24]; + float Hfusion[24]; Vector24f H_MAG; Vector24f Kfusion; float mag_innov_var; @@ -30,11 +30,11 @@ int main() q2 /= length; q3 /= length; - const float magN = 2.0f * ((float)rand() - 0.5f); - const float magE = 2.0f * ((float)rand() - 0.5f); - const float magD = 2.0f * ((float)rand() - 0.5f); + const float magN = 2.0f * ((float)rand() - 0.5f); + const float magE = 2.0f * ((float)rand() - 0.5f); + const float magD = 2.0f * ((float)rand() - 0.5f); - const float R_MAG = sq(0.05f); + const float R_MAG = sq(0.05f); const bool update_all_states = true; // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 @@ -50,8 +50,8 @@ int main() } // common expressions used by sympy generated equations - // calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gainss - const float HKX0 = -magD*q2 + magE*q3 + magN*q0; + // calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gainss + const float HKX0 = -magD*q2 + magE*q3 + magN*q0; const float HKX1 = magD*q3 + magE*q2 + magN*q1; const float HKX2 = magE*q1; const float HKX3 = magD*q0; @@ -78,16 +78,16 @@ int main() const float HKX24 = 1.0F/(HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG); // common expressions used by matlab generated equations - float SH_MAG[9]; - SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1; - SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2; - SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3; - SH_MAG[3] = sq(q3); - SH_MAG[4] = sq(q2); - SH_MAG[5] = sq(q1); - SH_MAG[6] = sq(q0); - SH_MAG[7] = 2.0f*magN*q0; - SH_MAG[8] = 2.0f*magE*q3; + float SH_MAG[9]; + SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1; + SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2; + SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3; + SH_MAG[3] = sq(q3); + SH_MAG[4] = sq(q2); + SH_MAG[5] = sq(q1); + SH_MAG[6] = sq(q0); + SH_MAG[7] = 2.0f*magN*q0; + SH_MAG[8] = 2.0f*magE*q3; // Compare X axis equations { @@ -139,8 +139,8 @@ int main() } // repeat calculation using matlab generated equations - // X axis innovation variance - mag_innov_var = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); + // X axis innovation variance + mag_innov_var = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); // Calculate X axis observation jacobians H_MAG.setZero(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp index 47dd3b1b45..73406f8c90 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp @@ -340,5 +340,3 @@ Kfusion(20) = HKZ70*(-HKZ13*P(16,20) - HKZ15*P(3,20) + HKZ17*P(0,20) + HKZ19*P(1 Kfusion(21) = HKZ69*HKZ70; Kfusion(22) = HKZ70*(-HKZ13*P(16,22) - HKZ15*P(3,22) + HKZ17*P(0,22) + HKZ19*P(17,22) + HKZ21*P(18,22) - HKZ23*P(2,22) + HKZ25*P(1,22) - P(21,22)); Kfusion(23) = HKZ70*(-HKZ13*P(16,23) - HKZ15*P(3,23) + HKZ17*P(0,23) + HKZ19*P(17,23) + HKZ21*P(18,23) - HKZ23*P(2,23) + HKZ25*P(1,23) - P(21,23)); - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp index 17befb802f..5535e860e7 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp @@ -45,5 +45,3 @@ Hfusion.at<2>() = IV18*P(16,21) + IV18*(IV18*P(16,16) + IV19*P(3,16) - IV20*P(0, // Kalman gains - axis 2 - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp index 0e6d60826a..5f53c848a3 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp @@ -191,5 +191,3 @@ Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20) Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) + HK9*P(21,23) - HK9*P(5,21)); Kfusion(22) = -HK22*HK32; Kfusion(23) = -HK27*HK32; - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp index 734b2d3d1b..51f8bedc5f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp @@ -198,5 +198,3 @@ Kfusion(23) = -HK79*HK84; // Kalman gains - axis 2 - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp index da34396676..375a0122bf 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp @@ -109,5 +109,3 @@ Kfusion(20) = HK55*(-HK24*P(0,20) - HK34*P(2,20) - HK35*P(3,20) - HK40*P(5,20) - Kfusion(21) = HK55*(-HK24*P(0,21) - HK34*P(2,21) - HK35*P(3,21) - HK40*P(5,21) - HK41*P(6,21) + HK42*P(21,22) - HK43*P(21,23) - HK44*P(4,21) - HK45*P(1,21)); Kfusion(22) = HK48*HK55; Kfusion(23) = HK49*HK55; - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp index 18b563ddd4..98a6c43a3f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp @@ -524,5 +524,3 @@ nextP(20,23) = P(20,23); nextP(21,23) = P(21,23); nextP(22,23) = P(22,23); nextP(23,23) = P(23,23); - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp index 03b144eb95..25cfbab94d 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp @@ -226,5 +226,3 @@ Kfusion(20) = -HK66*(HK47*P(0,20) + HK50*P(4,20) + HK52*P(5,20) + HK53*P(6,20) + Kfusion(21) = -HK66*(HK47*P(0,21) + HK50*P(4,21) + HK52*P(5,21) + HK53*P(6,21) + HK54*P(1,21) + HK55*P(2,21) + HK56*P(3,21)); Kfusion(22) = -HK66*(HK47*P(0,22) + HK50*P(4,22) + HK52*P(5,22) + HK53*P(6,22) + HK54*P(1,22) + HK55*P(2,22) + HK56*P(3,22)); Kfusion(23) = -HK66*(HK47*P(0,23) + HK50*P(4,23) + HK52*P(5,23) + HK53*P(6,23) + HK54*P(1,23) + HK55*P(2,23) + HK56*P(3,23)); - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp index d13faa5c5d..202d82616c 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp @@ -218,5 +218,3 @@ Kfusion(20) = -HK110*(HK100*P(2,20) + HK101*P(3,20) + HK92*P(0,20) + HK95*P(4,20 Kfusion(21) = -HK110*(HK100*P(2,21) + HK101*P(3,21) + HK92*P(0,21) + HK95*P(4,21) + HK97*P(5,21) + HK98*P(6,21) + HK99*P(1,21)); Kfusion(22) = -HK110*(HK100*P(2,22) + HK101*P(3,22) + HK92*P(0,22) + HK95*P(4,22) + HK97*P(5,22) + HK98*P(6,22) + HK99*P(1,22)); Kfusion(23) = -HK110*(HK100*P(2,23) + HK101*P(3,23) + HK92*P(0,23) + HK95*P(4,23) + HK97*P(5,23) + HK98*P(6,23) + HK99*P(1,23)); - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp index f63b262958..f879462e7a 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp @@ -80,5 +80,3 @@ Kfusion(20) = HK26*(HK12*P(0,20) - HK18*P(1,20) - HK19*P(2,20) + HK20*P(3,20)); Kfusion(21) = HK26*(HK12*P(0,21) - HK18*P(1,21) - HK19*P(2,21) + HK20*P(3,21)); Kfusion(22) = HK26*(HK12*P(0,22) - HK18*P(1,22) - HK19*P(2,22) + HK20*P(3,22)); Kfusion(23) = HK26*(HK12*P(0,23) - HK18*P(1,23) - HK19*P(2,23) + HK20*P(3,23)); - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp index 56c59ee421..7c783dc46a 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp @@ -63,5 +63,3 @@ Kfusion(20) = -HK9*(HK5*P(16,20) - P(17,20)); Kfusion(21) = -HK9*(HK5*P(16,21) - P(17,21)); Kfusion(22) = -HK9*(HK5*P(16,22) - P(17,22)); Kfusion(23) = -HK9*(HK5*P(16,23) - P(17,23)); - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp index 5cc31b84fe..aa1d05c8e9 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp @@ -23,18 +23,18 @@ int main() Vector24f Hfusion_matlab; Vector24f Kfusion_matlab; - const float R_TAS = sq(1.5f); + const float R_TAS = sq(1.5f); const bool update_wind_only = false; - // get latest velocity in earth frame - const float vn = 9.0f; - const float ve = 12.0f; - const float vd = -1.5f; + // get latest velocity in earth frame + const float vn = 9.0f; + const float ve = 12.0f; + const float vd = -1.5f; - // get latest wind velocity in earth frame - const float vwn = -4.0f; - const float vwe = 3.0f; + // get latest wind velocity in earth frame + const float vwn = -4.0f; + const float vwe = 3.0f; // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 SquareMatrix24f P; @@ -77,7 +77,7 @@ int main() const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS); // Observation Jacobians - SparseVector24f<4,5,6,22,23> Hfusion; + SparseVector24f<4,5,6,22,23> Hfusion; Hfusion.at<4>() = HK4; Hfusion.at<5>() = HK5; Hfusion.at<6>() = HK3*vd; @@ -189,11 +189,11 @@ int main() } } - if (max_diff_fraction > 1e-5f) { - printf("Fail: Airspeed Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); - } else { - printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction); - } + if (max_diff_fraction > 1e-5f) { + printf("Fail: Airspeed Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); + } else { + printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction); + } // find largest Kalman gain difference as a fraction of the matlab value max_diff_fraction = 0.0f; @@ -217,11 +217,11 @@ int main() } } - if (max_diff_fraction > 1e-5f) { - printf("Fail: Airspeed Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); - } else { - printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction); - } + if (max_diff_fraction > 1e-5f) { + printf("Fail: Airspeed Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); + } else { + printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction); + } return 0; } diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp index 5a5bf5e64d..41a9c2cc1e 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp @@ -70,5 +70,3 @@ Kfusion(20) = HK16*(-HK0*P(20,22) + HK0*P(4,20) - HK1*P(20,23) + HK1*P(5,20) + P Kfusion(21) = HK16*(-HK0*P(21,22) + HK0*P(4,21) - HK1*P(21,23) + HK1*P(5,21) + P(6,21)*vd); Kfusion(22) = HK15*HK16; Kfusion(23) = HK14*HK16; - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp index 1f6f9a6b28..b43f11e858 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp @@ -244,5 +244,3 @@ Kfusion(20) = -HK27*(-HK13*P(3,20) - HK14*P(4,20) + HK15*P(0,20) + HK16*P(5,20) Kfusion(21) = -HK27*(-HK13*P(3,21) - HK14*P(4,21) + HK15*P(0,21) + HK16*P(5,21) - HK17*P(2,21) + HK18*P(6,21) + HK19*P(1,21)); Kfusion(22) = -HK27*(-HK13*P(3,22) - HK14*P(4,22) + HK15*P(0,22) + HK16*P(5,22) - HK17*P(2,22) + HK18*P(6,22) + HK19*P(1,22)); Kfusion(23) = -HK27*(-HK13*P(3,23) - HK14*P(4,23) + HK15*P(0,23) + HK16*P(5,23) - HK17*P(2,23) + HK18*P(6,23) + HK19*P(1,23)); - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp index 7a66b237c5..c84b40b73d 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp @@ -247,5 +247,3 @@ Kfusion(20) = -HK85*(-HK71*P(3,20) - HK72*P(4,20) + HK73*P(0,20) + HK74*P(5,20) Kfusion(21) = -HK85*(-HK71*P(3,21) - HK72*P(4,21) + HK73*P(0,21) + HK74*P(5,21) - HK75*P(2,21) + HK76*P(6,21) + HK77*P(1,21)); Kfusion(22) = -HK85*(-HK71*P(3,22) - HK72*P(4,22) + HK73*P(0,22) + HK74*P(5,22) - HK75*P(2,22) + HK76*P(6,22) + HK77*P(1,22)); Kfusion(23) = -HK85*(-HK71*P(3,23) - HK72*P(4,23) + HK73*P(0,23) + HK74*P(5,23) - HK75*P(2,23) + HK76*P(6,23) + HK77*P(1,23)); - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp index 0fb98bc13f..e130ddf526 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp @@ -17,5 +17,3 @@ _ekf_gsf[model_index].P(1,1) = P(1,1) + P(1,2)*S6 + S1*dvyVar + S3*dvxVar + S6*S _ekf_gsf[model_index].P(0,2) = S5; _ekf_gsf[model_index].P(1,2) = S9; _ekf_gsf[model_index].P(2,2) = P(2,2) + dazVar; - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp index 07a1db0e72..1fa155cf39 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp @@ -64,5 +64,3 @@ _ekf_gsf[model_index].P(1,1) = P(1,1) - t16*t30 + t22*t26; _ekf_gsf[model_index].P(0,2) = P(0,2) + t19*t24 + t20*t25; _ekf_gsf[model_index].P(1,2) = P(1,2) + t23*t27 + t30*t31; _ekf_gsf[model_index].P(2,2) = P(2,2) - t23*t33 - t25*t36; - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp index cbed2c8658..931d287a58 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp @@ -148,5 +148,3 @@ H_YAW.at<20>() = 0; H_YAW.at<21>() = 0; H_YAW.at<22>() = 0; H_YAW.at<23>() = 0; - - diff --git a/src/modules/ekf2/EKF/python/terrain_flow_derivation/derive_terrain_flow.py b/src/modules/ekf2/EKF/python/terrain_flow_derivation/derive_terrain_flow.py index c393bd3fe7..2ddf00621e 100644 --- a/src/modules/ekf2/EKF/python/terrain_flow_derivation/derive_terrain_flow.py +++ b/src/modules/ekf2/EKF/python/terrain_flow_derivation/derive_terrain_flow.py @@ -95,4 +95,3 @@ H_y_simple = cse(H_y, symbols('t0:30')) write_simplified(H_x_simple, "flow_x_observation.txt", "Hx") write_simplified(H_y_simple, "flow_y_observation.txt", "Hy") - diff --git a/src/modules/ekf2/EKF/python/wind_cov_init/derivation.py b/src/modules/ekf2/EKF/python/wind_cov_init/derivation.py index 0b2708d214..c79cfd1aed 100644 --- a/src/modules/ekf2/EKF/python/wind_cov_init/derivation.py +++ b/src/modules/ekf2/EKF/python/wind_cov_init/derivation.py @@ -29,7 +29,7 @@ R_tas_init = 1.4**2 ######################################################################################################################### -# define symbols: true airspeed, sidslip angle, +# define symbols: true airspeed, sidslip angle, V, beta, yaw, groundspeed_body_x, groundspeed_body_y = symbols('V beta yaw vx_body vy_body') R_tas, R_beta, R_yaw = symbols('R_tas R_beta R_yaw') @@ -64,4 +64,4 @@ P_wind_earth_numeric = P_wind_earth_numeric.subs([(groundspeed_body_x, groundspe print('P[22][22] = ' + str(P_wind_earth_numeric[0,0])) print('P[22][23] = ' + str(P_wind_earth_numeric[0,1])) print('P[23][22] = ' + str(P_wind_earth_numeric[1,0])) -print('P[23][23] = ' + str(P_wind_earth_numeric[1,1])) \ No newline at end of file +print('P[23][23] = ' + str(P_wind_earth_numeric[1,1])) diff --git a/src/modules/rc_update/params_deprecated.c b/src/modules/rc_update/params_deprecated.c index 960403f363..93c06dd19c 100644 --- a/src/modules/rc_update/params_deprecated.c +++ b/src/modules/rc_update/params_deprecated.c @@ -203,4 +203,3 @@ PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0); * @value 18 Channel 18 */ PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); - diff --git a/src/systemcmds/sd_stress/CMakeLists.txt b/src/systemcmds/sd_stress/CMakeLists.txt index 8d4ccc9e80..346218764c 100644 --- a/src/systemcmds/sd_stress/CMakeLists.txt +++ b/src/systemcmds/sd_stress/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( sd_stress.c DEPENDS ) -