Whitespace cleanup.

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
-

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
-

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
-

Tools/compress.py

@@ -17,4 +17,3 @@ def save_compressed(filename):
             f.write(content_file.read())
 
 save_compressed(filename)
-

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)
-

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))
-

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%;
 }
 

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))
-

Tools/test_keys/key0.pub

@@ -1,5 +1,5 @@
 //Public key to verify signed binaries
-0x4d, 0xb0, 0xc2, 0x1, 0x5, 0x55, 0x2a, 0x3c, 
+0x4d, 0xb0, 0xc2, 0x1, 0x5, 0x55, 0x2a, 0x3c,
-0xd7, 0xfb, 0xaf, 0x5c, 0xba, 0x7a, 0xb0, 0x81, 
+0xd7, 0xfb, 0xaf, 0x5c, 0xba, 0x7a, 0xb0, 0x81,
-0x1b, 0x36, 0x63, 0xdb, 0x28, 0x52, 0x5e, 0xdb, 
+0x1b, 0x36, 0x63, 0xdb, 0x28, 0x52, 0x5e, 0xdb,
-0x14, 0x36, 0xf2, 0x57, 0x8d, 0x2, 0xb7, 0xfd, 
+0x14, 0x36, 0xf2, 0x57, 0x8d, 0x2, 0xb7, 0xfd,

Tools/test_keys/rsa2048.pub

@@ -1,38 +1,38 @@
 
-0x30, 0x82, 0x1, 0x22, 0x30, 0xd, 0x6, 0x9, 
+0x30, 0x82, 0x1, 0x22, 0x30, 0xd, 0x6, 0x9,
-0x2a, 0x86, 0x48, 0x86, 0xf7, 0xd, 0x1, 0x1, 
+0x2a, 0x86, 0x48, 0x86, 0xf7, 0xd, 0x1, 0x1,
-0x1, 0x5, 0x0, 0x3, 0x82, 0x1, 0xf, 0x0, 
+0x1, 0x5, 0x0, 0x3, 0x82, 0x1, 0xf, 0x0,
-0x30, 0x82, 0x1, 0xa, 0x2, 0x82, 0x1, 0x1, 
+0x30, 0x82, 0x1, 0xa, 0x2, 0x82, 0x1, 0x1,
-0x0, 0xcf, 0xa3, 0xb8, 0xec, 0x4f, 0xac, 0x37, 
+0x0, 0xcf, 0xa3, 0xb8, 0xec, 0x4f, 0xac, 0x37,
-0xe6, 0x1b, 0xfa, 0x1e, 0xb3, 0x92, 0xe9, 0x1a, 
+0xe6, 0x1b, 0xfa, 0x1e, 0xb3, 0x92, 0xe9, 0x1a,
-0x83, 0x36, 0x2d, 0x27, 0xa6, 0x4a, 0xab, 0x7e, 
+0x83, 0x36, 0x2d, 0x27, 0xa6, 0x4a, 0xab, 0x7e,
-0x38, 0x6f, 0x93, 0x7a, 0x1f, 0x60, 0x83, 0x83, 
+0x38, 0x6f, 0x93, 0x7a, 0x1f, 0x60, 0x83, 0x83,
-0xac, 0xfd, 0x2e, 0x61, 0xbf, 0x19, 0x10, 0x1f, 
+0xac, 0xfd, 0x2e, 0x61, 0xbf, 0x19, 0x10, 0x1f,
-0x39, 0xe3, 0x6c, 0xb6, 0xcc, 0xf5, 0x85, 0x7c, 
+0x39, 0xe3, 0x6c, 0xb6, 0xcc, 0xf5, 0x85, 0x7c,
-0xb2, 0x38, 0x27, 0x14, 0x43, 0xd7, 0xfe, 0xbc, 
+0xb2, 0x38, 0x27, 0x14, 0x43, 0xd7, 0xfe, 0xbc,
-0xa1, 0xe8, 0x7d, 0x33, 0x74, 0x93, 0xe7, 0xd6, 
+0xa1, 0xe8, 0x7d, 0x33, 0x74, 0x93, 0xe7, 0xd6,
-0x2b, 0x2f, 0x79, 0x87, 0x59, 0xb0, 0x63, 0x95, 
+0x2b, 0x2f, 0x79, 0x87, 0x59, 0xb0, 0x63, 0x95,
-0x5c, 0xe7, 0x4f, 0x60, 0x46, 0xc9, 0xa9, 0xf, 
+0x5c, 0xe7, 0x4f, 0x60, 0x46, 0xc9, 0xa9, 0xf,
-0xde, 0xa9, 0x36, 0x9e, 0x3e, 0x34, 0xba, 0x77, 
+0xde, 0xa9, 0x36, 0x9e, 0x3e, 0x34, 0xba, 0x77,
-0x18, 0x50, 0xe3, 0x6b, 0x3, 0x93, 0xaa, 0xc7, 
+0x18, 0x50, 0xe3, 0x6b, 0x3, 0x93, 0xaa, 0xc7,
-0xf2, 0x2d, 0xe, 0xf3, 0x2b, 0x5, 0x34, 0x60, 
+0xf2, 0x2d, 0xe, 0xf3, 0x2b, 0x5, 0x34, 0x60,
-0xa4, 0xf7, 0xa9, 0xe2, 0x37, 0xa3, 0x6d, 0x27, 
+0xa4, 0xf7, 0xa9, 0xe2, 0x37, 0xa3, 0x6d, 0x27,
-0xd8, 0xa3, 0x71, 0x9c, 0x6b, 0xb2, 0x5e, 0xdd, 
+0xd8, 0xa3, 0x71, 0x9c, 0x6b, 0xb2, 0x5e, 0xdd,
-0x12, 0x2b, 0x73, 0xe1, 0xdb, 0x9d, 0xf4, 0xc3, 
+0x12, 0x2b, 0x73, 0xe1, 0xdb, 0x9d, 0xf4, 0xc3,
-0xc, 0xd6, 0x5, 0x1d, 0xac, 0xa4, 0xdc, 0x69, 
+0xc, 0xd6, 0x5, 0x1d, 0xac, 0xa4, 0xdc, 0x69,
-0x2a, 0xf5, 0x7c, 0x30, 0x24, 0x7b, 0x2b, 0x80, 
+0x2a, 0xf5, 0x7c, 0x30, 0x24, 0x7b, 0x2b, 0x80,
-0x1b, 0x7a, 0xec, 0x68, 0x77, 0x12, 0x97, 0x2a, 
+0x1b, 0x7a, 0xec, 0x68, 0x77, 0x12, 0x97, 0x2a,
-0xb2, 0xe5, 0xd2, 0xff, 0x41, 0x1d, 0xf5, 0x6, 
+0xb2, 0xe5, 0xd2, 0xff, 0x41, 0x1d, 0xf5, 0x6,
-0xa4, 0xb6, 0x47, 0xa5, 0xfa, 0x5, 0x7e, 0xf0, 
+0xa4, 0xb6, 0x47, 0xa5, 0xfa, 0x5, 0x7e, 0xf0,
-0xa1, 0xb8, 0xad, 0xde, 0x5c, 0xae, 0x2c, 0x8b, 
+0xa1, 0xb8, 0xad, 0xde, 0x5c, 0xae, 0x2c, 0x8b,
-0x4e, 0xad, 0xaf, 0xac, 0x6d, 0x46, 0x62, 0xe1, 
+0x4e, 0xad, 0xaf, 0xac, 0x6d, 0x46, 0x62, 0xe1,
-0xe9, 0xfe, 0xa1, 0xc5, 0xc9, 0xf4, 0x48, 0xd2, 
+0xe9, 0xfe, 0xa1, 0xc5, 0xc9, 0xf4, 0x48, 0xd2,
-0x80, 0x2b, 0xe5, 0xa9, 0x3e, 0xc1, 0xfc, 0x98, 
+0x80, 0x2b, 0xe5, 0xa9, 0x3e, 0xc1, 0xfc, 0x98,
-0xde, 0x3, 0xa9, 0xac, 0x86, 0xb7, 0xb5, 0x19, 
+0xde, 0x3, 0xa9, 0xac, 0x86, 0xb7, 0xb5, 0x19,
-0xe5, 0x75, 0x6a, 0x63, 0x83, 0x3f, 0x27, 0x7a, 
+0xe5, 0x75, 0x6a, 0x63, 0x83, 0x3f, 0x27, 0x7a,
-0x5e, 0xf8, 0xf1, 0x6d, 0xb2, 0xd2, 0x36, 0xa1, 
+0x5e, 0xf8, 0xf1, 0x6d, 0xb2, 0xd2, 0x36, 0xa1,
-0xc0, 0xd2, 0x5f, 0x93, 0x9c, 0x90, 0xe8, 0x3e, 
+0xc0, 0xd2, 0x5f, 0x93, 0x9c, 0x90, 0xe8, 0x3e,
-0xca, 0x89, 0xd7, 0xca, 0xd0, 0xc7, 0xe8, 0x8f, 
+0xca, 0x89, 0xd7, 0xca, 0xd0, 0xc7, 0xe8, 0x8f,
-0x4b, 0xb7, 0x60, 0x92, 0x5e, 0x36, 0x43, 0x70, 
+0x4b, 0xb7, 0x60, 0x92, 0x5e, 0x36, 0x43, 0x70,
-0x23, 0x36, 0xfb, 0xb5, 0xa5, 0x6, 0x9a, 0xc0, 
+0x23, 0x36, 0xfb, 0xb5, 0xa5, 0x6, 0x9a, 0xc0,
-0xcb, 0x2, 0x3, 0x1, 0x0, 0x1, 
+0xcb, 0x2, 0x3, 0x1, 0x0, 0x1,

boards/modalai/fc-v2/init/rc.board_defaults

@@ -2,4 +2,3 @@
 #
 # ModalAI FC-v2 specific board defaults
 #------------------------------------------------------------------------------
-

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()
-

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)
-

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
-

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)

platforms/nuttx/src/bootloader/stm/CMakeLists.txt

@@ -32,4 +32,3 @@
 ############################################################################
 
 add_subdirectory(${PX4_CHIP})
-

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
-

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
-

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
-

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
-

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
-

src/drivers/px4io/module.yaml

@@ -26,4 +26,3 @@ actuator_output:
             200: PWM200
             400: PWM400
         reboot_required: true
-

src/drivers/tap_esc/CMakeLists.txt

@@ -50,4 +50,3 @@ px4_add_module(
 		output_limit
 		tunes
 	)
-

src/drivers/tap_esc/module.yaml

@@ -4,4 +4,3 @@ actuator_output:
     - param_prefix: TAP_ESC
       channel_label: 'TAP ESC'
       num_channels: 8
-

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
-

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})
-
-
-

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)
-

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 };
 };
-

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))
-

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);
-

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;
-
-   

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];
+     2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2];

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))];

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
+end

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
+end

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
+end

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
+end

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
+end

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
+end

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

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
-

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

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
+end

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", 
+% "A new strapdown attitude algorithm",
-% R. B. MILLER, 
+% 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;
 

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

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');
+save(fileName,'output');

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);
 

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; 

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];

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.
+Output data is written to the Matlab workspace in the ‘output’ struct.

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;

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};

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)

src/modules/ekf2/EKF/python/ekf_derivation/__init__.py

@@ -5,4 +5,3 @@ Created on Sat Mar 14 13:02:26 2020
 
 @author: roman
 """
-

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 magN = 2.0f * ((float)rand() - 0.5f);
-	const float magE = 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 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
+    // 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 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];
+    float SH_MAG[9];
-	SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
+    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[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[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3;
-	SH_MAG[3] = sq(q3);
+    SH_MAG[3] = sq(q3);
-	SH_MAG[4] = sq(q2);
+    SH_MAG[4] = sq(q2);
-	SH_MAG[5] = sq(q1);
+    SH_MAG[5] = sq(q1);
-	SH_MAG[6] = sq(q0);
+    SH_MAG[6] = sq(q0);
-	SH_MAG[7] = 2.0f*magN*q0;
+    SH_MAG[7] = 2.0f*magN*q0;
-	SH_MAG[8] = 2.0f*magE*q3;
+    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
+        // 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));
+        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();

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));
-
-

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
-
-

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;
-
-

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
-
-

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;
-
-

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);
-
-

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));
-
-

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));
-
-

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));
-
-

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));
-
-

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
+    // get latest velocity in earth frame
-	const float vn = 9.0f;
+    const float vn = 9.0f;
-	const float ve = 12.0f;
+    const float ve = 12.0f;
-	const float vd = -1.5f;
+    const float vd = -1.5f;
 
-	// get latest wind velocity in earth frame
+    // get latest wind velocity in earth frame
-	const float vwn = -4.0f;
+    const float vwn = -4.0f;
-	const float vwe = 3.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) {
+    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);
+        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 {
+    } else {
-		printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction);
+        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) {
+    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);
+        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 {
+    } else {
-		printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction);
+        printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction);
-	}
+    }
 
     return 0;
 }

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;
-
-

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));
-
-

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));
-
-

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;
-
-

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;
-
-

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;
-
-

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")
-

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]))
+print('P[23][23] = ' + str(P_wind_earth_numeric[1,1]))

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);
-

src/systemcmds/sd_stress/CMakeLists.txt

@@ -38,4 +38,3 @@ px4_add_module(
 		sd_stress.c
 	DEPENDS
 	)
-