Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 02:17:07 +08:00
Code Issues Actions 33 Packages Projects Releases Wiki Activity

update attitude estimator ekf to latest version

Browse Source 
mainly saves stack size
This commit is contained in:
Thomas Gubler
2014-08-21 11:25:09 +02:00
parent 507450c707
commit 58aabe648a
7 changed files with 300 additions and 406 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/AttitudeEKF.m
+37 -25
View File
@@ -166,15 +166,16 @@ P_apr=A_lin*P_apo*A_lin'+Q;
%% update
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
R=[r_gyro,0,0,0,0,0,0,0,0;
0,r_gyro,0,0,0,0,0,0,0;
0,0,r_gyro,0,0,0,0,0,0;
0,0,0,r_accel,0,0,0,0,0;
0,0,0,0,r_accel,0,0,0,0;
0,0,0,0,0,r_accel,0,0,0;
0,0,0,0,0,0,r_mag,0,0;
0,0,0,0,0,0,0,r_mag,0;
0,0,0,0,0,0,0,0,r_mag];
% R=[r_gyro,0,0,0,0,0,0,0,0;
% 0,r_gyro,0,0,0,0,0,0,0;
% 0,0,r_gyro,0,0,0,0,0,0;
% 0,0,0,r_accel,0,0,0,0,0;
% 0,0,0,0,r_accel,0,0,0,0;
% 0,0,0,0,0,r_accel,0,0,0;
% 0,0,0,0,0,0,r_mag,0,0;
% 0,0,0,0,0,0,0,r_mag,0;
% 0,0,0,0,0,0,0,0,r_mag];
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag];
%observation matrix
%[zw;ze;zmk];
H_k=[ E, Z, Z, Z;
@@ -184,7 +185,9 @@ if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
y_k=z(1:9)-H_k*x_apr;
S_k=H_k*P_apr*H_k'+R;
%S_k=H_k*P_apr*H_k'+R;
S_k=H_k*P_apr*H_k';
S_k(1:9+1:end) = S_k(1:9+1:end) + R_v;
K_k=(P_apr*H_k'/(S_k));
@@ -196,13 +199,16 @@ else
R=[r_gyro,0,0;
0,r_gyro,0;
0,0,r_gyro];
R_v=[r_gyro,r_gyro,r_gyro];
%observation matrix
H_k=[ E, Z, Z, Z];
y_k=z(1:3)-H_k(1:3,1:12)*x_apr;
S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
% S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)';
S_k(1:3+1:end) = S_k(1:3+1:end) + R_v;
K_k=(P_apr*H_k(1:3,1:12)'/(S_k));
@@ -211,13 +217,14 @@ else
else
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0
R=[r_gyro,0,0,0,0,0;
0,r_gyro,0,0,0,0;
0,0,r_gyro,0,0,0;
0,0,0,r_accel,0,0;
0,0,0,0,r_accel,0;
0,0,0,0,0,r_accel];
% R=[r_gyro,0,0,0,0,0;
% 0,r_gyro,0,0,0,0;
% 0,0,r_gyro,0,0,0;
% 0,0,0,r_accel,0,0;
% 0,0,0,0,r_accel,0;
% 0,0,0,0,0,r_accel];
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel];
%observation matrix
H_k=[ E, Z, Z, Z;
@@ -225,7 +232,9 @@ else
y_k=z(1:6)-H_k(1:6,1:12)*x_apr;
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
% S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
@@ -233,12 +242,13 @@ else
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
else
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1
R=[r_gyro,0,0,0,0,0;
0,r_gyro,0,0,0,0;
0,0,r_gyro,0,0,0;
0,0,0,r_mag,0,0;
0,0,0,0,r_mag,0;
0,0,0,0,0,r_mag];
% R=[r_gyro,0,0,0,0,0;
% 0,r_gyro,0,0,0,0;
% 0,0,r_gyro,0,0,0;
% 0,0,0,r_mag,0,0;
% 0,0,0,0,r_mag,0;
% 0,0,0,0,0,r_mag];
R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag];
%observation matrix
H_k=[ E, Z, Z, Z;
@@ -246,7 +256,9 @@ else
y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr;
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
%S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj
+3 -4
View File
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<deployment-project plugin="plugin.matlabcoder" plugin-version="R2013a">
<configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="2197173368">
<configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="1213478164">
<profile key="profile.mex">
<param.MergeInstrumentationResults>false</param.MergeInstrumentationResults>
<param.BuiltInstrumentedMex>false</param.BuiltInstrumentedMex>
@@ -112,7 +112,7 @@
<param.EnableVariableSizing>false</param.EnableVariableSizing>
<param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
<param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
<param.StackUsageMax>200000</param.StackUsageMax>
<param.StackUsageMax>4000</param.StackUsageMax>
<param.MultiInstanceCode>false</param.MultiInstanceCode>
<param.FilePartitionMethod>option.FilePartitionMethod.SingleFile</param.FilePartitionMethod>
<param.GenerateComments>true</param.GenerateComments>
@@ -215,7 +215,6 @@
<param.PurelyIntegerCode />
<param.DynamicMemoryAllocation />
<param.DynamicMemoryAllocationThreshold />
<param.StackUsageMax />
<param.MultiInstanceCode />
<param.GenerateComments />
<param.MATLABFcnDesc />
@@ -492,7 +491,7 @@
<vista>false</vista>
<linux>true</linux>
<solaris>false</solaris>
<osver>3.15.5-1-ARCH</osver>
<osver>3.16.1-1-ARCH</osver>
<os32>false</os32>
<os64>true</os64>
<arch>glnxa64</arch>
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+1 -1
View File
@@ -132,7 +132,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
14000,
7200,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c
+256 -373
View File
File diff suppressed because it is too large Load Diff
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Fri Jul 25 14:06:41 2014
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Fri Jul 25 14:06:41 2014
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Fri Jul 25 14:06:41 2014
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/