mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 21:55:34 +08:00
Whitespace cleanup.
This commit is contained in:
mcsauder
Daniel Agar
@@ -7,4 +7,3 @@
|
||||
|
||||
param set-default FW_THR_CRUISE 0.0
|
||||
param set-default RWTO_TKOFF 0
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -17,4 +17,3 @@ def save_compressed(filename):
|
||||
f.write(content_file.read())
|
||||
|
||||
save_compressed(filename)
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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))
|
||||
|
||||
|
||||
@@ -25,7 +25,7 @@ div.frame_common table, div.frame_common table {
|
||||
}
|
||||
|
||||
div.frame_common table {
|
||||
float: right;
|
||||
float: right;
|
||||
width: 70%;
|
||||
}
|
||||
|
||||
|
||||
@@ -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))
|
||||
|
||||
|
||||
@@ -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,
|
||||
|
||||
+37
-37
@@ -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,
|
||||
|
||||
@@ -2,4 +2,3 @@
|
||||
#
|
||||
# ModalAI FC-v2 specific board defaults
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
|
||||
@@ -55,4 +55,3 @@ function(px4_list_make_absolute var prefix)
|
||||
endforeach(f)
|
||||
set(${var} "${list_var}" PARENT_SCOPE)
|
||||
endfunction()
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -32,4 +32,3 @@
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(${PX4_CHIP})
|
||||
|
||||
|
||||
@@ -41,4 +41,3 @@ add_subdirectory(lps33hw)
|
||||
add_subdirectory(maiertek)
|
||||
add_subdirectory(ms5611)
|
||||
#add_subdirectory(tcbp001ta) # only for users who really need this
|
||||
|
||||
|
||||
@@ -9,4 +9,3 @@ actuator_output:
|
||||
max: { min: 1600, max: 2200, default: 2000 }
|
||||
failsafe: { min: 800, max: 2200 }
|
||||
num_channels: 8
|
||||
|
||||
|
||||
@@ -9,4 +9,3 @@ actuator_output:
|
||||
max: { min: 1600, max: 2200, default: 2000 }
|
||||
failsafe: { min: 800, max: 2200 }
|
||||
num_channels: 16
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -5,4 +5,3 @@ actuator_output:
|
||||
- param_prefix: ${PWM_MAIN_OR_HIL}
|
||||
channel_label: ${PWM_MAIN_OR_HIL}
|
||||
num_channels: 16
|
||||
|
||||
|
||||
@@ -26,4 +26,3 @@ actuator_output:
|
||||
200: PWM200
|
||||
400: PWM400
|
||||
reboot_required: true
|
||||
|
||||
|
||||
@@ -50,4 +50,3 @@ px4_add_module(
|
||||
output_limit
|
||||
tunes
|
||||
)
|
||||
|
||||
|
||||
@@ -4,4 +4,3 @@ actuator_output:
|
||||
- param_prefix: TAP_ESC
|
||||
channel_label: 'TAP ESC'
|
||||
num_channels: 8
|
||||
|
||||
|
||||
@@ -17,4 +17,3 @@ actuator_output:
|
||||
max: { min: 0, max: 1000, default: 1000 }
|
||||
failsafe: { min: 0, max: 1000 }
|
||||
num_channels: 8
|
||||
|
||||
|
||||
@@ -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})
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -240,4 +240,3 @@ private:
|
||||
uORB::Subscription _topic{ORB_ID(actuator_controls_2)};
|
||||
float _data[3] { NAN, NAN, NAN };
|
||||
};
|
||||
|
||||
|
||||
@@ -72,4 +72,3 @@ enum class OutputFunction : int32_t {{
|
||||
{2}
|
||||
}};
|
||||
'''.format(os.path.basename(__file__), yaml_file, function_defs))
|
||||
|
||||
|
||||
@@ -281,4 +281,3 @@ PARAM_DEFINE_INT32(SYS_FAILURE_EN, 0);
|
||||
* @group System
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SYS_CTRL_ALLOC, 0);
|
||||
|
||||
|
||||
@@ -19,5 +19,3 @@ quaterion(3,1) = (cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi);
|
||||
quaterion(4,1) = (cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi);
|
||||
|
||||
return;
|
||||
|
||||
|
||||
|
||||
@@ -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];
|
||||
|
||||
@@ -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))];
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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');
|
||||
|
||||
+15
-15
@@ -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);
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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];
|
||||
|
||||
@@ -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.
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user