Skip to content

Commit 5383275

Browse files
fibrfibr
committed
- more attitude bugfixes
- attitude estimation test scenario - controller flying in closed loop with estimator data
1 parent 291d516 commit 5383275

10 files changed

+160
-51
lines changed

‎config/QuadControlParams_Estimator.txt

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
############################## SLR SIMPLECONFIG ############################
2+
# this is a comment. [X] is a namespace. [X:Y] initializes X from Y
3+
# Namespace and parameter names are not case-sensitive
4+
# X=Y sets X to Y. Y may be a string, float, or list of 3 floats
5+
############################################################################
6+
7+
[QuadControlParams]
8+
9+
UseIdealEstimator=1
10+
11+
# Physical properties
12+
Mass = 0.5
13+
L = 0.17
14+
Ixx = 0.0023
15+
Iyy = 0.0023
16+
Izz = 0.0046
17+
kappa = 0.016
18+
minMotorThrust = .1
19+
maxMotorThrust = 4.5
20+
21+
# Position control gains
22+
kpPosXY = 2
23+
kpPosZ = 2
24+
KiPosZ = 20
25+
26+
# Velocity control gains
27+
kpVelXY = 8
28+
kpVelZ = 8
29+
30+
# Angle control gains
31+
kpBank = 16
32+
kpYaw = 1
33+
34+
# Angle rate gains
35+
kpPQR = 70, 70, 15
36+
37+
# limits
38+
maxAscentRate = 5
39+
maxDescentRate = 2
40+
maxSpeedXY = 5
41+
maxHorizAccel = 12
42+
maxTiltAngle = .7

‎config/QuadEstimatorEKF.txt

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,13 @@
11
[QuadEstimatorEKF]
2-
InitState = 0, 0, 0, 0, 0, 0, 0
3-
InitStdDevs = 1, 1, 1, 1, 1, 1, 1
2+
InitState = 0, 0, -1, 0, 0, 0, 0
3+
InitStdDevs = .1, .1, .3, .1, .1, .3, .05
44

55
# Process noise model
6-
QPosXYStd = .01
7-
QPosZStd = .01
8-
QVelXYStd = .01
9-
QVelZStd = .01
10-
QYawStd = .01
6+
QPosXYStd = .003
7+
QPosZStd = .003
8+
QVelXYStd = .003
9+
QVelZStd = .009
10+
QYawStd = .001
1111

1212
# GPS measurement std deviations
1313
GPSPosXYStd = 1
@@ -19,5 +19,5 @@ GPSVelZStd = .3
1919
MagYawStd = .1
2020

2121
dtIMU = 0.002
22-
attitudeTau = 3
22+
attitudeTau = 5
2323

‎config/SimulatedSensors.txt

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,8 @@
11
# note that sensors can not be faster than the controller rate (0.002)
22

33
[SimIMU]
4-
AccelStd = 1, 1, 3
5-
GyroStd = 1, 1, 1
4+
#AccelStd = 1, 1, 3
5+
#GyroStd = .5, .5, .5
66
dt = .002
77

88
[SimBaro]
@@ -15,6 +15,6 @@ dt = .01
1515

1616
[SimGPS]
1717
PosStd = 1, 1, 3
18-
PosRandomWalkStd = 0, 0, 0
18+
PosRandomWalkStd = .1, .1, .1
1919
VelStd = .1, .1, .3
2020
dt = .1

‎config/X_DebugAttitudeInterp.txt

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
# Hover at the initial point using full 3D control
2+
3+
INCLUDE QuadPhysicalParams.txt
4+
5+
# simulation setup
6+
Sim.RunMode = Repeat
7+
Sim.EndTime = 5
8+
Sim.Vehicle1 = Quad1
9+
10+
# Controller selection
11+
Quad.ControlType = QuadControl
12+
Quad.ControlConfig = QuadControlParams
13+
14+
# reference trajectory
15+
QuadControlParams.Trajectory=traj/Turn.txt
16+
17+
# graphing commands
18+
Commands += AddGraph2.Quad1.Est.S.Yaw
19+
Commands += AddGraph2.Quad1.Est.Yaw
20+
Commands += AddGraph2.Quad1.Yaw
21+
Commands += AddGraph2.Quad1.Ref.Yaw
22+
23+
INCLUDE QuadControlParams.txt
24+
INCLUDE Simulation.txt
25+
26+
# USE ACTUAL ESTIMATOR!
27+
INCLUDE SimulatedSensors.txt
28+
[QuadControlParams]
29+
UseIdealEstimator=1
30+
INCLUDE QuadEstimatorEKF.txt
31+
32+
# relax control so we can move around the vehicle
33+
[QuadControlParams]
34+
kpPosXY = .5
35+
36+
# Vehicle-specific config
37+
[Quad1:Quad]
38+
InitialPos=0,3,-1

‎config/X_EstimatorAtt.txt

Lines changed: 15 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@ INCLUDE QuadPhysicalParams.txt
22

33
# simulation setup
44
Sim.RunMode = Repeat
5-
Sim.EndTime = 10
5+
Sim.EndTime = 5
66
Sim.Vehicle1 = Quad
77

88
# Controller selection
@@ -15,15 +15,23 @@ INCLUDE SimulatedSensors.txt
1515

1616
# Initial state & reference trajectory
1717
Quad.InitialPos=0,0,-1
18-
QuadControlParams.Trajectory=traj/CircleNoFF.txt
18+
QuadControlParams.Trajectory=traj/AttDebug.txt
1919

2020
# graphing commands
21-
Commands += AddGraph1.Quad.Est.D.AccelRoll
22-
Commands += AddGraph1.Quad.Roll
23-
Commands += AddGraph1.Quad.Est.Roll
24-
Commands += AddGraph2.Quad.Est.D.AccelPitch
21+
22+
Commands += AddGraph1.Quad.Est.S.Yaw
23+
Commands += AddGraph1.Quad.Est.E.Yaw
24+
Commands += AddGraph1.Quad.Est.E.Roll
25+
Commands += AddGraph1.Quad.Est.E.Pitch
26+
27+
#Commands += AddGraph2.Quad.Est.D.AccelRoll
28+
Commands += AddGraph2.Quad.Roll
29+
Commands += AddGraph2.Quad.Est.Roll
30+
#Commands += AddGraph2.Quad.Est.D.AccelPitch
2531
Commands += AddGraph2.Quad.Pitch
2632
Commands += AddGraph2.Quad.Est.Pitch
33+
Commands += AddGraph2.Quad.Yaw
34+
Commands += AddGraph2.Quad.Est.Yaw
2735

2836
INCLUDE QuadControlParams.txt
2937
INCLUDE Simulation.txt
@@ -33,4 +41,4 @@ INCLUDE QuadEstimatorEKF.txt
3341

3442
[]
3543
# to use estimator to fly the vehicle (not the actual state), uncomment next line
36-
#QuadControlParams.UseIdealEstimator = 0
44+
QuadControlParams.UseIdealEstimator = 1

‎config/X_EstimatorDebug.txt

Lines changed: 10 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -18,28 +18,27 @@ QuadControlParams.Trajectory=traj/Square.txt
1818
#Commands += AddGraph1.Quad1.Est.D.ax_g
1919
#Commands += AddGraph1.Quad1.IMU.AX
2020
#Commands += AddGraph1.Quad1.Acc.X
21+
Commands += Toggle.ActualTrajectory
22+
2123
Commands += AddGraph1.Quad1.Pos.Y
2224
Commands += AddGraph1.Quad1.Vel.y
2325
Commands += AddGraph1.Quad1.Est.Y
2426
Commands += AddGraph1.Quad1.Est.VY
25-
Commands += AddGraph2.Quad1.Est.S.Yaw
26-
Commands += AddGraph2.Quad1.Est.Yaw
27-
Commands += AddGraph2.Quad1.Yaw
28-
Commands += AddGraph2.Quad1.Ref.Yaw
2927

30-
INCLUDE QuadControlParams.txt
28+
Commands += AddGraph2.Quad1.Pos.Z
29+
Commands += AddGraph2.Quad1.Vel.Z
30+
Commands += AddGraph2.Quad1.Est.Z
31+
Commands += AddGraph2.Quad1.Est.VZ
32+
33+
INCLUDE QuadControlParams_Estimator.txt
3134
INCLUDE Simulation.txt
3235

3336
# USE ACTUAL ESTIMATOR!
3437
INCLUDE SimulatedSensors.txt
3538
[QuadControlParams]
36-
UseIdealEstimator=1
39+
UseIdealEstimator=0
3740
INCLUDE QuadEstimatorEKF.txt
3841

39-
# relax control so we can move around the vehicle
40-
[QuadControlParams]
41-
kpPosXY = .5
42-
4342
# Vehicle-specific config
4443
[Quad1:Quad]
45-
InitialPos=0,3,-1
44+
InitialPos=0,0,-1

‎config/X_Scenarios.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
X_EstimatorDebug
2+
X_DebugAttitudeInterp
23
X_EstimatorAtt
34
X_TestMavlink
45
X_TestManyQuads

‎config/traj/AttDebug.txt

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
# time, x, y, z, vx, vy, vz, yaw
2+
0.0, 0, 0, -1, 0, 0, 0, 0
3+
4+
0.1, 1, 0, -1, 0, 0, 0, 0
5+
6+
0.2, 0, 0, -1, 0, 0, 0, 0
7+
1.2, 0, 0, -1, 0, 0, 0, 0
8+
9+
1.3, 0, -1, -1, 0, 0, 0, 0
10+
11+
1.4, 0, 0, -1, 0, 0, 0, 0
12+
2.4, 0, 0, -1, 0, 0, 0, 0
13+
14+
2.6, 0, 0, -1, 0, 0, 0, 1.57
15+
3.0, 0, 0, -1, 0, 0, 0, 3.14
16+
3.4, 0, 0, -1, 0, 0, 0, -1.57
17+
3.8, 0, 0, -1, 0, 0, 0, 0

‎config/traj/Square.txt

Lines changed: 20 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -1,24 +1,22 @@
1-
#init
2-
0,0,3,-1,0,0,0,0
1+
# time, x, y, z, vx, vy, vz, yaw
2+
1.0, 0, 0, -1, 0, 0, 0, 0
3+
2.0, 3, 0, -1, 0, 0, 0, 0
34

4-
#translate (2s)
5-
1,0,3,-1,0,0,0,0
6-
3,0,-3,-1,0,0,0,0
5+
3.0, 3, 0, -1, 0, 0, 0, 1.57
6+
4.0, 3, 3, -1, 0, 0, 0, 1.57
7+
5.0, 3, 3, -1, 0, 0, 0, 3.14
8+
6.0, -3, 3, -1, 0, 0, 0, 3.14
9+
7.0, -3, 3, -1, 0, 0, 0, -1.57
10+
8.0, -3, -3, -1, 0, 0, 0, -1.57
11+
9.0, -3, -3, -1, 0, 0, 0, 0
12+
10.0, 3, -3, -1, 0, 0, 0, 0
13+
11.0, 3, -3, -1, 0, 0, 0, 1.57
714

8-
#rotate (1s)
9-
4,0,-3,-1,0,0,0,1.57
10-
11-
# translate (2s)
12-
6,-3,-3,-1,0,0,0,1.57
13-
14-
#rotate (1s)
15-
7,-3,-3,-1,0,0,0,3.14
16-
17-
# translate (2s)
18-
9,-3,3,-1,0,0,0,3.14
19-
20-
#rotate (1s)
21-
10,-3,3,-1,0,0,0,-1.57
22-
23-
# translate (2s)
24-
12,3,3,-1,0,0,0,-1.57
15+
12.0, 3, 3, -1, 0, 0, 0, 1.57
16+
13.0, 3, 3, -1, 0, 0, 0, 3.14
17+
14.0, -3, 3, -1, 0, 0, 0, 3.14
18+
15.0, -3, 3, -1, 0, 0, 0, -1.57
19+
16.0, -3, -3, -1, 0, 0, 0, -1.57
20+
17.0, -3, -3, -1, 0, 0, 0, 0
21+
18.0, 3, -3, -1, 0, 0, 0, 0
22+
19.0, 3, -3, -1, 0, 0, 0, 1.57

‎config/traj/Turn.txt

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
# time, x, y, z, vx, vy, vz, yaw
2+
0.0, 0, 0, -1, 0, 0, 0, 0
3+
0.5, 0, 0, -1, 0, 0, 0, 1.57
4+
1.0, 0, 0, -1, 0, 0, 0, 3.14
5+
1.5, 0, 0, -1, 0, 0, 0, -1.57
6+
2.0, 0, 0, -1, 0, 0, 0, 0

0 commit comments

Comments
 (0)