Dynamics/Estimation Exam Solution

Accelerometer readings. 20pts



We model a bicycle and its rider as a single 2D rigid body (we neglect the wheels dynamics). We introduce three coordinate systems:




* * *

An inertial frame xed on the road and denoted as [xI ; yI ; zI ];
* * *

A body- xed frame located at the center of mass and denoted as [xB; yB; zB].



Another body- xed frame located at the front wheel axle (point A) and with axes parallel
* * * * * *

to [xB; yB; zB] and denoted as [xS; yS; zS].




Assume you have two IMUs (each one composed of accelerometer and a gyro). The rst IMU is mounted at the center of mass (point CoM) and the second IMU is mounted the front wheel axle (point A) as shown in the next gure.























































Please answer the following questions.




(a) The rider does a wheelie with the bike and at some point in time it moves forward with

*

constant acceleration a of the CoM (with respect to the inertial frame) with direction shown

* *

in the gure below and magnitude a (i.e. a = axI ). What value does the accelerometer mounted in CoM measures?

*sen * *

Please answer the question in the form of aCoM = xB + zB.































































Available: April 5, 6:00PM Due: April 6, 11:59PM










Answer:



































































(b) At another point in time the rider starts bringing the front axle towards the ground. The

*sen
*
*






accelerometer in A measures aA
= axs + czs (where a and c are given constants) and the








*
*
gyro (mounted in A) measures a constant angular velocity wA=I
= ys. What is the bike
CoM acceleration?


*
*
*








Please answer the question in the form of aCoM
= xB + zB. Note I asked the \CoM
acceleration", not what the accelerometer mounted at the CoM measures.












































































* *

(NOTE: As discussed in class the angular velocity wA=I is the same as wCoM=I . Do not

*

confuse the angular velocity wA=I with the wheel angular speed. There are no wheels in this exercise and all IMUs are mounted on the bike frame.). Answer:



















3 of 15
Available: April 5, 6:00PM Due: April 6, 11:59PM












Quadcopter failure. 20pts






This problem uses the same notation, the same motor numbering and the same symbols of the document \DroneDynamics.pdf".




A hovering quadcopter is initially stationary (hovering at zero longitudinal and angular speeds and at a given constant height from the ground). Motor number 3 fails, and suddenly produces zero force/torque. The vehicle parameters are given below (in MKS)




m = 0:0680


d = 0:0624


b = 0:0107
(1)
k = 7:8264e 04


IB = diag([0:0686e
3; 0:092e 3; 0:1366e 3])



(a) Find the instantaneous acceleration of the quadcopter vx, vy, vz














































(b) Find the instantaneous angular acceleration of the quadcopter !x, !y, !z





























































































5 of 15
Available: April 5, 6:00PM Due: April 6, 11:59PM










Quadcopter Modeling. 10pts






Consider the drone model studied in class and represented in the next gure.




















































Explain why propellers 1 and 3 need to rotate in the opposite direction of propeller 2 and 4. Use the equations of motions derived in class to help your explanation.


































































































































6 of 15
Available: April 5, 6:00PM Due: April 6, 11:59PM










On our drone (and any quadrotor) we have two type of blades shown in the next gure. The design of blade B is symmetric to blade A. Explain why we need two di erent shapes. You























































Figure 1: Blade types.




can use sketches and equations to help your explanation.































































































































7 of 15
Available: April 5, 6:00PM Due: April 6, 11:59PM










Kalman ltering. 50pts






Concisely, the state equations for any time-invariant, linear estimator K which measures y and produces an estimate xestk, and has its own internal state z, are written as



zk+1
= AK zk + BK yk
(2)
xkest
=
CK zk + DK yk


where y is the measured output of a process model, driven by a random process w,


xk+1
=
Axk + Ewk
(3)
yk
=
Cxk + F wk





whose state, x is to be estimated. De ne the estimation error ek := xk xestk. Combine the time-invariant process model, (3), with the linear estimator (2) to obtain the equations which govern the relationship between w and e.

2 zk+1
3
= 2




3 2 zk
3
4
xk+1
5
4




xk
5
ek




5 4 wk
This is depicted in the gure below





































So, your task is to determine the entries of the matrix above (all marked with ). These will be expressions involving A; E; C; F; AK ; BK ; CK ; DK as well as identity and zero ma-trices. For later reference, notate this combined system (input w, output e) as G, or more speci cally G(A; E; C; F; AK ; BK ; CK ; DK ), illustrating its dependence on all of the matri-ces, comprising the Process and Estimator. (Note that this task is a simpler version of the problem you solved in Kalman Filtering (Part 3), problem #5). Remark: It is critical that you get this problem (and the code in the next part) correct. The computational exercises we carry out later rely on this code being correct.




Write a function, combineProcessEstimator.m, with function declaration-line begin code






function Gsys = combineProcessEstimator(A,E,C,F,AK,BK,CK,DK) end code






which builds this combined system derived in part (4a) (as a Matlab ss object). Make sure to set the sample-time to 1 (which means discrete-time, with unspeci ed sampled time).




An alternate (to Kalman Filtering) approach for state estimation, which will not be optimal (but has some other advantages) is mentioned early in the slides. Here we investigate this



8 of 15
Available: April 5, 6:00PM Due: April 6, 11:59PM










further. Matrices L0; L1; : : : ; LN are xed, and for each k, the state estimate of xk is simply a combination of the current measurement yk along with the previous N measurements, yk 1; yk 2; : : : ; yk N . Mathematically, this is




xest = L y
k
+ L
y
k 1
+ L
y
k 2
+:::+L
N
y
k N
(4)
k
0
1


2











Each Li 2 Rnx ny are chosen by some optimization criteria, once the process model, (3), is given.




Let K denote the system mapping input y to output xest through the formula




xestk = L0yk + L1yk 1 + L2yk 2 + : : : + LN yk N




A suitable state-space realization for K is





2 0ny
0ny
Iny
0ny
0ny
0ny
3






2 0ny
3






6
0ny
Iny
0ny
0ny


0ny
0ny
7






6
0ny
7




AK =
0
ny
0
ny
0
ny
0ny


0
ny
0ny
;
BK =
0
ny
(5)


6
0
ny
0
ny
0
ny
I
ny


0
ny


ny
7
6
0
ny
7




6








.


0
7






6


7








.


.


.


.


.


.








.






6


..


..


..


..
. .


..


..
7






6


..
7






6


























7






6




7






6
0
ny
0
ny
0
ny
0
ny


0
ny
I
ny
7






6
0
ny
7






6














7






6


7






6
0ny
0ny
0ny
0ny
0ny
0ny
7






6
Iny
7






6


7






6
7


and


4
























5






4




5




















































CK =
LN
LN 1
LN 2
LN 3






L2
L1
;
DK=L0
(6)
where the
dimensions of the matrices are










































































AK 2 Rny N ny N ;




BK 2 Rny N ny ;


CK 2 Rnx ny N ;


DK 2 Rnx ny





Task to complete in this part: Use the notation y for input to K and z for state of K, so that K is implemented as

zk+1
=
AK zk + BK yk
(7)
xkest
=
CK zk + DK yk


Show that for k N, the structure of AK and BK in (5) imply that the state zk is






2
ykyk(NN
1)
3


zk =
6
y
..




7
;


6
.




7




6






7






k
2






6
y
k
1


7




6




7




4








5


regardless of initial condition. In other words, the state zk merely holds the previous N values of the input.




Let Lmat denote the concatenation of the Li matrices, compatible with their appearance in CK and DK of the state-space realization of the estimator described in equation (6) in part (4c)



Lmat := LN LN 1 LN 2 LN 3 L2 L1 L0 2 Rnx (N+1)ny




Write a function, called createKfromLmat.m, with function declaration shown below




9 of 15
Available: April 5, 6:00PM Due: April 6, 11:59PM










begin code







function [AK,BK,CK,DK] = createKfromLmat(Lmat,nX,nY,N)



% verify that size of Lmat is [nX (N+1)*nY]



% extract CK matrix from Lmat



% extract DK matrix from Lmat



% create AK matrix using N, nY



6 % create BK matrix from N, nY




end code







The purpose is to create the linear, discrete-time state-space matrices for K, described in part (4c), equations (5) and (6), given the matrix Lmat.




(nothing to do - but read carefully) Recall from (KalmanFiltering (Part 3), problem 3) this fact about time-invariant, stable linear systems: Suppose G is a stable, linear, time-invariant linear system with input u and output y. Suppose the input is zero-mean, unit-intensity white noise, namely for all k and all j 6= k
Euk = 0; EukuTk = Inu ; EukuTj = 0nu ;




then

1=2

lim ykT yk = kGk2




k!1




This fact relates the frequency-response function of a stable system to its response to white-noise inputs.




Refer back to parts (4a) and (4d). In part (4a), we use the notation G(A; E; C; F; AK ; BK ; CK ; DK ) to denote the combined system for some arbitary estimator with state-space realization
(AK ; BK ; CK ; DK ). In part (4d), we develop the mapping from Lmat to the matrices AK ; BK ; CK ; DK for the speci c type of estimator given by (4). Let’s agree to modify the notation to represent this combined system as G(A; E; C; F; K(Lmat)), which accounts for the fact that the system K is determined from the elements in Lmat.




Since the 2-norm is mathematically equivalent to the steady-state output variance under the white-noise input, the estimator design can be based on minimization of the 2-norm, namely




min
k
G(A; E; C; F; K(L
))
k2
Lmat2Rnx ((N+1)ny
mat


Task: Write the following function which computes the k k2 cost associated with an esti-mator (speci ed by the Lmat argument) and a process model. The function declaration line, along with some comments that outline the functionality of the code is below.

begin code







function cost = evaluateCost(A,E,C,F,Lmat)



% compute nX from A



% compute nY from C



% compute N from size of Lmat



% use createKfromLmat to obtain matrices AK,BK,CK,DK



% use combineProcessEstimator to create G



7 % use norm to obtain cost.




end code







10 of 15
Available: April 5, 6:00PM Due: April 6, 11:59PM










Remark: The cost function,




f(Lmat) := kG(A; E; C; F; K(Lmat))k2




is a convex, di erntiable function of Lmat. This is great news for optimization...




Note: With this cost-function speci ed, Matlab’s unconstrained optimizer, fminunc, can be used to perform the optimization (and hence the design of the L matrices). A script- le to perform the design, assuming the process model is de ned, could be of the form below begin code







% variables that should exist:



%process model (A, E, C, F) consistent dimensions



%N, positive integer, dictates complexity of estimator



%nX and nY, consistent with (A,C)



%



% Create the function handle for the cost function. The



% decision variable is the matix Lmat



f = @(Lmat) evaluateCost(A,E,C,F,Lmat);



%



LmatInit = zeros(nX,(N+1)*nY);



% Call the solver, with the function handle for cost, and



% the initial value (just 0) for the decision variable matrix



[Lopt, optCost] = fminunc(f, LmatInit);



%



% Form the state-space model of the estimator



[AK,BK,CK,DK] = createKfromLmat(Lopt,nX,nY,N);



%



% Perform analysis on the estimator’s performance...



end code







Data for A; E; C; F is given in the le midtermModel1.mat. Use your code to solve for the estimator (4) which minimizes the k k2 norm from w to e. Do this for several val-ues of N, namely N = 1; 2; 3; 4; 5; 8; 12; 15. Plot the obtained norm, kGk2 versus N, Since we are minimizing, the norm should decrease with increasing N. Also use the code formSteadyStateKF.m to compute the steady-state Kalman lter, and speci cally the es-



timate x^kjk (please read the rst few items in Remarks/Hints at the end of this problem). Let GKF denote the combination of the process and Kalman lter, with input w and output ek := xk x^kjk. Compute kGKF k2 to compare with the results from the alternate form of the estimator. Use the covar command to compute the steady-state variance of the ek, both for the Kalman lter, and for the design with N = 15. Arrange your results neatly and concisely.




(nothing to do here - for your own use) In order to assist you in debugging, another example is provided in testCaseSingleProcessModel.mat The results (for the same tasks listed above in problem 4g) are shown below


















11 of 15
Available: April 5, 6:00PM Due: April 6, 11:59PM




































































































estErrVarWithL15 =




1.2679 0.6435 -0.1704




0.6435 0.3391 -0.0833




-0.1704 -0.0833 0.2384







estErrVarWithKF =




1.2676 0.6449 -0.1706




0.6449 0.3326 -0.0825




-0.1706 -0.0825 0.2383




Robust Estimation: An attractive feature of the optimization-based approach is the simple modi cation needed to handle uncertain process models. In the simplest case, suppose the process model is not a single model, but m separate models,






(A1; E1; C1; F1); (A2; E2; C2; F2); : : : ; (Am; Em; Cm; Fm)
(8)



Here we assume that the dimensions are consistent across these m models. The robust estimation problem is to chose a single estimator K(L) that works well for all the process models. One approach is to minimize the worst-cost associated with




min
max
k
G(A
; E
; C
;F ;K(L


))
k2
Lmat2Rnx ((N+1)ny
1 i M
i


i




i
i
mat
























|worst
case cost
across all process models






{z






}
Write a command called evaluateWorstCostManyModels.m, with function declaration line




12 of 15
Available: April 5, 6:00PM Due: April 6, 11:59PM










begin code







function cost = evaluateWorstCostManyModels(A,E,C,F,Lmat) end code






where input arguments A; E; C; F are 3-dimensional arrays, of dimension nx nx m, nx nw m, ny nx m, ny nw m respectively, corresponding to the multple process models in equation (8). The argument Lmat corresponds to the estimator parameters, as before. The output argument cost is the worst-case cost associated with the estimator, namely

max kG(Ai; Ei; Ci; Fi; K(Lmat))k2

1 i m




Again, this cost function is a convex function of Lmat, but unfortunately, because of the max structure, it is not di erentiable. Please read the Remarks/Hints for a bit more information (not necessary for the exam though).




There is a data le, midtermMultipleModels.mat for a robust design, with 11 models (m = 11, represented with a variable nModels in the le). The (A; E; C; F ) matrices are



3-dimensional, so A(:,:,i) is Ai, and i runs from 1 to 11. Careful examination reveals that the model is derived from the single-model example, midtermModel1.mat, with one




key di erence. The (1; 2) element of A varies from 1 to 0, across the 11 models, whereas




in part (4g) it was constant, with value equal to 0:5. In this example, the other matrices,




E; C; F do not change along their 3rd dimension. Task:




Carry out a robust design, using the data in midtermMultipleModels.mat, the cost function in evaluateWorstCostManyModels.m, and an appropriate call to fminunc. Do this for N = 1; 2; 3; 4; 5; 8; 12, recording the optimized worst-case cost for each design.




Analyze more carefully the robustness of the estimator designed with N = 12. Read the Hints/Remarks section about assessing various combinations of estimators and process models (the and arrays).




Make some appropriate plots, a few comments, and document your script le.




In order to check your work along the way, a relevant plot (I am not providing much information, but once you dig into this problem, the plot below might serve as a useful comparison) is shown below.





























































13 of 15
Available: April 5, 6:00PM Due: April 6, 11:59PM




































































































Remarks/Hints:




When you use formSteadyStateKF.m to obtain Kalman lter for any given process model, be careful about the outputs. We are interested in comparing x^kjk to the result of the estimator in (4), since that estimator does use yk in the estimate xestk. Be sure to use to correct outputs from the system returned by formSteadyStateKF.m. Remember that in addition to x^kjk, the outputs include x^kjk 1 and w^kjk which we are not interested in.




The 5th input argument to formSteadyStateKF.m is the variance of wk, assumed to be constant for all k. Since the k k2 norm calculation relates to the output variance when the input is zero-mean, unit-intensity white noise, use Inw for the variance of wk.




In the robust problem, there are many possible design/calculations and comparisons to do...




For each choice of N, one robust estimator design. Call this design Krob;N
One Kalman lter design at each process model (Ai; Ei; Ci; Fi). Use formSteadyStateKF.m and call this design KFi .



The only performance metric is kGk2, where G is the combination of one of the process models and one of the estimators. We are interested in 2 speci c combinations



{ i, for the robust estimator, Krob;N combined with the i’th process model, (Ai; Ei; Ci; Fi) { i;j, for the i’th process model (Ai; Ei; Ci; Fi), combined with the j’th Kalman

Filter, KFj.




By plotting i;i versus i, we see what is achievable at each xed i, which acts as sort of baseline performance. By plotting i;j versus i (for xed j) we see how the j’th Kalman lter performs across the entire set of process models (perhaps one of these Kalman Filters does well across the entire set of process model, or maybe not - perhaps the only way to achieve good robustness is to use the tools developed in part 4i). Finally,







14 of 15
Available: April 5, 6:00PM Due: April 6, 11:59PM










by plotting i versus i, we see how the Robust lter performs across the entire set of process models. In terms of worst-case performance, this should be better than any one of the individual Kalman lter designs.... Is it?




Important Remark for later use (nothing to do on exam): The cost functions we have de ned, for the single process model, and for the multiple-model case, are both convex functions of the decision-variable Lmat. Unfortunately, in the multiple-model case, the cost-function (since it involves a max) is not di erentiable. This nondi erentiabilty can cause general-purpose solvers, such as fminunc to prematurely stop. There are other more advanced solvers that can handle the non-di erentiability, but that is not the focus of the problem. However, if you nd yourself faced with a robust-estimator design problem, you may have to use alternative optimization methods to obtain a good solution with the approach ((4)) presented here.























































































































































15 of 15