OPTIMAL STATE ESTIMATION Kalman, Hoo, and Nonlinear Approaches Solution Manual Dan Simon Cleveland State University ‘A JOHN WILEY & SONS, INC., PUBLICATIONCopyright ©2006 by Job Wiley ke Sons Ine. All rghte reserved. Published by dobn Wiley & Sons, ney Hoboken, New Jersey Pubsahed simaltencomal se Canada Ko part of thi pulation may be eptoducnd, stored Ip 4 nesieval water, or tananstted in ns form or peony tieom, reetnie; mechonial photocopving, noord, Santina, at otherwise, exec at Derived unser Section 107 of 108 ofthe Ph Linked Semen Copyright Act withow tens toe pet ‘rte permission ofthe Publisher, or aathoraation throagh payment ofthe appropriate fee copy fee fe the Copyright Clataoco Conver, Tne. 222 Rastwoot Drive, Danvers AEA 01032, (078) 760.8400 oe (ote) esevs000, or on the neb a! wvew-copreighcom, Requasts fo she Publaber fr permiston slaud bersadveased Uo the Perminaioce Deparment: John Wily © Sons, Ine. 121 River Sete boket, NI ‘07080, (901) "48-8012 fe (201) TAREONS Lunt of Linbiley/Disclalmer of Waeramy: While the publiner ard author have used their bet efforte In'biepasng ths book, choy mabe no rapravntations ce arrntie ih tepae! to ten atearaey os some Plecenese ofthe coutenteaf his book and spocficllydislim inpied aera of etna fr Rows for a particular purpose. No watrnty hy be crowd ore extended Dy anes Toprsentatnee fr written sues suatevnls "The adview aad vratpies contained hers ay othe suitable for your sds Shinn You chould consult with prowemioral where appropriate Hache the pusher or itor sal Se line for any law of pot or ey ater omnereel domes, including but ot linived Uo spel, Incidental, comreguensa. or sther dampen For general information our cthar products and services plage contac! fet with the C8 a? BYP-TGE 287A ance the C Ses 917 ITE SWOG of ae 3 ‘Curtomee Cone Departs one Wiley alo publihes its books in m varity of lation friars, Some contant that apposte by pent Ulweven; may not be writ electcn cma Library of Congress Catalosig-e-Pabiention Data ‘Oprina State Eatinaticn Soluuion Marual / Dan J. Simon. Intuces isloprapbieal referenos and inde ISBN sooooue T Ralean fering, 2 Hz Seng. Nosliesr filtering. 1. Simon, Duo J. ast 2.6878 2004 boitasieze 2n0coeses Print in the United States of Amen, wesresaaayCONTENTS TT Introduction 1 Linear systems theory 2. Probability theory 3. Least squares estimation 4 Propagation of states and covariances 5. The discrete-time Kalman filter 6 Altemate Kalman filter formulations 7 Kalman filter generalizations 8 The continuous-time Kalman filter 9 Optimal smoothing 15 29 41 51 65 79 93, 103iv contents 10 Additional topi in Kalman filtering 11 The Mx: filter 12 Additional topics in Ha. filtering 13 Nonlinear Kalman filtering 14 The unscented Kalman filter Source code for computer problems 13 qr aal 149 169 179 193INTRODUCTION a Frit solution manual is companion tothe text Optimal State Estimation: Kalmen, fie: and Nonlinear Approaches, by Dan Simon {John Wiley & Sons, 2008), “he MATLAB! source code for the computer exercise solutions is giver at the ond of this solution manual. The references in this solution manual refer to the references Section in the text Optimal State Estimation, The equation numbers in this solution ‘manual refer to the equations in the book Optimal State Estimation, Although the MATLAB code for the solutions is not available on the Internet, MATLAB: based source code for the examples in the text is available a: the suthorg Web site? The author's email address is also avstlable on the Web site. and [ ‘eagerly invite feedback, comments, suggestions for improvements, and correcrions, A note on notation ‘Three dots between delimiters (parenthesis, brackets, or braces) means that the Guantity between the delimiters is the same as the quantity between the previous set of identical delimiters in the same equation. For example, (A+ BCD) +(-)" = (A4 BCD) +(A+ BCD)” A+(BC- DBE] = A480 + DI“ ABC + DV) [MATLAB is registered trademark of The MathWorks, In. ‘iito:/ (academic such ed/simond/etimation ifthe Web sie addres changes, it should be ensy to find with an Internet search,CHAPTER 1 ———— $$ Linear systems theory SanBssannnusnnapsneannnnsnssssesnnessnnseeeesseeneness ee Problems Written exercises LAL Find the ek of the mic [ [> 6] Solution ‘The rank of a matrix A can be defined as the dimension of the Jargest submatrix consisting of rows and columns of A whose determinant is nonzero. With this definition we see that the rank of the zero matrix is zero. 1.2 Find two 2x 2 matriees A and B such thas A # B, neither A nor B are diagonal, A 4 cB for ony scalar ¢, and AB = BA. Find the eigenvectors of A and B. Note that they share an eigenvector. Interestingly, every pair of commuting matrices shares ut least one eigenvector [Hor85, p. 54} Optimal State Estimation Solution Manal, First Bation, By Dat 3. Si “M06 John Wiley & Sons, ne 12 unex sysreus HcORY Solution Suppose A and B are given as As [g “| a2 a3 bh f [t &] ‘Then we see that = [ah tone abe + ans ete jeeee gb, + egbs = [Mbit aabe arbi + agbe BA aes ayby + oaba We see that AB = BA if a:b) +aaby ‘ab; + 03b2. This will be true, for example, fay =1.ay= Lhe by = 8, and by = 1, This gives For these matrices A has the eigenvalues ~1 and 3, B has the eigenvalues —2 and 4, and both A and B have the eigenvectors {~1 1 J¥ and [1 1 J”. 1.8. Prove the three identities of Equation (1.26). Solution a). Suppose A is an n x m matrix, and B is an m x p matrix. Then Au Asm ] [ Bis By \\7 (apy = Ant Anm | | Bet += Bop LAyBn oo DA By 1? VAw Bir Ang Bip DAyBa DAu Bir DAyBip LAr Bip Bu An An. Brat = Bhp Aim Am EBay DB Any LE BywAay YE BipAnsProus 3 QED bb). Suppose thar (AB)? = C, Then CAB = I, Postmultiplying both sides of this equation by B-! gives CA = Bo}. Postmultiplying both sides of this equation by A~® gives C= B~'A™. Hence we see that (4B)-! = Bol A™) QED cc). Suppose A is ann x m matrix, and B is an m x n matrix. Then An Aim Bu Bin TAB) = Te Ant Anm Brat Bran DAyBa LAB -5 DAayBia LD Ang Bin = Ay Bye By Bip] fdn oe Am ] TBA) = LByAn LByAjm LDBmgAn LBs Asm = LL BAy QED 1.4 Find the partial derivative of the trace of AB with respect to 4, Solution Suppose 4 Is an n xm matrix, and B is an m xn matrix. Then OTr(AB) ae BA 7 Ba Dy Dy aig Dia Dy debs wig Ths Spa ABs AisBys Bie Dia SPs Au Bic4 near systens Micon 1.5 Consider the mats ; [Ee] Recall thatthe eigenalves of A ane ound by fd the root of the polynomial P{4) = |AT— Al. Show that P(A) = 0. (This is an illustration of the Cayley Hamilton theorem [Bay99, Che99, Kai00}) Solution PO) = |-Al = [aca 2 = [oe ase Ya (aboA+ac~ ve P(A) = AP (+344 (ac- 240% abtbe ab) fac 0 [Sik BIS ]-ero[s ]-[05" ote] Joo] [0 04 1.6 Suppose that A is invertible and [3 4][e}-[*] 2 aifél-[? Solution A’ + AC oO Saigo Band Cnn ee B= A+at 1.7. Show that AB may not be symmetric even though both A and B are sym- mettle Solution Suppose the symmetric matrices A ond B are given as a- [22] ea - [bb oo [he]Prosews 5 “Then we se thet SE b+ aby ashe + ata 40=| 22 Toh antes | AB is not symmetric if ayby + agby # aaby + agby ao [$e] where 0, band c are res. and a and ¢ are nornegative 1a) Compute the solutions of the characteristic polynomial of A to prove that the eigenvalues of A are real b) For what values of 5 is A positive semidefinite? 1.8 Consider the matrix Solution a). The characteristic polynomial of A is PO) eal pear gete “b Ane ~ (ated +ac— Finding the roots of this gives And fetes Veo re ‘The discriminant is non-negative £0 2 is real QED b). In order for A to be positive semnidefinite, its eigenvalues must be positive. ‘The eigenvalues are ates Vat 3 [eves Vesoreae] jlere- Va= FFP] “The first eigenvalue is always non-negative, ‘The second eigenvalue is non- negative ifo +e > y/(a—o)? +42, Solving this equation gives |b| < vac as tho condition of postive semidefinitencs. 1.9 Derive the properties of the state transition matrix given in Equation (1.72). Solution6 near sysTeus tutoRr ‘This proves the first equality. After writing the third expression of the above sequence of equations, we can bring the common factor A out to the right to obtain Sem = [reas Ge J4 = 4 ae 1.10. Suppose that the matric A has eigenvalues Ay and eigenvectors ty ( = Assn). What are the eigenvalues and eigenvectors of —A? Solution Av, = Ain, therefore Avi. From this we see thot —A hay eigenvalues A, and eigenvectors vy 1.11 Show that |e*#| = el4# for any square matrix A. Solution: et = T+ Att eae a 2 ley = ins jan + EL [Ape = sia ins lays Ae = ate aep 1.12 Show that if 4 BA, then ial “E 7 THB)IAL Solution: ‘The equation A = BA can be solved as A= ¢#*4(0). Taking the determinant of this equation gives Al = [e?*400))prosions 7 = [eI A(0}| "| 4(0)] From this we see that [Biel A(0)] ‘Tr(B)| Al QED 1.18. The linear position p of an object under constant acceleration is 1 pa pot ptt where po is the initial position of the object. a) Define a siate vector asr=[ pj B ]” and write the state space equar tion # = Az for this system 1b) Use all three expressions in Equation (1.71) to find the state transition matrix e“* for the system. ©) Prove for the state transition matrix found above that e™ al? 01 0],p al®)=|o01]]e B ooolls b). From the firs: expression in Equation (1.72) we obtain Solution Me 1 = fo 0 From the second expression in Equation (1.72) we obtain eM = £-Yst— Ay s -1 0 = ctlo s a oo s8 unease systems THEORY As 1s? 1/8? = £71 0 ifs af? 0 0 4s it a2 = jor? oo 3 From the third expression in Equation (1.72) we obtain At ett? e ‘The ejgendats of A are found to be A = {0.0.9} {VEE Actually we ean note that A is already in Jordan form, which means that its eigenvalues are on the diagonal, and its eigenvectors form the identity matrix when augmented together. Recall for a third order Jordan block that eh tet Be Malo et jem © 0 et Tn our case A= 0 s0 ). From the ebove expression for e*, if we substitute f = O we see that e QED 1.14 Consider the following system matrix. 10 on won [3 2] Show that the matrix satisfies the relation S(t) = AS(t), but S(¢) is not the state transition matrix of the syscem, SolutionProsieus 9 AS() 1 1 oj][e¢ o o -1] [0 2 _fe¢ oo = [0-2 ‘We see that S(t) = AS(t). However, the state transition matrix is found to be w(t] 115 Give an example of a discrete-time system that is marginally stable but not asymptotically stable QED Solution The system ari: = a is marginally stable, because the stave is bounded for any initial hounded state, but itis not asymptotically stable, because it is not true that, the state approaches zero for all initial states. 1.16 Show (H, Fis an observable matrix pair if and only if (H, F~}) is observable (assuming that F is nonsingular), Solution If (HF) is observable, then Qa #0 for all nonzero x, where a HE HEe! Since F is nonsingular F~(-Vx spans the entire n-dimensional space, (That is, any n-element. vector can be written as F—("—"x for some n-element vector x.) So the observability of (F1, F) is equivalent to QP~O-"x 0 for all nonzero z. This is equivalent to Q's #0 for all nonzero sr, where EO} HP-O-?) H which is the observability matrix of (H, F~'), QED Computer exer 1.17 The dynamics of » DC motor ean be described as J+ Pb=T10 mean systeus raoer where @ is the angular position of the motor, J is the moment of inertia, F is the coefficient of viscous friction, and T is the torque applied to the motor a) Generate @ two-state linear system equation for this motor in the form &o Art Bu ) Simulate the system for 5 s and plot the anguler postion and velocity. Use J = 10 kg m?, F = 100 kg m/s, (0) = [0 0 ]?, and T=10N mn, Use rectangular integration with a step sige of 0.05 s. Do the output plots look correct? What happens when you change the step size At to 0.2 8? What happens when you change the step size 10 0.5 «? What are the eigenvalues of the A matrix, and hove can you relate their magnitudes to the step size that ie requized fora cortect simulation? Solution 4. Then 01 ° [0 te ]+Liede »). Output plots for various simulation step sizes are shown With At = 0.05 the simulation works fine, With At = 0.2 the simulation resulte are obviously incorrect, alshough the simulation is still stable, With Ai = 05 the simulation blows up. The eigenvalues of 4 are 0 and —10. The simulation step size should be appreciably smaller than 1/|Alay, which implies that the step size should be simaller than 1/10. which is consistent with our experimental results. a). Let 2) =8 and 22 a Figures 111.3, tea cos ‘Seconds Figure 1.1 Problem 1.17 simulation with At = 0.05. Good siulation 1.18 The dynamic equations for a series RLC clreuit can be written as u = IRLLI+Mroses ML Figure 1.2 Problem 1.17 simulation with At = 0.2. Poor simulation 10; i 12, i Wa a 7 2 3 + 3 Secones Figure 1.3 Problem 1.17 simulation with At = 0.5. Unstable simulation r= Where is the applied voleage, J is the current through the circuit, and Vz is the voltage across the capacitor. 8) Write a state-space equation in matrix form for this system with y as the capacitor voltage and 2 as the current. b) Suppose that R= 3, b= 1, and C = 0.5, Find an analytical expression for the capacitor voltage for # 2 0, assuming that the inital state is zero, and the input voltage is u(t) = e~* ©) Simulate the system using rectangular, trapezoidal. and fourth-order Runge- Kutta integration to obtain a numerica solotin for the capacitor voltage12 uneaa sestes THEORY Simulate from ¢ = 0 to t = 5 using step sizes of 0.1 and 0.2. Tabulate the RMS value of the error between the numerical and analytical solutions for the capacitor voltage for each of your six simulations. Solution us mR+ Lig tes my = Ch Putting this in matrix form gives a o Le 0 Ht [ -Yk Aa |*+ [ ne * ). ait) = e*2(0) + f At") Bulr) de Plugging in the values of R, L, and C into the A matrix end computing e** 7 [4 deft 4] Substituting everything into the expression for x(2) an clement of z(t) gives of fetes et] eo ar 2fe* — = te) computing the frst a(t) ¢). ‘Table 1.1 shows the RMS error of the mumerical integration methods. ‘Table 1.1 Solution to Problem 1.18. RMS errors when numerically Integrating the orice RLC circuit, for various ncegration algorithars, and for various time step sizes T. T=0i T=02 Rectangular 0.016 0.035 Trapezoidal 0.012 0.024 Fourth order Runge Kutta 0.0018 0.0035 1.19 The vertical dimension of « hovering rocket can be modeled as Ku-ger OM ae = Rt? fg = -uproeuens 13 where 2 is the vertical position of the racket, a2 is the vertical velocity, 22 is the mass of the rocket, u is the control input (the flow rate of rocket propusion). K = 1000 is the thrust constant of proportionality, g = 60 is the drag constant, G =0.673E — 11 m/kg/s? is the universal gravitational constant, M = 5.98E24 lig is the tmess of the earth, and R= 6.876 m is the radius of the earths radius fa) Find u(é) = wo(t) such that the aystem is in equilibrium at x(t) = 0 and all b) Find 2o(¢) when y(t) = 0 and ait ©) Linearize the aystem around the state trajectory found above ) Simulate the nonlinear system for five seconds and the linesrized system for five seconds with u(t) = wo(t) + Awcos(t). Plot the altitude of the rocket for the nonlinear simulation and the Linear simulation (on the same plot} when Su = 10, Repeat for Au = 100 and Au = 300. Hand in your source code and your three plots. Whst do you conclude about the ‘accuracy of your Iineatization? Solution a), Ku-ar2 fe Bs n Solving the above for u(t) gives GMs uit) = Dies b), From the thied state equation and the equilibrium point obtained above we et Solving for 33(f) gives 25(t) = adver ( cc). Use the notation aio(?) = 0, x2o(t) = 0, aaoft) = as(O}exp (SBR). and ‘ua(t) = EF to denote the nominal trajectory on hi an = ans Bans on Bea Lnn(t) zat). asn).at®) = Ar A = Brae, 14 tan + Se 2 ars + 22 an or Gu Yayo(th zeke) 2008). n0l0) 26M, 2 cM ‘Ar, K oyhn- pete tea aol Paw)?” F008) rs14 umar svstexs nicony 2+ 8 ary 4 HA, Ais a a Iesa(thaas(t) zl). molt) 4). Figure 1.4 shows simulation results for various values of Au, As Au increases ‘he linenrized simulation becomes less accurate (ie., the linearized sitnulation does not track the nonlinear simulation as accurately) eee eee ew a = ime : til Jo, :” ce iy 8a nes i mel = owen igure 1.4 Rocket simulations for Problem 1.19CHAPTER 2 —_—_——————————— es Probability theory Ee eee eee Problems Written exercises 2.1 What is the Jth moment of an RV? What is the Oth central moment of an po Solu ith moment of 2 = E(x!) Oth moment ofz = Elz") BQ) 1 ith central moment of = Bl{r—2)'] Oth central moment of = Bi(x— 2) = BQ) <1 Optimal State Estimation Solution Manual, First Edition. By Don J. Simon 5 {©2006 John Wiley ke Sone, Ine16 PropasuiTy THEORY 2.2 Suppose a deck of 52 cards is randomly divided into four piles of 13 cards each. Find the probability thet each pile contains exactly one ace [Gre0il. Solution: Consider the first pile, There are a total of 52-choose-18 possible first piles. There fare a total of 48-choose-12 different ways of selecting 12 non-Aces from the re maining 48 non-Ace cards. The odds that the first pile hes exactly one Ace is therefore 4(48-choose-12}/(S2-choose-13). If tis event occurred we see that there are 39 cards remaining to be deslt, including three Aces. Therefore, the odds that the second pile has exactly one Ace is 3(86-choose-12)/(39-choose13), If both of the previous events occurred we soe tiint there are 26 cards remaining to be dealt, Including two Aces. Therefore, the odds that the third pile bas exactly one Ace is 2(24-choose-12}/(26-choose-13). Given that all three of the previous events oc- currad, the odds that the fourth pile has exactly one Ace is 1. Multiplying these odds together gives the total probability of 10.55%, 2.8 Determine she value of a in the function _facl-2) re [01] pater {OT tie so that frr(z) is a valid probability density function [Lie67]. Solution: In order for foe(z) to be a valid paf its integral from —oc to oe must be equal to 1 [trie = [ oa-nee a ‘Therefore a 24 Determine the value of a in the function i@)= 5 so that fx(2) is a volid probability density function. What is the probability thet Ixisv? Solution: In order for fx (2) 10 be & valid pdf its integral from —oe to +9e must be equal to 1 flame = [ata atan *(e")[~ an/2 »Proseens 17 ‘Therefore a The probability that |X| <1 is computed us P(X sy = 2.5 ‘The probability density function of an exponentially distributed random vari able is defined as foliows. seta) = (GQ 228 where a > 0 a) Find the probability distribution function of an exponentially distributed random variable, b) Find the mean of an exponentially distributed random variable, ¢) Find the second moment of an exponentially distributed random varisble, 4) Find the variance of an exponentially distributed random variable. ©) What is the probability that an exponentially distributed random variable takes on o value within one standard deviation of its mean? Solution, a). Pie) = foo a lo = ft-e® apo = 0 a0 b), [200° ae Using integration ty parts we obtain z= senles [ oe de i ©). Ele")18 provasury mcoRY integration by parts we obtain Ble) = steer ie dee" de [fovea 7 a 4) °). P@e-aS2St+0) = 2.6 Derive an expression for the skew of a random variable as a function of its first, second, and third moments, Solution: skew = Bl(~2)"] Ele — S2?e + 8277 - 25) w= Ble) — SEB (2) + 239 2.7 Consider the following probebility density function ab re a) Determine the value of «in the so that fx (2) is valid probability density function. (The corre, value of « makes (2) a Cauchy pdf.) b) Find the mean of a Cauchy random variable. Ex(z) b>o Solution: 1). In order for for() to be a valid pai its integral from —s0 to +20 must be equal to 1 ~ ab typ) [ogtae = atan“"(a)}Therefore a= 1/x. b). EQ) = bse or = 3fow i = pulse = ‘This indicates that the mean is oo, but what it really says is that the integral does not converge to a real number, so the mean of « Cauchy random variable does not exist. 2.8 Consider two zero-mean uncorrelated random variables W and V with stan- dard deviations oy and ¢,, reapectively. What is the standard deviation of the random variable X = W+V? Solution: E(x - 2)? EW +V—w- 3)? E(W+V/] E(W?) + E(V?) + 2E(WV) = ch+o+0 2.9 Consider two scalar RVs X and Y. a) Prove that if X and ¥ are independent, then their correlation coofficient, b) Find an example of two RVs thet are not independent but that have a correlation coefficient of zero. ¢) Prove that if Y is a linear function of X then p= +1. Soluti a). IX and ¥ are independent then £[(X—X)(¥ ~Y)] = EUk —X)e0"-¥). Therefore20 Froenguty reeoRY E(X -X)E(Y —¥) Fore esey (k= 2y"-¥) b). Suppose the discrete RV X has a 25% probability of heing —1, 1 25% prob. ability of being +1, and a 50% probability of being 0. This means that X= 0. Suppose that ¥ = |X] so that X and Y ate clearly dependent, and E(XY) = E(X|X|) =0. Thea Cxy E(XY)-X¥ 0 which means that p= ©). IY = aX for some constant @ then p= Hx 7 a i “abi(x =x) VEX — XP] VE[aX — aX?) a Tal = a If a is positive (negative) then p = 1 (1). 2.10 Consider the following function (Lio67] feten={ ON 220030 otherwise a) Find the value of @ so that Jay (x.y) is a valid joint probability density function, b) Calculate 2 and g. ©) Calculate E(X*), E(¥?), and E(XY). @) Calculate the autocorrelation matrix of the random vector [XY ]” €) Calculate the variance a2, the variance a2, and the covariance Cry 1) Calculate the antocovariance matrix of the random vector [ X_¥ J7. 5) Calculate the correlation coefficient between X and Y. Solution: a). In order for fxy{(v.y) vo be a valid pdf its double integral from —90 to +90 most be equal to 1 [flowtenss = ff free eM dedyproses 21 ‘Therefore a = 6. »). Foe [Ustsee é La[f torte vas] de ~ Fearon iz 6 fe [feral a 9. BQ?) = f Myla)ee - (re [forena} Similarly, we can obtain22 PROBABLITY THEORY a) — | Bix) BEY) Bar = | gay) BW) 12 1/6 176 2/9 °). ok = B(X*)- ote “a of = EW?)- 9 tee “9 Cay = EXY)— a9 =0 f). eee EX = £7) mH” | = [| Bray —) BOT) _ [4 0 = 1 19 8) Cav 0 aaty =o 2.11 A stochastic process has the autocorrelation Rx(r) = AeW#I"l, where A and kk are positive constants, a) What is the power spectram of the stochastic process? b) What is the total power of the stochastic process?” ©) What value of & results ia half of the total power residing in frequencies less than 1 Ha? Solutior Sx) = [aetnaprccutys 23 adtooedr [sei bo ») Pxo= if Seeds 1 /* _2dk = ele OG \ \ g =A Therefore k = 2a. 2.12 Suppose X isa random variable, and Y(t) = X cost is a stochastic process. a) Find the expected vaiue of (0). b) Find A[Y()], the time average of ¥ (0) ¢) Under what condition is g(t} = Aly (t))? Solution: a). VW) = EX cost] = Feast b), Aya) = ial w(t) at24 eRoeapiry THEORY ie = tng [iroea =0 ). Gt) = ALY (#)] if and only if # = 0. 2.13 Consider the equation Z =X +V. The pdf's of X and B are given in Figure 2.1. a) Plot the pdf of (Z|) as a function of X for Z = 05. b) Given Z = 0.5, what is conditional expectation of X? What is the most probable value of X? What is the median value of X? 15) i os Aso os tS. 15; pat os Figure 2.1 péf's for Problem 2.15 Sci} PUZ=03X=m) = PX +V=05|X— 2) PV =05-n) pal(Z = 0.5|2) is therefore equal to f\:(v) shifted to the right by 0.5, except, tha: the pdf must be truncated at # = I since x cannot be greater than 1 ‘The truncation results in 9 scaling of the pdf by a factor of 8/7 in order to give the pdf an area of 1. This is shown in Pigure 2.2 b). From pdf(Z = 0.5|z) we see that che conditional expectation of X is, [snes = ae)ae [osGe+3) 2+ La( 17/42Promens 25 15 0 sa is Figure 2.2 Solution to Problem 2.13. ‘The most probable value of X’ is the peak of the paf(Z = 0.5|2) curve, which #=1/2 ‘The median value of X is the midpoint on the a-axis of the pdilZ — 0.5|z) carve, which is 14 2.14 The temperature at noon in London is « stochastic process. Is it ergodic? Solution: A stochastic process is ergodic only if its time average is equal to the mean of the time-varying RV at each moment in time. Since the temperature in London, considered as an RV, hes a different mean at different times of the year, it is not ergodic. Computer exercises 2.15 Generate N’ = 50 independent random numbers, each uniformly distributed between O and I. Plot a histogram of the random numbers using 10 bins. What is the sample mean and standard deviation of the numbers that you generated? What would you expect to see for the mean and standard deviation (i.e, what are the theoretical mean and standard deviation)? Repeat for N = 50K) and N= 5,000 random tiumbers. What changes in the histogran: do you see as NV increases? Solution: ‘The solution is given in Table 2.1 and Figure 2.3,26 PRosABLITY THEORY ‘Table 2.1 Solution to Problem 2.15. Your results may vary depending on the particular random muraber esquence that vou generated. In general, the sample mean and standard doviation get closer to te theoretical values as V increases, ‘Theoretical 0 N= 500 N= 5000 Mean 0.500 0565 0476 0.504 Standard Deviation yT/T2~ 0.289 0.209 0.297 0.288 wy Figure 2.8 Solution to Problem 2.15. As N’ inereasos, the histogram gets closer to a truly uniform distribution. ‘Thetis, the sample pdf approaches the theoretic’ pt. 2.16 Generate 10,000 samples of (0: +22)/2, where each 2, is 2 random mamber uniformly distributed on [-1/2,+1/2]. Plot the 50-bin histogram. Repeat for (ey + ey +2 + 24)/4, Describe the difference between the two histograms. Solution:Proaeus 27 = lian Pe] 0» 20 qe LT MDs EERE eee ene] «eo xe earl Mea 8 ° os Figure 24 For N= 2 che histogram looks like a tringular pe. Por N= 6 she histogram looks more lke a Gaussian peCHAPTER 3 Least squares estimation Problems Written exercises 3.1 In Equation (3.6) we computed the partial derivative of our cost Function with respect to our estimate and sot the result equal to Oto solve for the optimnal estimave. However, the solution mninimizes the cost function only if the second derivative of the cost function with respect to the estimate is positive semidefinite. Find the second derivative of the cost function and show that it is positive semidefinite Solution: HT Fe This is positive semidefinite because for any vector y we have VQHTH)y = 2(Hy)" (Hy) = HyliE 20 Optimal State Estimoton Solution Manual, Fvet Edition. By Dan J. Sion 2 06 Jahn Wiley 4 Son. Ine30 Least squanes esnwarion QED 3.2 Prove that the mattix PF that is computed rom Equation (3.25) will alweys be positive definite if P,1 and Ry are positive definite. Solution Po = (I~ KyHp)Phoa(l — KeHs)” + Ketek? Let @ be an arbitrary vector of correct dimensions so that aP,a” is a scalar. Then aPha™ = all — KyHy)Pa(U - Ke Hy)"a” + 0K (RRP oT = bP bh + eR eT where b and ¢ are defined by she above equation. If Fy. and Ry are both positive efinite. then the above expression is positive for all b end e, which means it is positive for all a, which means that Py is positive definite, QED. 3.3 Consider the recursive least squares estimator of Equations (3.28)-(3.80). If 2210 information about the initial state Is available, then Py = 90F. Suppose that you have a system Iike this with Hy = 1. What will be the values of K, and P,? Solution: R Ay = him mee +R =1 Po > O-KJIR+ KR =R 8.4 Consider a battery with a completely unknown voltage (Py — 00}. Two independent measurements of the voltage are taken to estimate the voltage, the first with variance of 1, and the second with a variance of 4. a) Write the weighted least squares vollage estimate in terms of the two measurements yx and yp b) If weighted least squares is used vo estimate the voltage, what is the var lance of voltage estimate after the first measurement? What is the variance ‘of thie voltage estimate after the second measurement? ©) If the voltage is estimated as (y: + y2]/2, an unweighted average of the measurements, what is the variance of the voltage estimate? Solution: a). From Equations (3.28)-(3.30) we calenlate 441 Fs gmt amrrosews 31 bb). From Equations (3.28)-(3.90) we caleulate Pos P= 4/5 ° Bilin ~m)/2~ VP) = Blfos/2)? + (02/2))] aaa 5/4 3.5 Consider a battery whose voltage is a random variable with a variance of I. ‘Two independent measurements of the voltage are taken to estimate the voltage. ‘the first with @ variance of 1, and the second with @ variance of 4 a) Write the weighted least squares voltage estimate in terms of the initial estimate #p and the two measurements i and yp. b) If weighted least squares is used to estimate the voltage, what is the vast ance of voltage estimate after the first measurement? What is the vatiance of the voltage estimate after the second measurement? Sok ‘b). From Equations (3.28)-(3. 30) we calculate Pos 1/2 Pr = 4/9 3.6 Suppose that (21.22.---2n} is a set of random variables, each with mean & and varionce a2. Fursher suppose that E[(ri ~ 2)(zy~ @)] = 0 for i# j. We estimate Zand a? as follows, a) Is # an unbiased estimate of #? ‘That is, is EU2) =a? b) Find Bixia,) in terms of F and a? for both i= j and i ¢ 5 ©) Is 6? an unbiased estimate of o?? That is, is £(2) = 027 If not, how should we change ¢* to make it an unbiased estimate of a2?32 Leasr souanes esmiaTion Solution E@) = ty Fla) i So yes, # an unbiased estimate of 2. b), For i= j we obtain = Bie. 2) Eta?) ae Eta) For i ¢ j the problem statement sells us that B[(e—2)(2,~#)] = 0. Therefore Blre))-# = 0 Bea) = 2 ° E@) = = [Ena 2S mine) +t vow) 7 ce its x & pe-4 $a Now use the fact that. £(2? simplify the above expression. = 0? + 2%, and Blziz,) = # fori # j, to Be) = + [uot 42) b din(o? +37) + nla - nz" We see thet 4% an not an unbiased estimate of o?. Based on the above derivation it is apparent that Seeproses 33 js an unbiased estimate of 0°, 8.7 Suppose a scalar signal has the values 1, 2, and 3. Consider three different estimates of this time-varying signal. The first estimate is 3, 4, 1. The second estimate is 1, 2, 6. The third estimate is 5, 6, 7. Create a table showing the RMS value, average absolute error, and standard deviation of the error of each estimate. Which estimate results in the error with the smallest RMS value? Which estimate results in the error with the smallest infinity-norm? Which estimate gives the error swith the smallest standard deviation? Which estimate do you think is best from an intuitive point of view? Which estimate do you think is worst from an intuitive point of view? Solution: For Estimate #1 the errors are 2, 2, and -2. We therefore find RMS Error = V@FP + EIS Ave Abs. Error = ([21+ [2|+|—2))/ Average Error = (24+2~2)/8= 2/3 Std. Dev. = V(@— 2/9? T @= 2S + 2 BPS = v3/3 Note that your standard devistion measurement will be different if you normalize by (n— 1} (which is 2 in this case) instead of n, The corresponding quantities for the other estimates are found similarly. Table 3.1 shows that Estimate #2 gives the smallest RMS error, Estimate #1 gives the smallest average absolute error, and Estimate #3 gives the error with the smallest standard deviation. ‘The error signals are {2,2,—2} for Estimate #1. {0,0,3} for Estimate #2, and (4,4,4) for Estimate #3. Intuitively it looks tike Estimate #2 is best and Estimate #3 is worst; however, this is a matter of opinion Decause the term “est” Is ambiguous. Table $1 Solution co Problem 8.7. RMS error Ave. abs. error Std, dev, of error Estimate #41 2 2 Estimate #42 a1 3 Estimate #43 4 4 3.8 Suppose a random variable x has the pal f(r) given in Figure 8.1 a) x can be estimated by taking the median of its pdf. That is, 2 is the solution to the equation [sean [roa Find the median estimate of z.34 Least Souanes esratavion b) © can be estimated by taking the mode of its pdé. That is, 4 = arg maxf(2} Find the mode ostimate of =. c) can be estimated by computing its mean. That is, [i stovte Find the mean of 2 ) x cam be estimated by computing the minimax value, That is. siamaxj~ 2 Find the minimax estimate of z, Figure 3.1 pdf for Probiem 3383 Solution: From Figure 3.1 we see that the pdf f(z) is given as 4e/3 2 € (0,3/4) Fle 8/5 ~dx/3 2 [3/4,2] 0 otherwise a). It should be clear from Figure 3.1 that the median estimate of a is greater than 3/4. Also, the total ates of the pdf is 1. We can therefore solve for the median of the pdf as 2 (8/5-42/5) dr = 1/2prowiens 38 8/54 257/5 — 82/5 Ye 2- V5 x 08820 1b). The mode of f(z) is that value of x where f(x) cskes its mascimum value. From Figure $.1 we see that thin is #=3/4 ¢). The mean of 2 is computed as [Lsteven [Crepes ff 0s a = 99/108 o.o167 u 4). The minimax estimate of z is that volue of « that minimizes the maximum possible estimation error. From Figure 3.1 we see that this is given as 3.9 Suppose you are responsible for increasing the tracking accuracy of a racer system. You presently have a radar thet has a measurement variance of 10. For equal cost you could either: (a} optimally combine the present radar system with 2 new radar system that has a measurement variance of 6; or, (b) optimally combine the present radar system with two new radar systems that both have the same performance as the original system [May79}. Which would you propose to do? Why? Solution: Assume that the initial position uncertainty of the varget is infipive ~ that is, Py = co. For option (s) we have Ry = 10 and Rz = 6, Equation (3.30) gives, Ky Poo= (-Ki)*Po+ KTR, =F A Fak 3/8 (= Ka)? Pi + KER = 47536 Leasr sauanes estmmatION For option (b) we have R, = Rz = Ry = 10, Equation (3.30) gives By KOs Rm =1 A (Ka) Pot APR, =k PB = Bam = 12 Po= (Katt KER, -5 "Rem = 1/3 Ps (1— Ks)’ Pa+ KERy = m/s 3.338 So we should choose option (b) because It gives s more accurate position estimate than option (a) 3.10 Consider the differential equation, E48 If the input u(é) is an impulse, there are two solutions 2(¢} that satisfy the differ ential equation, One solution is causal and stable, the other solution is anticausal and unstable. Find the two solutions. Solution: ‘The causal and stable solution is 2(0) = e"U) where U(f} is the unit step function. The anticausal and unstable solution is a(t) = -e*U(—} 3.11 Suppose a signal 2(¢) with power spectral density i-st Se(s) is corrupted with additive white noise v(t) with « power spectral density S,(s) = 1 a) Find the optimal noncausal Wiener filter to extract the signal from the noise corrupted signal 'b) Find the optimal causal Wiener filter to extract the signal from the noise corrupted signal.Prosusms 37 Solution: a). The optimal noncausel Wiener Slter is given as Se(s) as) Seals) Se(s) Se(s) + Su(s) ea b). The optimal causal Wienor filter is obtained as follows VE s~ Sto Sale) Sate So(8) =I s-2 Szuls) (+ 2(8—De- Ve =1 (8+ 2)(6— VB) 1 L LEVEE BTV ‘causal part 1 Suis) Seta (enna nro =) ( ste ) 1 ~ Ass V6/ BE VhGF 3 1 * Vier 8.12 A system has the transfer function Gls) = G(s) = s-3 Ir the input is an impulse, there are two solutions for the output 2(t) that satisfy the transfer function. One solution is causal and unstable, the other solution ie antiesusal and stable. Find the two solutions. Solution: ‘The causal and unstable solution is a(t) = SU) ‘The anticausel and stable solution is a(t) = —"U(—t)38 jeast squares ssriarion Computer exercises 3.13. The production of sve] in the United States between 1046 and 1956 was 66.6, 84.9, 88.6, 78.0, 96.8, 105.2, 95.2, 111.6,88.3, 117.0, and 115.2 million tons {Sor80, Find the least squares fit to these data using (a) Iineas curve 88; (b) quadratic curve fit. (c) cubic curve ft; (2) quartic curve fit. For each case give the following: (1) plot of the original date along with the least squares curve; (2) the RMS error of the least squares curve; (3) the prediction of stee] production in 1987. Solution: Figure 3.2 shows the four regression curves. ‘The RMS errors ofthe regression curves fare 8.782 (linear curve fit), 8.667 (quadratic carve fit). 8.280 (cubic curve ft}, and 8.282 (quartic curve fit), ‘The predicted steel production In 1997 is 118.7 (linear curve fit), 114.5 (quadratic curve fit), 126.2 (cubie curve ft), and 128.9 (quartic carve fit) 3 10 Figure 8.2 Solution to Problem 3.13 3.14 Implement the Wiener filters for the three examples given in Section 3.4 land verify the results shown in Section 3.4.5: Hint: Example 8.6 shows that if = 2 + w whore w(¢) is white noise witha variance of Qe = 2, then. SO) = FET Fron Sections 14 and 8.1 we see that this system can be simulated as (t+ Ath Malt) + w(t} VQeAt vt) = 2) 4 (QV RIAt where w(2) snd v(t) are independent zero-mean, unity varianoe random variables.Promevs 39 Solution: 1 tan 20 simulations with a simulation time of 100 s and a step size of 0.1 s. The average E(e*) that I obtained was 0.95 for the parametric Wiener filter. 0-82 for the causal Wiener filter. and 0.78 for the noncausal Wiener filterCHAPTER 4 ———— Propagation of states and covariances Ba ere ee CU ee eee ee CHEE Eee EEE Problems Written exercises 4.1 Prove that a dz) wed = 2 |S Solution: ‘The derivative and expectation operators are both linear, therefore they can be inerchanged, 4.2 Suppose that a dynamic scalar system is given a8 zh41 = fry + te, where ‘wx Is zero-mean white noise with variance 4. Show that if the variance of zy is 2 for all k, then it rust be true that f? = (e! — l/o? Solution: From the system equation we see that a = Sor +e Optimal State Estimation Solution Monat, First Fattion. By Dan J. Simon a 2006 Jahn Wiley £ Sons, Ine42. pROPAGATION OF STATES AnD COVARIANCES Wok, = of = 2? for all &, then ? fetta P= (t= ajo? QED 4.3. Consider the system a= [Saalese[t}= at pace ‘a) Find all possible steady-state values of the mean of zr. b) Find all possible steady-state values of the covariance of 2x Solution: Banaras ae pee aaa eae — (tale =. _ : Actually (1) will be equal to its initial value Zo(2). bb). The steady-state solution to Equation (44) can be watten 2s P = FPFT+Q Pa Pas 1 17fP Pe]f[? © ],f9 9 Pa Po 0 2} | Pe Pelli y2)*[o2 Equating individual elements on the let and right side of the above equstion ves Pa Part 2Pi2+ Poo Po = (Pot Pu)/2 Pa = Praldth Solving these equations gives Pa = 4/8 Pa = 4/3Promems 43, Pi: does not have a steady-state solution. 44 Consider the system of Example 1.2. 1a) Discretize the system to find the single step state transition matrix Fi, the discrete-time input matrix Gy, and the multiple-step scate transition matrix Fh. 1b). Suppose thie covariance ofthe initialstate is Py = diag(1,0), and zero-mean discrete-time white noise with a covariance of Q = diag(1,0) is input to the discrevotime system, Find a closed-form. solution for F Solutio a). Denote the discretization step size as T. From Section 14 we see that, “Ty cs Ge = fi [em are Fi is equal to zero for & <4, and it is equal to the identity matrix for k For £> 7 it is equal to Fea FeosFea2--- Fi FE 1 (k-or oo7 ‘b). Poo = Fea Pe-aFia + Qk [mat mat] = [oT] [8 Multiplying this out gives Pua = Pie Pak Pog = Para Since Pro, that Pio that Pre Owe see that Pra, Oto see that Pia EAL. Therefore 0 for all k. Combine this with the fact O for all k, We can use these facts to see44 PROPAGATION OF STATES AND COVARIANCES 4.5 Two chemical mixtures are poured into a tank. One has concentration ¢; and is poured at rate Fj, and the other lias concentration oy and is poured at rate Fo ‘The tank has volume V, and its outflow is at concentration e and rate F. This is typical of many process control systems (Kwa72}. The Imearized equation for this system can be written a6 ae[% 8 ],. fee oe P| am sae] where Fy, Vo: and eo are the linearization points of F, V, and @ The state x consists of deviations from the steady-state values of V and cand the tose input w consists of the deviations from the steady-state values of F, and F;. Suppose that Fo = 2Vp, e; ~ 00 = Vo, and ee ~ ey = 2V Suppose the noise input w has an identity covariance matrix a) Use Equation (4.27) to calculate Qx-. bb) Use Equation (428) to approvimave Qs 1 c) Evaluate your answer to part (a) for small (ty ~ thy) to verify that it matches your answer to pert (b). Solution: 18). First compute Qe as follows. Qe = BE(ww")B™ 23 35 Now compute Qj.-1 a8 follows. oer = ff eta er = te PY eget rire o [Stree eee ee Btn Bitte 1-et b), Qo * QT fe 3r 5r ). The answer to part (2) can be written as Qa- t-G-27+ Sy.) edesry ay 7° Li-a-ar+ 4.) ga- ars BRE yg)ewop.es a5 For small T the higher order powers of T can be neglected, which gives an approximate value for Qh which is the same as part (b) 4.6 Suppose thet certain sampled data system has the following state-transition matrix and approximate Q,_; matrix [as calculated ly Equation (4.28) _ ma = [9 on | [3 3] ae ar ar where T = t,~ tis the discretization sep size. Use Fqustion (4.26) to compute ‘the steady-state covariance of she state asa funetion of J Solution: Im steady state Equation (4.26) can be written as PP + Qe Multiplying out and equating individual elements of the matrices on the right and left side of this equation gives, Pe Po = Py or Po = eT Py iar Po = Pa ot Solving these equations gives 4.7 Consider the tank system described in Problem 4.5. Find closed-form solu: ‘ions for the elements of the state covariance as functions of time, Solution: First compute Q. as follows & BE(ww"}BT [53] Pram Ego 1) [A e}-[¢ SIR ELL & By46 PROPAGATION OF STATES AND COVARANCES ‘Multiplying out and equating the metrix elements on the right and left side of this equation gives Py = -2Put? Po = -8P2+3 Pa = Path Solving these equations gives 4.8 Consider the system aa = [1 yy Jaem uy, ~ (0,9) 2 a (8 ‘Use Equation (4.5) to find the steady-state covariance of the state vector, 1a 1/4 136 1/16 Jee vedo [6° tes] 4.9 The third condition of Pheorem 22 gives a sufficient condition for the discrete time Lyapunov equation to havea unique, symmetzic, positive semidefinite solution, Since the condition is sufficient bui not necessary, there may be cases that do not meet the criteria of the third condition that still have a unique, symmetic, positive Semidefinite solution. Give an example of one such case with & nonzeto solution.proaieus 47 Solution: “There ate many sich examples. Ones 20 r- [55] 00 a= [5%] This situation does not meet the third condition of Theorem 21 because F is not stable. However, substituting these matrices in the discrete-time Lyapunov equa tion gives [me m]-[6 | Pa Hy[3 o]+[8 ak Pa Pe} "Lo o][ Pa Poj(oo|*t{ar Expanding this equation and solving gives the unique solution vfs] which is symmetric and positive semidefnite 4.10 Prove the sifting property of the continuous-time impulse fumetion 4(1), which can be stated as Stt}6(t — a} dt = flay Solution: Note that 6(t — a) = 0 everywhere except at t — a, In addition, we know that 5(¢— a) has an area of one, Therefore [stos-osae = seoyse—ee so J é(t— a)at Sto) QED Computer exercises 4.11 Write code for the propagation of the mean and variance of the state of Exsmple 4.2. Use my = 1, Py = 2, f = —05 and q, = 1. Plot the mean and variance of x for 5 seconds. Repeat for Py = 0. Based on the plots, what does the steady-state value of the variance appear to be? What is the analytically determined steady-state value of the variance?48 PROPAGATION OF STATES ANO COMMRNCES os DES are rere terete. a Taranea =] 18pm vatence P os % + 2 3 « 6 Figure 4.1 Solution to Problem 4.11 Solution: Figure 4.1 shows the solution. The steady-state value of the variance appears vo be 1. Analytically, the steady-state value of the varinnee is ~ge/2f = 1 4.12 Consider the RLC circuit of Example 18 with R C= 1, Suppose the applied voltege is continuous-time zero-mean white noise with @ variance of 1. The initial capacitor voltage is a random variable with a mean of I and a variance of 1, The initial inductor current is a random variable (independent of the initial capacitor voltage) with a mean of 2 and a variance of 2. Write a program to propagate the mean and covariance of the state for five seconds. Plot the two clements of the mean of the state, and the throe unique elements of the covariance. Based on the plots, what does the steady-state value of the cavariance appear to be? What is the analytically determined steady-state value of the covariance? (Hint: The MATLAB fanction LYAP can be used to solve for che continuous-time algebraic Lyapunov equation.) Solution: To solve this we first must see that Q. = Bq-B7, where q, = 1 is the variance of ‘the epplied voltage. Figure 4.2 shows the solution. The steady-state value of the satan appean to ve | ¥2 172]. anata, be wendy vale of te variance is the solution of the Lyapunov equation AP + PAT + Q. = 0, which is wes 2 1/2 |" 4.18 Consider the RLC circuit of Problem 1.18 with R= 3, L = 1, and C 0.5, Suppose the applied voltage is continuous-time zero-mean white noise with 8 variance of 1. We can find the steady-state covariance of the state a couple of different ways,proses 49 =a] 1 m2) Figure 4.2 Solution to Problem 4.12 © Use Equation (4.49) 1 Discretize the system and use Equation (44) along with the MATLAB func tion DLYAP. In this case, the discrcte-time white noise covariance @ is elated to the continuous-time white noise covariance Qe by the equation Q = TQe, where T is the discretization step size (see Section 8.1.1). a) Analytically compute the continuous-time, steady-state covariance of the state. 1b) Analytically compute the discretized steady-state covariance of the state in the limit as T+ oe. ‘€) One way of measuring the distance between Iwo matrices is by using the MATLAB function NORM to Lake the Frobenius norm of the difference between the matrices. Generate a plot showing the Frobenius norm of the difference between the continuous-time, steady-state covariance of the state, and the discretized steady-state covariance of the state for T between 0.01 and 1 Solution: a), First we note that Qe = BgeB, where ge = 1 is the variance of the input, and B is found in Problem 1.18 as B= [0 1 ]7. The continuous-time steady-state Lyapunov equation is AP + PA? +Qo=0. Analyticaly colving this equation gives ae [ 3 0 ] 0 1/6 b). The discrete-time system matrix F and the discrete-time noise covariacice are found as Fo AT50 pRceAGATION OF STATES AND COVARIANCES lim F = Tae Substituting these values into the diseretiaed steady-state Lyapunov equation P= FPFT+Q and solving for P gives Jim P= [ cai 0 0 0 & ©). The solution is shown in Figure 4.3. 09 steady stata covatiance aor 0 a2 ba 06 08 1 ‘sacetzaton sep ice Figure 4.3 Solution to Problem 4.13CHAPTER 5. V—————— The discrete-time Kalman filter SE SEE eee Problems Written exercises 3.1 A redionctive mass has « halflife of r seconds, At each time step the num- ber of emitted particles z is half of what it was one time step ago, but there fo some error te (zero-mean with variance Q) in the number of emitted particles duc to background radiation. At each time step, the number of emitted particles is counted, The instrament used to count the number of emitted particles hes a dom error at time k of ux, which is zero-mean with a variance of R. Assume thea tw and vp are uncorrelated, a) Write the linear system equations for this eystem, 1b) Suppose we want to use a Kalman filter to find the optimal estimate of the number of emitted particles at each time step. Write the one-step a posteriori Kalman filter equations for this system ©) Find the steacly.state a posterior estimation-error variance for the Kalman filter. 4) What is the steady-staie Kalman gain when Q > R? What is the sveady- state Kelman gain when @ = 2R? Give an intuitive explanation for w Optimal State Rstimation Solution Manual. First Edition. By Dan J. Simon 51 ‘©2006 Jolin Wile & Sons, Ine52 THE OSCRETETINE KALMAN FLTER the steady-state gain changes the way it does when the ratio of Q to R changes. Solution a), ») Ke Ky The one-step « posterior’ 1 ae = pte tte z a te = Feb = Paki Pha + Qe = GRia+@ = PoHE(HAP, HE + Rey rg + Kae — Heke) = det + Wilma) = (~ KeM) Py RP, +42) P+ 4Q+R) = PRHTR, Pe, +4Q Pet aQ+R) equations can be suinmarized as follows, RUPE + 4Q) PL +4 +B) 1 t42 FR, + Q4+R) = pth t Kulm az, py 3 R(P*+ +4) P= BE ER@+prosiems 53. Solving for P* gives Pt = 3 [-Uq+ an) + VEQTORP + 16OF] ‘We use the plus sign to get a positive solution for PY. 4). Use the formula Ay = PZHERG* to gt the seady-scate gain Ke iz [Ue + 3R) + VEQT SRF ST 160R] When Q = ft this gives K = 0.59, When Q = 22 this gives K = 0.65, We see thal as process noise increases relative to measurement noise, she gain increases, ‘This 1s because the Kalman filter plaoas more emphasis on the measurements when the system model is less certain, 5.2 This problem illustrates the robustness that is achieved by the use of the Joseph form of the covariance measurement! update equation. Suppose you have a discrete-time Kalman filter for a scalar system. a) Find OP /4K, for the third form of the covariance measurement update in Fquation (6.19). b) Find F/R; for the Joseph form (the first form) of the covariance mea surement update in Equation (5.19). After you get your answer, substitute for Ky frorn the Kalman gain expression. ©) Use the above results to explain why the Joseph form of the covariance rmeasureiment-update equation is stable ad robust. Solution: 4). For » scalar system, the third form of the eovatiance measurement update is Pt = (1~ KH)P~. From this we obtain ap+ : aK = ~#P b). For a scalar system, the Joseph form of the covariance measnrement update is PY = (1 - KHY!P~ + RK?. From this we obtain or aK Substituting for K from Equation (6.19) gives 2H(1—KH)P-42KR apt P-H? int 2P-HR (Ing =a)? P-iP+R ~2H*(P~)? — 2H POR + 2H8(P~)? + 2HP-R ER =0 ¢). The above results show that the Joseph form of the covariance measurement update is insensitive to numerical errors in the Kalman gain,BA He osscReTE-TWe KAMAN FLTER 5.3 Prove that Elé{(#{)"] = 0. Hints Since 2 = Blip] ie o constant and 2 = 2)—2f is zero-mean, we know that B[3$ (Z$)") = 0. Given this information, prove that Fi} (7 )*] = 0. From this point, use induction to complete the proof. Solution: ‘This solution is taken from [Gel74]. We will prove that If F[ée7f] = 0, then Plix8,,] = 0. Note that the + superscript has been dropped for ease of nota- ‘ion. We will assume that the input is zero, again for ease of notation, Pliendbyl = Bf [Fate + Kees (-MessFate + Hasse + thss)] (Fy Kes Head” +02 Keene —D? + thar Ea] = ~KasrHes PePo(FE ~ FEE KE) + Kart Hiss Qu(HEa Kin ~ + Kes Ren Kien = —KearHeni(FePEPE + Qu) + KierHia (PPPs + QW Pb sKEyy + Kee Rekgey Ki Mes Pog (l — KE HB) + Ke Re KE Kei(-Hist PEs + Resa) =0 Since it is true for k= 0, it 3s also true for & = also true for k= 2 ete, Proof by induction. QED 5.4 Suppose that you have a fish tank with zp pirenhas and 29 guppies [Bay99} ‘Once per week, you put guppy food into the tank {which the piranhas do not eat) Each week the piranias ext some of the guppies. The birth rate of the piranha is proportional to the guppy population, and the dexth rate of the piranhas is proportional to their own population (due to overcrowding). ‘Thexefore ap(h-+ 2) = pli) + ki2g(k) — haxp(k) + wp(E), where hy and kz ere proportionality constants and up{B) is white noise with a variance of one that accounts for mismodeiing. The birth Fate of the guppies is proportional to the food supply wu, and the death rate ff the guppies is proportional ta the piranha population. ‘Therefore, x(k + 1 9{k) + u{k)~ kazp{K) +Ug{k), where ks is @ proportionality constant ond w(K) is white noise with a variance of one that accounts for mismodeling. The step size for his model is one week. Every week, you count the piranas and guppies, You can cout the pirantias accurately because Uey are so large, but your guppy count has zerormnean, noise with a vatiance of ane, Assume thet ky = Land ky = fy = 1/2 a) Generate « linear state-space model for this system. b) Suppose that at the initial time you have a perfect count for rp anc 25. Us ing a Kelman filler to estimate the guppy population, what is tie variance of your guppy population estimate after one week? What is the varianoe after two weeks? ©) What is the ratio of the piranha population to the guppy population when they reach steady state? Assume that the process noise if zero for this part of the problem. since it is true for & = 1, it isSolution: Poosiews 55 a). Let the first element of the state vector be the piranha population, and the second element of the state vee! ‘equations are given as 1-ky ve a 7 welds we ~ (0,Q) um ~ (,R) 7 e- [it i a= [ot 'b). Since the initial population cou vor be the guppy population. The state space ky nfs [t]ee[ 2] 1 Jove[2 Jue T+ pk Wok int is perfect we have oo 00 n-| From the Kalman filter equations we obtain Pe K PY FRSFT+Q 10 o1 POHT HPS HT + Ry 10 0 1/2 = KulyP 0 6 01/2 FPEET+Q 3/2 1/2 4/2 3/2 Py HTTPS HT + Ry 10 at aft KoHPr | u 0 0 4/7 ‘We see that the guppy population estimation variance is 1/2 after one week ‘and 4/7 after two weeks.56 THE DScAETE TIE Kalen ALTER ©). In steady state 2.43 = ty 80 Th = Pret Gur (F-P)*Gug _f2 = |Tv ‘We sce that the ratio of the piranhe population to the guppy population in steady stave is 2:1 m 5.5 ‘The measured output of a simple moving average process is ye = 2k + 2hay where {2,} is zero-mnean white noise with @ variance of one, 1a) Generate a state-space description for this system with the first element of aq equal to 24: and second element equal to 2. b) Suppose that the initial estimation-error covariance is equal to the identity matrix. Show that the a pasteriori estimation-error covariance is given by we bps eee 4] ©) Find £ [lize —4f IJ] as a function of k. Solution: 1a). The state space description is given as wa ~ [2 dsr [2]an [to jare we ~ (0.Q) a= [55] Ue [1 Ljecto um ~ (0.R) 1b). Going through the Kalman filter equations we can derive a[-i 7] mil] ‘Then fj, can be derived as follows. Now suppose that Poa = FPZPT+QProsiens ST fia Fei lo ks Kat = Pigall” HP EH + BYP SE pee. “Rea ke Pia = Kea) Poy pei 1-1 ~ Real a 1 Since Fy has the specified form for k = 1, it also hes the specified form for 2. Proof by induction, QED Elite 273] = EAT [Cen af) (em — 8)" ]} = THLE lee - apex —2f)7]} = TPP) 2 rT 5.6 In this problem, we use the ausilisry variable Si = HARE HP 4 Ry. Note that to O]f Sem) LL SPS -Pruts || pcHp Pe oR Use the product rule for determinants to show that y+) _ [Pele IPgl= TST Solution: Using the given matrix equation and the product rule for determinants of Equa tion (1.48), we obtain WellSe— HaPe HET = |SeliPi) JPELRe| = |SeIIP We IFal eth = el = Fee QED 5.7 In Seotion 4.1, we saw that Dy, the covariance of the state of a discrete: time eystem, is given as Eii1 = FLEKFY + Qy. Use this along with the one-step ‘expression for Ue 6 priori estimation-error covariance of the Kalman filter to show that Sx—Fj" 2 0 forall . Given intuitive explanation for this expression [A nd)5B THe DSCRETE-TIE KALMAN FLTER Solution: We have Men = EFT + Qe Po FP FE — Fel HY (HaPy Hi + Ra) Ae Py FE + Qe Subtracting the second equation from the first gives Deen = Bea Poy PARE + FP HE (HPS HE + Re) 1HaPe FP We note that if Ay Is positive semidefinite and Re Is positive definite, then Aeya is positive semicefinite, So since A; ~ 0 (positive semidefinite) we see that Ag 2 0, ‘which in turn means that As > 0, etc, We conclude that x — Py > 0 for all & Intuitively this means that the covariance of the state is larger than the covariance of the state estimate, which mesns that the measurement always allows us to reduce our uncertainty in the state 5.8 Consider the system of Problem 6.1. a) Use the method of Section 5.4 to find a closed-form solution for Pe, as- suming that @ = 1, R= 5, and Po =0. b) Use your result from abave to find the steady-state value of Fr Solution: ‘a). From Equation (5.50) we obtain [2] ‘The cigendata of W are found to be 5/2 0 = [5 os] aya 4 w= [84 a4 lf 4 6 we [it] From this we obtain vt MA‘) al (6 2)-G (4 2] a]- elt5.9 prcoums 59 From this we obtain Suppose thet Kalman filter is designed for the systerm Fes = te fk = mtu % ~ OR) a) Suppose that (a3) = 1. Design a Kalman filter for the system and find « closed-form expression for PE. What is the limit of Py as & — 0? 1b) Now suppose that the trae process equation is actually te41 <2 + We where we ~ (0,9). Find a difference equation for the variance of the a priori estimation error if the Kalman filter that you designed in part (a) js used to estimate the state. What is the limit of the estimation-error variance as k — oo? Solution Since Pr = Pit = 1, we see that60 1Heascrere-rine KALMAN ene Wo see that jim Pr =0 bb). The a prion’ state estimate can be written as fi = A+ Kile 4) L = a+ peers) me eee ea BT 8 RST FT ‘The state at time (b+ 1) is given as ma Ste be Subtracting #4, ftom ens: gives fan = ane = tw (ph egg ~ ere men Taking the vosauce of bath sides gives . BY? ca R Fd) = (gEq) HOD +O+ Because of the @ on the right side of the above equation, we see that fim Ee 5.10 Suppose that @ Kalman filter is designed for a discrete LTI svstem with an assumed measurement noise covariance of R, but the actual measurement noise covariance is (+ AR). The output of the Kalman filter will indicate that the a priori estimation-error covariance is Pr, but the actual a priori estimation-error covariance will be Sz. Find a difference equation for Ay = (By — Fy}. Will Ax always be positive definite? Solutio From Equation (6.24) we obtain jy, = PU ~ Kell ay + PK where we have assured 1x = 0 for ease of notation. Given the system equations en = Pret we Hag tePromiens 61 where wx ~ (0,Q) and tx ~ (0, R}, we can obtain FU Kyle + we ~ PK gee Par = Blea) PUL ~ Ka \Pp(L ~ Kx) PT + Q + PKGRKE EE ‘This is the estimation-error covariance if the trae measurement noise covariance Is equal to R as assumed. If the measurement noise covariance is instaad equal to E+AR, then the estimation-error covariance is equal to Bz and is instead governed by the equation Sign = FU Ra ER UT — Ke) PT 4 Q4 PRR 4 ARKEET Subtracting the Po, equation from the Sz, equation gives Aen That — Pea FUL ~ Ky Hj Ag(0 — KyH)? FT + PRAARK] FT TF AR is not positive definite, then Ay might also not be positive definite. Computer exercises 5.11 Let pk denote the wombat population at time k and J, denote the size of the wombat’s food supply at time &, From one time step to the next, half of the ‘existing wombat population dies, but the number of new wornbats is added to the population is equal to twice the food supply. The food supply is constant exept for zero-mean random fluctuations with « variance of 10, At each time step the wombat population is counted with an error that has zero mean and a variance of 10. The initial state is mm = 650 fo = 250 ‘The initial state estimate and uncertainty is 600 Elo —m)?] = 500 fo = 200 El(fo—fo)"] = 200 Design a Kalman filter to estimate the population and food supply. a) Simulave the system and the Kalman filter for 10 time steps, Hand in the following. # Source code listing, ‘* A plot showing the true population and the estimated population as a function of time62 THE DcRETE-TME KALMAN eLTER ‘© A plot showing the true food supply and the estimated food supply as a function of time. ‘© A plot showing the standard deviation of the population and food supply estimation error as a function of time, ‘A plot showing the elements of the Kalman gain matrix as a funetion of time. 1b) Compare the standard deviation ofthe estimation error of your siraulation with the meady-state theoretical standard deviation besed on P'. Why is there such a discrepancy? ©) Run the simulation again for 1000 time steps and compare the experi mental estimation error standard deviation with the theoretical standard devietion, Solution a). The plots are shown in Figure 8.1 v9 eo po —— §|/ E exo (| — Minutes} § 220)) [strats 005 5 2005 5 1 * opuaion] | } an} tavetoy} 0a] z coy ii 29 Ka a | —— o cc vo 0 5 10 Figure 5.1 Sohition to Problem 5.11 1b). Depending on the simulated noise history, the standard deviations of the population and food supply estimation errors from the simulation are both around 15. The steady-state theoretical standard deviations that come out of the Kalman filter equations are 2.9 for population and 3.5 for food supply. ‘The reason for the discrepancy is the large initial estimation error and the short simulation time. ). When we run for 1000 time steps the standard deviations of the population and food supply estimation errors from the simulation are about 3.3 and 3.9 respectively. This is mich closer to the thearetical standard deviation because the simulation time is stretched out long enough that the initial estimation errors do not swamp the errors at the later times.proses 63 3.12 Consider the RLC circuit described in Problem 1.18 with R=, = 1. and 0.5. The input voltage is zro-mean, unity variance white noise. Suppose that the capacitor voltage is measured at 10 Hz with zero-mean, unity variance white noise, Design Kelman flier to estimate the inductor curvent, with am initial covariance Pf = 0. Generate a plot showing the a prior and a posteriori variances of the inductor curren: estimate for 20 time seps. Based on the plot, what is the steady-state value of Pf? Use the development of Setion 54.1 to approximate the eady-state value of PE using 1, 2,8, and 4 successive squares of the ¥ matrix. Solution Figure 5.2 shows the « priori variances of the inductor current estimate for 20 ‘time steps. Based on the plot it appears that Py converges to 1.9592. If we use Section 5.4.1 10 approximate the steady-state value of Pr, we obtain the following: pol 5367 p=2 15995 pad: Pr = 19936 pati: P= 1.9592 D 5 16 © 70 tee step Figure 5.2 Solution to Problem 5.12CHAPTER 6 Alternate Kalman filter formulations Problems Written exercises 6.1 _In this chapter, we discussed alternatives to the standard Kalman filter for- mulation. Some of these alternatives include the sequential Kalman filter, the information filter, and the square root filter. a) What is che advantage of the sequential Kalman filter over the batch Kalman filter? What is the advantage of the batch Kalman filter over the sequential Kalman filter? b). What is the advantage of the information filter over the standard Kalman filter? What is the advantage of the standard Kalman filter aver the information filter? c) What is the advantage of the square root filter over the standard Kalman filter? What is en advantage of the standard Kalman filter over the square root Kalman filter? Optimal State Batimation Selution Manuel, First Easion. By Dan J. Simon 65 006 Job Wiley & Sons, Ine66 ALTERUATE KALMAN FIER FORARLATIONS Solution: fa). An advantage of the sequential filter ig that it does not require any matrix: inversions. An advantage of the batch filter is that it can handle time-varying nondiagonal measurement noise covarianees. bb). An advantage of the information filter is that it requires less computational effort if the noise covariances sre constant and the number of measurements is much greater than the number of states, Also, we can represent 2¢t0 i formation exactly with the information flier, but only approximately with the standard Kalman fier. An advantage of the standard Kalman filter is thas it requires less computational effort if the number of states is greater ‘than the number of measurements. Also, we esn represent zero uncertainty exactly with the standard Kalman fier, but only approximately with the information filter. ). An advantage of the square root flter is that it results in twice as much numerical precision. An advantage of the sterdard Kalmen filter is that it requires less computationel effort. 6.2 Suppose that you have a system with the following measurement and mea surement noise covariance matrices You want to use a sequential Kalman filter to estimate the state of the system. Derive the normelized measurement, measurement matrix, and measurement noise covariance matrix that could be used in a sequential Kalman filter. Solution: First we compute the eigendata of R. The eigenvalue matrix # and eigenvector matrix § can be computed as 10 a= [os] Ly v2 v2 s= 31-8 v4] ‘The normalizeei measurement isproeys 67 6.8 Consider the two alternative forms for the information matrix timeupdate equation, What advantages does Equation (6.28) have? What advantages does Equation (6.20) have? Solution: Equation (6.28) guarantees that Zy will be symmetric positive definite, assuming that Z{_, is symmetric postive definite, However, Equation (6.28) requizes two nxn mrix inversions. IQs is constant, Equation (6.30) requires only one nx 1 matrix inversion. 6.4 A radioactive mass has @ half-life of + seconds. At each time step k the ‘number of emitted particles « is half of what it was one time step ago, but there is some error ws (zero-mean with variance Qe) in the number of emitted particles due to background radiation. At each time step the number of emitted particles is, counted with two separate and independent instruments. ‘The Instruments used to count the number of emitted particles both have a random ezror at each time step ‘hat is zero-mean with @ unity variance. The inivial uncertainty in the number of radioactive particles is a random variable with zero mean and unity variance. a) The discrete-time equations that model this system have s one-dimensional state and a two-dimensional measurement. Use the information filter to compute the @ prior’ and a posteriori information matrix at k= 1 and 2, Assume that Qo = 1 and Q, = 5/4 1b) Another way to solve this problem is to realize that. the two measurements ccan be averaged to form a single measurement with a smaller variance than the two indepencent measurements. What is the variance of the averaged measurement at each tine step? Use the standard Kalman filter equations to compute the @ priori and @ posterior covariance matrix at = 1 and k= 2, and verify that it is the inverse of the information matrix that you ‘computed in part (a) Solution: ‘The system model is given as al To yea tte 1 we = | y faetee where wy ~ (0, Qu) and vg ~ (0,1) '8). The information matrix equations are given in Equation (6.38), from which wwe obtain68 aLTenNeTe KaUwan FLTER FORMULATIONS 4 1b). Suppose we have two independent mneasurements of the scalar 2, each with variance R, Then the variance of the averaged measurement is computed as °{[in+m-4]'} - { [jer-2)hon-0) } B{ios/2+ w/a} RA R/A Rp In this problem each measurement has unity vaziance, so the variance of the averaged measurement is 1/2. We can use Equation (5.19) 10 obtain woe ta i et = [et eaten)” Note that the P and P* quantities obtained here are the iaverses of the Z> and Ty* quantities obtained in part (a) 6.5 Prove that the singular values of a diagonal matrix are the magnitudes of the diagonal elements,Prosieus 69 Solution: Suppose that P= disg(Ps, Pa). Then PTP = dieg(P2,.--,P2). The singular values of P are computed as o(P) = PPP) {IPali-++1Pal} QED 6.6 Prove that S$ is symmetric positive semidefinite for any $ matrix. Solution: For any compatibly dimensioned column vector 2 we have 278872 < yy = WI 2 0, where y= ST x. Since 27587 x > 0 for all 2, we see that 8S" is Positive semidefinite, Also note that (SS?)7 = (ST)TST ~ $80. so SST is also symmetric. QED 6.7 Find an upper triangular matrix $ (using only paper and pencil) such that gr [1 3 ss=[ 3 3] Is your solution unique? Solution: (@ S][% 2] Eauatiog the elements of the matrices on sight skes of the above equations gives Sie=3. Sip tard Sy = 0 _fou s-[2 3] 8 will also solve the problem, so the above solution is not unique, 6.8 Find an upper triangular mattix $ (using only paper and pencil) such that 5 2-2 ssP=] 202-1 2-21 How many solutions exist to this problem?70 _AcTemare KALMAN ILTER FORMULATIONS Solution: Sia Sa Sis] [Sr 0 0 Sh+Sh +82, SiaS2+ SisS2s SiaSe0 0 Se Sol} | Si Sn 0 | = | S2S22+SiSs StS SoaSaa 0 0 Sia Sx Sta Sie S953 | Equating the elements of the matrices on the right sides of the abave equations ges eight possible solutions. a1 0-2 #1 0 2 s=] 041 -J]or] oa 1 0 0 1 0 0-1 6.9 Verify Equation (6.70). Hint: Equate the two sides of the equation, take the tyace, and solve for 7. Make suze to explain why taking the trace is valid. Solution: Equation (6.70) says Tage" = (I~ ay907 * ‘This gives = 1 = 203687 + 0?97(007 0 = (ood? 298 — 2aye0" + aps” ‘The matrix on the right in the above equation bas a rank less than or eqtial to one {since @ is « column matrix), Therefore its eigenvalues are {0,--+.0,,2}. where p: night be nonzero. $o if we adjust to make the trace of the matrix 2er0 then all of the eigenvalues will be zero (since the trace is equal to the sure of the eigenvalues). ‘This will make j= 0 which will reduce the rank of the matrix to zero, thereby rmalking the mattix equal to 0. Taking the trace of the matzix on the right in the above equation gives (097 $)?94 — 2nd? dy + add agton? 27 +1 (aRie 19? + 29-1 ake = POH 4 tWRaa = 1-9 yeqaRe = 1 QEDProsicus 71 6.10 Suppose that an orthogonal matrix 7’ is desired to satisfy Equation (6.97), where Cholesky factorization is used to compute the matrix square roots on the left Side of the equation. This equation can then be written as U = TA, where U is an upper triangular matrix. Show that such a transformation cannot be found unless ‘the two-norm of the first column of A happens to be equal to [Ui |. [Note that this does not necessarily prevent the possiblity of the transformation of Equation (8,97), because U’ could be nontriangular if nontziangular square root matrices are used io form the U matrix] Solution: We have U; = 7.4), where Us and Ay are the fist columns of and A. Since F is orthogonal, 777 = J. Since U is upper triangular. Uy =[ Ur 0 ol. This means that ft, = Pay rag Uh = ATTA, ATA; Wal = flAlla QED 6.12 te he Hone td aig ely popes ad pe) wf an 1a 22 o1 22 Solution Following the algorithm in Section 6.3.5.1 gives a= 3 a = ie vw) = [420 2]7 we ta 3-3 @. | 0 0 A a o 1 0 0 ete & = w® = [011 0]? (272 ALTERNATE KALMAN ELTER FORMULATIONS ‘oe ae a o 0 = r= [23] T= (Pfau uF — Gulu?) -1/3 -2/9 0-2/8 o o 1 0 28 2/3 0 1/3 2/8 1/8 0 2/3 8 6.12 Use the modified Gram-Sehmidt method (using only paper and pencil) to solve Probiem 6.11. Solution: Following the algorithm in Section 6.3.5.2 gives a = 3 m = [3 3] q = [1/3 4/3 0 4/3) AP = [oo 10] a= 1 wm = [01] h = [001 0) m= [1000] r= [0100] Th - [9010] th = [0001] Now we perform a standard Gram-Schmids orthonormalization procedure om the last four rons of T (Le., Ts through To) Ty = Ty-(GTP)T~ (ATF Te {ao -2/9 0 -2/9] Th = Ti/iTlle [23/3 -1/3v3 0 -1/3v8] Ty = Ta~(UaTP YD ~ ATP Ta — (TZ IT = [0 1/2 0 3/2) Ty = TilTlle = [0 va2 0 -v3/2)promens 73 Since we have four nonzero rows of T the algorithm is complete, 13/8 dB 0 o 1 0 2v3/3 -1/3V2 0 -1/3v2 0 v3/20 0 -V3/2 TT 6.13 Compute the U-D factorization (using only paper and pencil) for the matzix [33] P uput 1uz][ai o]f1 0 ob jl 0 de} fae 13) _ [dutubdes sede 30) > | unde dee Equating elements inthe two matrices above gives diz = 9. uiz-= 1/8, and di [5] ~ " Computer exercises 6.14 Consider the RLC circuit of Example 1.8 with R= 100 and L = C= 1 Suppose the applied voltage is continuous-time, zero-mean white noise with a stan- dard deviation of 3. ‘The initial capacitor voltage and inductor current are both zero. Discretize the system with a time step of 0.1. ‘The discrete-time measure: ‘ments consist of the capacitor voltage and the inductor current, both measurements containing zero-mean unity variance noise. Implement a sequential Kalman filter {or the system. Simulate the system for 2 seconds. Let the initial sate estimate bbe equal to the initial state, and the initial estimation covariance be equal to 0.11. int: Set the discrete-time process noise covariance Q = QeAt, where Qe is the co- variance of the continuous-time process noise, and Af is the discretization step size, will be nondiagonal, which means you need to use the algorithm in Section 2.7 to simulate the provess noi 1a) Generate a plot showing the « priori variance of the capacitor voltage estimation error, und the two a posteriori variances of the capacitor voltage estimation error. b) Generate # plot showing a typical trace of the true, a posterior estimated, find measured capacitor voltage. What is the standard deviation of the capacitor voltage measurement error? What is the standard deviation of the capacitor voltage estimation error?TA ALTERNATE KAMAW ALTER FORMULATIONS Solution: a). Figure 6.1 shows the variance of the capacitor voltage estimation error. b). Figure 6.2 shows the true, estimated, and measured capacitor voltage far typical simulation. Your results may vary depending on the noise history ‘that you realized. The measurement error hss a standard deviation of 1. The estimation error has a standard deviation of about 0.3, ageacesaace oent = figure 6.1 Solution to part (a) of Problem 6.14 copactorvotege a ary 5 04 as Pose eee eee oe Figure 6.2 Solution to part (b) of Problem 6.14proaeus 75 6.15 The pitch motion of an aircrafi fying at constant speed can be approxi- mately described by the following equations (SteO4} = [708680 17.9800], f 9.1760 0.1750) | 37.9800], = [1.0000 22870 0.0010 -0.0010 1.2370 | Ut) = ate) +u where a1 is the pitch rate, 29 is the angle of attack, u consists of the elevator and flap angles, and w isdisturbance due to wind. Suppose that the variance of the wind Gisturbance is 0.001, and the measurement variances are 0.3. Discretize the system with a step size of 0.01 and simulate the system and a square root Kalman filter for 100 time steps, Use an initial state of zero, an initial state estimate of zero, an initial estimation-error covariance of 0.01, and a control input of zexo. Hint: Set the discrete-time process noise covariance Q = Q.At, where Qe is the covariance of ‘the continuous-time process noise, aud At is the discretization step size. @Q will be niondiagonal, which means you need to use the algorithm in Section 2.7 to simulate the process noise. 8) Generate « plot showing the a posterior’ variance of the estimation errors of the two states b) Generate a plot showing a typical trace of the true, a posteriori estimated, ‘and measured pitch rate. ‘What is the standard deviation of the pitch rate measurement error? What is the standard deviation of the pitch rave estimation error? ¢) Generate 2 plot showing a typical trace of the true, a posteriori estimated, ‘and measured angle of attack. What is the standard deviation of the angle of attack measurement error? What is the standard deviation of the angle of attack estimation error? Solution: 4). Figure 6.3 shows the variance of the estimation error of the two states, ). Figure 6.4 shows the true, estinnated, and measured pitch rate for « typical simulation, Your results may vary depending on the noise history that you realized. The measurement error has a standard deviation of VO3 ~ 0.55. ‘The estimation error has a standard deviation of about 0.19. €). Figure 6.5 shows the true, estimated, and measured angle of attack for o typical simulation. Your results may vary depending on the noise history that you realized. The measurement error has a standard deviation of V0.3 = 00.55. ‘The estimetion error has « standard deviation of about 0.15,76 ATERUATE KALMAN FLTER FORMULATIONS OCUeSC SC te Figure 6.3 Solution to pact (e) of Problem 6.16 a eSCSSCO Figure 6.4 Solution to part (b) of Problem 6.15,engi latch Figure 6.5 Solution to part (c} of Problem 6,15 Proa.sus nCHAPTER 7 Kalman filter generalizations Problems Written exercises TL Consider the scalar we att Wk Zk + UE ptt Get where wy ~ (0,Q) and & ~ (0.Q¢). Let Q = Qe = 1, a) Design a Kalman filter in whieh the dynamics of the messurement noise tj are ignored and it is assumed that vy is white noise with a variance of Qc Based on the incorrect Kalman élter equations, what doc the Kelmen filter think that the steady-state a posterior estimtion covariance is? b) Based on the incorrect Kalman filter equations, what is the true stendy= state a posterior’ estimation covariance E(ez)? Hint: Find a recursive equation for Bef) in terms of Bief_,), E(wj_y}, E(u). and E(ex—1%), then solve for the steady-state value of E(ef) Optimal State Bstimatim Solution Monnal, Fist Etion. By Dan J. Sino {©2005 Joka Wiley & Sons, Ine 79