fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 20:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Matlab: Improve efficiency of odometry derivation

Browse Source
This commit is contained in:
Paul Riseborough
2017-06-03 12:39:24 +10:00
parent 510b8763ea
commit 9f61622044
1 changed files with 15 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
+15 -1
View File
@@ -76,7 +76,6 @@ dVelBias = [dvx_b; dvy_b; dvz_b];
% define the quaternion rotation vector for the state estimate
quat = [q0;q1;q2;q3];
% derive the truth body to nav direction cosine matrix
Tbn = Quat2Tbn(quat);
@@ -377,6 +376,21 @@ fix_c_code('K_VELZ.c');
clear all;
reset(symengine);
% calculate Kalman gains vectors for X,Y,Z to take advantage of common
% terms
load('StatePrediction.mat');
load('temp1.mat');
load('temp2.mat');
load('temp3.mat');
load('temp4.mat');
K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector
K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector
K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector
K_VEL = simplify([K_VELX,K_VELY,K_VELZ]);
ccode(K_VEL,'file','K_VEL.c');
fix_c_code('K_VEL.c');
%% derive equations for fusion of 321 sequence yaw measurement
load('StatePrediction.mat');