dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Whitespace cleanup.

This commit is contained in:
mcsauder 2021-10-01 11:06:15 -06:00 committed by Daniel Agar
parent 4cf8eb8226
commit 21163d859e
76 changed files with 300 additions and 367 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider
View File

@ -7,4 +7,3 @@
param set-default FW_THR_CRUISE 0.0
param set-default RWTO_TKOFF 0

1
ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc
View File

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

1
ROMFS/px4fmu_common/init.d/airframes/4072_draco
View File

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

1
Tools/compress.py
View File

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

1
Tools/module_config/generate_params.py
View File

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

1
Tools/module_config/output_groups_from_timer_config.py
View File

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

2
Tools/px4airframes/markdownout.py
View File

@ -25,7 +25,7 @@ div.frame_common table, div.frame_common table {
}
div.frame_common table {
float: right;
float: right;
width: 70%;
}

1
Tools/px4events/jsonout.py
View File

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

8
Tools/test_keys/key0.pub
View File

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

74
Tools/test_keys/rsa2048.pub
View File

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

1
boards/modalai/fc-v2/init/rc.board_defaults
View File

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

1
cmake/px4_list_make_absolute.cmake
View File

@ -55,4 +55,3 @@ function(px4_list_make_absolute var prefix)
endforeach(f)
set(${var} "${list_var}" PARENT_SCOPE)
endfunction()

1
msg/actuator_motors.msg
View File

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

1
msg/actuator_servos.msg
View File

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

25
msg/tools/generate_msg_docs.py
View File

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

1
platforms/nuttx/src/bootloader/stm/CMakeLists.txt
View File

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

1
src/drivers/barometer/CMakeLists.txt
View File

@ -41,4 +41,3 @@ add_subdirectory(lps33hw)
add_subdirectory(maiertek)
add_subdirectory(ms5611)
#add_subdirectory(tcbp001ta) # only for users who really need this

1
src/drivers/linux_pwm_out/module.yaml
View File

@ -9,4 +9,3 @@ actuator_output:
max: { min: 1600, max: 2200, default: 2000 }
failsafe: { min: 800, max: 2200 }
num_channels: 8

1
src/drivers/pca9685_pwm_out/module.yaml
View File

@ -9,4 +9,3 @@ actuator_output:
max: { min: 1600, max: 2200, default: 2000 }
failsafe: { min: 800, max: 2200 }
num_channels: 16

3
src/drivers/pwm_out/module.yaml
View File

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

1
src/drivers/pwm_out_sim/module.yaml
View File

@ -5,4 +5,3 @@ actuator_output:
- param_prefix: ${PWM_MAIN_OR_HIL}
channel_label: ${PWM_MAIN_OR_HIL}
num_channels: 16

1
src/drivers/px4io/module.yaml
View File

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

1
src/drivers/tap_esc/CMakeLists.txt
View File

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

1
src/drivers/tap_esc/module.yaml
View File

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

1
src/drivers/uavcan/module.yaml
View File

@ -17,4 +17,3 @@ actuator_output:
max: { min: 0, max: 1000, default: 1000 }
failsafe: { min: 0, max: 1000 }
num_channels: 8

3
src/lib/component_information/CMakeLists.txt
View File

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

1
src/lib/events/CMakeLists.txt
View File

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

1
src/lib/mixer_module/functions.hpp
View File

@ -240,4 +240,3 @@ private:
uORB::Subscription _topic{ORB_ID(actuator_controls_2)};
float _data[3] { NAN, NAN, NAN };
};

1
src/lib/mixer_module/generate_function_header.py
View File

@ -72,4 +72,3 @@ enum class OutputFunction : int32_t {{
{2}
}};
'''.format(os.path.basename(__file__), yaml_file, function_defs))

1
src/lib/systemlib/system_params.c
View File

@ -281,4 +281,3 @@ PARAM_DEFINE_INT32(SYS_FAILURE_EN, 0);
* @group System
*/
PARAM_DEFINE_INT32(SYS_CTRL_ALLOC, 0);

2
src/modules/ekf2/EKF/matlab/EKF_replay/Common/EulToQuat.m
View File

@ -19,5 +19,3 @@ quaterion(3,1) = (cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi);
quaterion(4,1) = (cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi);
return;

4
src/modules/ekf2/EKF/matlab/EKF_replay/Common/Quat2Tbn.m
View File

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

2
src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatMult.m
View File

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

4
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/AlignHeading.m
View File

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

16
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBodyVel.m
View File

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

16
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m
View File

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

16
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m
View File

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

28
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FusePosition.m
View File

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

28
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseVelocity.m
View File

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

4
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/GenerateEquations24.m
View File

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

1
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitCovariance.m
View File

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

65
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PlotData.m
View File

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

6
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictCovariance.m
View File

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

6
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictStates.m
View File

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

94
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/RunFilter.m
View File

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

6
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_apm_data.m
View File

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

30
src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m
View File

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

58
src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m
View File

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

2
src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m
View File

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

4
src/modules/ekf2/EKF/matlab/EKF_replay/readme.txt
View File

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

10
src/modules/ekf2/EKF/matlab/analysis/estimatorLogViewerPX4.m
View File

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

6
src/modules/ekf2/EKF/matlab/analysis/importPX4log.m
View File

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

4
src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m
View File

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

1
src/modules/ekf2/EKF/python/ekf_derivation/__init__.py
View File

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

38
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp
View File

@ -198,5 +198,3 @@ Kfusion(23) = -HK79*HK84;
// Kalman gains - axis 2

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp
View File

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

38
src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp
View File

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

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp
View File

@ -148,5 +148,3 @@ H_YAW.at<20>() = 0;
H_YAW.at<21>() = 0;
H_YAW.at<22>() = 0;
H_YAW.at<23>() = 0;

1
src/modules/ekf2/EKF/python/terrain_flow_derivation/derive_terrain_flow.py
View File

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

4
src/modules/ekf2/EKF/python/wind_cov_init/derivation.py
View File

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

1
src/modules/rc_update/params_deprecated.c
View File

@ -203,4 +203,3 @@ PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0);
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);

1
src/systemcmds/sd_stress/CMakeLists.txt
View File

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