USB-C battery pack power stage design

This is my capstone project in Coursera's Power Electronics Specialization from University of Colorado at Boulder, wherein I design the switching power delivery and battery charging circuit.  The product requirement is for 3-cell LiPo in series (which will range from 9.6 V to 12 V, with an equivalent series resistance R_bat = 50 m𝛀) to work in the following operating points:

1. Provide power to USB bus, up to 2 A at V_bus = 5 +/-0.1V (therefore with effective R_bus = 2.5 𝛀), when V_bat = 12.6 V.
2. Provide power to USB bus, 3 A at V_bus = 20 V (therefore with effective R_bus = 6.7 𝛀), when V_bat = 9.6 V. 
3. Charge the battery pack (which is at V_bat = 11.1 V) from the USB bus, drawing up to 3 A at 20 V.  Since R_bat = 50 m𝛀 and we want 3A going to the battery, the supply side should be higher than V_bat by 150 mV, so we need D V_bus = 11.1 + 0.150 = 11.25, so D =  11.25 / 20 = 0.56.

In all cases, the given switching frequency Fs = 1/Ts = 100 kHz.

I used Omni Graffle for idea sketch, and LTSpice IV for the schematic capture and simulation (after reading this book on LTSpice).

Switch design

Since the battery voltage will be higher than the 5 V (the output voltage in mode 1) but lower than 20 V (the output voltage in mode 2), neither the pure step down (buck) nor step-up (boost) converter will do the job alone; I need a buck-boost converter, which will work like the buck converter if Vbat > Vbus, but like the boost converter when Vbat < Vbus, as you can see below.

Let the switch duty cycle D is the portion of the time (out of one switching period Ts) that the switch is in position 1.  Then:
Left: lossless buck converter duty cycle D = Vbus / Vbat = 5 / 12.6 ≈ 0.4

Right: lossless boost converter duty cycle (1-D) = Vbat / Vbus = 9.6 / 20 = 0.48 => D = 0.52

  In both cases, the average current flows from left to right (Vbat to Vbus).

If you combine the 2 of them in series, using the same inductor in the middle, then I get a non-inverting buck boost convert converter, shown below.

Unlike the inverting buck-boost converter, the 2 switches do NOT move together at the transition of the duty cycle in a switching period; one of the switches remain in one position depending on the mode (step up/down) while the other switch does all the swiching.

Because of the duality of buck and boost (step down and step up), I can keep the switches in the boost mode when charging the battery from the bus (whose voltage is guaranteed in this problem to be higher); to visualize this, note the perfect equivalence with the buck converter if I keep S_buck  closed and switch S_boost between positions 1 and 2 in the picture below.

When charging the battery, Vbus becomes the input voltage and Vbat becomes the output voltage of a buck converter flipped on the back.  The average current flows from right to left, and the switch duty cycle D is the complement of the ratio Vbat / Vbus = 11.1 / 20 = 0.555; that is, D = 0.445.

Note that the switch setting for S_boost is the opposite of S_buck, so Vbat / Vbus ratio during charging is not the usual duty cycle ratio, but the complement of that: D'.  To prevent downstream confusion, let's summarize the rough output voltage ratio in the 3 operating cases:

1. Buck mode: Vout = D Vin
2. Boost mode: Vout = Vin / D'
3. Charge mode: Vin = D' Vout

Ignoring losses, in the buck converter mode, the inductor current ripple 𝚫i_L ≈ (V_bat - V_bus) D Ts / 2L.  Using the given operating point values in this mode (operating point 1 in the intro),

𝚫i_L ≈ (12.6 - 5) 0.4 / (2 100k L) ≈ 15E-6 / L

Another way to look at this is to compare against the average inductor current, which must equal the average current to the load: i_L ≈ 2 A.  The ripple percentage is then 𝚫i_L / I_L ≈ 15E-6 / L / 2 = 0.75 E-6 / L.  To hold 𝚫i_L within say 10% of the steady-state current then, L must be greater than 0.75 E-6 / 10% = 7.5 𝛍H.

And again ignoring losses, in the boost converter mode:

• The voltage ripple at the the output of the converter 𝚫v = V_bus D Ts / (2 R_bus C), where C is the capacitance in series with the load.  If we want 𝚫v < 0.1 V, then C must be larger than V_bus D Ts / (2 R_bus 𝚫v) = i_bus D Ts / (2 𝚫v) = (3 A) (0.52) (10 𝛍s) / (2 * 0.1) = 78 𝛍F.
• 𝚫i_L ≈ V_bat D Ts / L which for 10% ripple target means the inductance must be larger than V_bat D Ts / (10% 3 A) = 166 𝛍H.

In charging mode, S_boost is actually working like a buck switch, so the current ripple analysis can be repeated with voltages flipped numbers: 𝚫i_L ≈ (V_bus - V_bat) D' Ts / 2L.  Note that the duty cycle is the complement of the usual buck converter duty cycle, because I am using S_boost as the buck converter.  For operating point 3, the ratio of the inductor current ripple 𝚫i_L/I_L then work out to (20 V - 11.1 V) 0.555 / (2 10% 100k L).  To keep this down below 10% requires L greater than (20 - 11.1) 0.555 / 20k = 247 𝛍H! The inductor is getting bigger and bigger!

(Solid state) switch design

To implement this schematic with the usual power electronics semiconductor devices, I need the devices Q1~Q4:

The "Q" just means some kind of semiconductor device, rather than a transistor necessarily.

All switch implementation that meet the following currents and voltages across the switches are permissible.  In each mode, I have to be careful about the voltage that a switch must block while it is open.

Current flow and voltage across the switches during the D Ts and D' Ts phase of the buck mode

Current flow and voltage across the switches during the D Ts and D' Ts phase of the boost mode

Current flow and voltage across the switches during the D Ts and D' Ts phase of the charging mode

From these diagrams, the following requirements can be placed on the Q1~Q4 and a natural choice for the implementation:

• Q1: active +/passive- current, but only + voltage ==> MOSFET in parallel with a diode
• Q2: passive - current, but only + voltage ==> diode
• Q3: active +/passive - current, but only - voltage ==> MOSFET in parallel with a diode
• Q4: active +/passive - current, but only + voltage ==> MOSFET in parallel with a diode

MOSFET choice

I choose Infineon BSC100N03MS as the MOSFET, because it was used in the instructor given examples.  According to the data sheet, the drain to source voltage (the blocking voltage) is 30 V, which is sufficient for the maximum of expected 20 V applied on the USB bus end.  It is capable of conducting 44 A nominally, and 176 in a pulse.  My expected gate-source voltage is 20 V, which is much larger than V_in (which is 12.6 V max).  In short, it has adequate voltage and current margin for the problem at hand.

S_buck implementation using diodes (and a MOSFET)

Q1-Q2 pair that comprises S_buck has only 1 active element (MOSFET) between them, so the control is rather straight-forward (at first), like this:

In S_buck, Q1 consists of fast recovery diode (D3 in this schematic) in anti-parallel with a MOSFET, and Q2 is just another diode.  (I am not sweating over the optimal part selection here; just grabbed any diode previously used in the class.)  The MOSFET conducts the actively turned-on + current, and the passively conducts the negative current in the other direction.  The diode on the right blocks the positive voltage at the source of the MOSFET when it conducts, but passively conducts the negative current (flowing from the bottom to the top, or ground to the inductor) when the MOSFET is turned off.

But since the MOSFET is not grounded, the gate will be floating, so we need a floating gate driver to supply enough current to the gate (to turn it on, or AKA close it) when commanded (c_buck in the schematic below).  The floating gate driver in turn needs floating (i.e. isolated) power supply.  In this class, a low power isolated unregulated DC-DC converter is recommended for use.

S_buck, with MOSFET gate driver powered by a floating unregulated DC-DC power supply.  Note Vss pin of the gate driver is connected to the SOURCE (downstream of the current) side of the MOSFET.  If you get this wrong, the MOSFET will always be on!

The isolated DC-DC converter (U8 above) in turn will get the power straight from the battery, and provide V_outP − V_outM = n V_in, where I choose n = 1 (can be between 1 and 2).

Note the resistor between the MOSFET gates and their drivers is there to limit the inrush current into the gate, and should be chosen for the particular MOSFET.  But conversely, we want to remove the gate charge from the MOSFET as quickly as possible during the turn off transition--again as much as the devices can tolerate.  So in some examples, I found a Schottky diode (which is a very fast diode with little voltage drop) shorting out the current limiting resistor (as shown below), to effectively remove the current limit during the turn off transition.  To be honest, I still don't understand why it's OK to remove current limit when removing the gate charge from the MOSFET, while a current limiting resistor is required when adding it.

S_boost implementation using diodes

S_buck starts out as an exact dual of S_buck in that it can pass the average current in either direction: through M_boost-D_{boost_Dp} pair during boost mode, or through M_chg-D_{chg_Dp} pair during charge mode.

During the boost mode (step up power supply), M_boost-D_{boost_Dp} work in a complimentary fashion to keep the average current flowing from left (Vbat) to right (Vbus).  During battery charging mode, M_chg-D_{chg_Dp} work together to keep the average current flowing from right (Vbus) to left (Vbat).  During buck mode (step down power supply), M_chg is always open to keep the current flowing only through D_{boost_Dp}.

Of course, the gate drivers are necessary to drive the MOSFETs.

BUG ALERT: I should have connected outM pin of Uchg_dd to the SOURCE pin of Mchg, but instead connected it to the drain pin.

MOSFET logic gate coordination

In this realization of the power stage, there are total of 3 MOSFETS that have to be turned on/off in a coordinated manner.  Let's enumerate their required on (closed) and off (open) states during the 2 phases in each mode:

Mode	Step down (buck)		Step up (boost)		Charge Battery
Phase	D Ts	D' Ts	D' Ts	D Ts	D' Ts	D Ts
Mbuck	1	0	1	1	0	0
Mboost	0	0	0	1	0	0
Mchg	0	0	0	0	0	1

The assignment provides 2 independent control signals: Vcontrol1 and Vcontrol2.  To control all 3 switches with just these 2 signals, I must reduce the above truth table to use only 2 inputs.  Digital logic reduces inadvertent switching action from noise, and I can generate the following 2 digital signals from Vcontrol1 and Vcontrol2, using the digital logic threshold voltage V_high/2 = 2.5V:

1. Boost = V(control1) > V_high/2
2. Charge = V(control2) > V_high/2

Since the HW recommended a 5 V digital logic, I am using 2.5 V as the threshold.  I still need a rapidly switching PWM from these 2 control voltages, so I can derive the PWM signals for the 3 modes like this:

1. For step-down mode, PWM(Vcontrol1), with VM=V_high/2, so that M_buck duty cycle will be continuous (D = 1) when going from the buck mode to the boost mode.
2. For step-up mode, PWM(Vcontrol1 - V_high/2)
3. For charge mode, PWM(Vcontrol2 - V_high/2)

In cases 2 and 3 above, I need a reference voltage and a subtractor.  I am allowed to use an IC to generate a precision reference voltage (despite the irony of having to use a voltage reference in a DC-DC converter design); the LT1121-5 (the "-5" in the part name is for 5 V output) was recommended, and I use a voltage divider to derive a 2.5 V from it.  With a 1% resistor, I know that the V_high/2 reference can be 2 % off (if one resistor is 1% higher and the other is 1% lower), but since the design does not require operation near 100 % duty cycle, I'll be OK.  The subtraction of the V_high/2 reference voltage can be done with an op-amp with sufficiently high open-loop gain.  I just used the instructor recommended part LT1498 (which has a rail-to-rail input-output, 10 MHz gain-BW product, and 6 V/us slew rate), as you can see below, where V(Ctrl1_2V5) = V(control1) - Ref2V5.

This subtractor is going to suffer from analog issues (like part and temperature variance).

The reference voltage takes quite a few microseconds to settle, and the reference's ability to track half the 5 V from the voltage generator is somewhat disappointing.

The subtracted signal is then fed into a PWM generator with VM=V_high/2.  The truth table for the MOSFETs can then be implemented with this mixed signal logic:

• duty(M_buck) = NOT(control2 > V_high/2) AND (control1 > V_high/2 OR PWM(control1))
• duty(M_boost) = NOT(control2 > V_high/2) AND control1 > V_high/2 AND PWM(control1 - V_high/2)
• duty(M_chg) = control2 > V_high/2 AND PWM(control2 - V_high/2)

To satisfy both the truth table and the MOSFET duty cycles for the operational points, control2 should be clearly be < V_high/2 during buck and boost modes.  ANDing the digital signal with PWM's output is necessary because achievable minimum duty cycle may not be 0.  The logic IC implements the "> V_high/2" operation internally, so I can simplify the above logic expression:

• duty(M_buck) = NOT(control2) AND (control OR PWM(control1))
• duty(M_boost) = NOT(control2) AND control1 AND PWM(control1 - V_high/2)
• duty(M_chg) = control2 AND PWM(control2 - V_high/2)

Here is the implementation of duty(M_buck), duty(M_boost), and duty(M_chg) that uses the 5 V and "2.5 V" explained above.

At first, I use hard coded values for Vcontrol1 and Vcontrol2 to simulate the converter response, so ideally, I want 3 distinct time periods to exercise the 3 operational modes of the converter.  That is, I want each of the 3 duty cycles to not overlap.  Ignoring noise, it looks like I will be able to do that.

Excessive power loss through the diodes

If I drive the converter in buck mode with duty(M_buck) shown above, I can see the expected response of a buck converter.

The current spikes through the MOSFET at the beginning of each  MOSFET turn-on is many times the inductor current, and is on the order of maximum current spike specified in the MOSFET and the diode data sheet.

The output voltage is much lower than the required 5 V, because of the forward voltage drop of 2 diodes in the forward path (1 in S_buck and 1 in S_boost): 0.85 V each.  2 A nominal current through 1.7 V drop comes out to 3.4 W, which is 34 % of the power delivered to the bus (2 A at 5 V) in the step-down operation mode: clearly unacceptable as a high efficiency switching converter, so back to the drawing board.  To remove the diodes in the current conducting paths, I need a complementary MOSFET, arranged with a dead time IC.

Switch implementation without diodes

From the switch current flow and voltage digram given at the outset, it seems that Q3 and Q4 can each pass current in either direction and block the voltage in a single direction.  Therefore, I only need to replace the diode I had used in Q2 with another MOSFET that turns on during the D' interval of the buck mode.  Here is the modified switch implementations that can handle all 3 operational modes.

MOSFET driver for Mbuck_DP does not need a high side voltage supply, because Mbuck_DP's source is connected to the ground.
Except for Mbuck_DP, all MOSFETs can pass current in either direction.  I control that by designing the gate control signal carefully.

In this design, 100ns dead-time is used everywhere.  Whenever the MOSFET source is grounded, the high side gate power supply is unnecessary, so the diode replacement MOSFET is somewhat simpler than the high side.

Now that the MOSFETs are doing the double duty, I have to expand the MOSFET truth table to consider the MOSFETs that are complementary to the existing MOSFETs.

	Step down control1 = 0 control2 = 0		Step up control1 = 1 control2 = 0		Charge control1 = 0 control2 = 1
MOSFET	D Ts	D' Ts	D Ts	D' Ts	D Ts	D' Ts
Mbuck	1	0	1	1	1	1
Mbuck_DP	0	1	0	0	0	0
Mboost	0	0	1	0	0	1
Mchg	1	1	0	1	1	0

The "x" in the truth table means "don't care"; which can be helpful in simplifying the digital logic to implement the truth table:

• duty(M_buck) = control2 OR control1 OR pwm(control1)
• duty(M_buckP) = NOT(control1) AND NOT(control2) AND pwm'(control1)
• duty(M_boost) = (control1 AND pwm(control1 - V_high/2)) OR (control2 AND pwm'(control2 - V_high/2))
• duty(M_chg) = (NOT(control1) AND NOT(control2)) OR (NOT(control2) AND control1 AND pwm'(control1 - V_high/2)) OR (control2 AND pwm(control2 - V_high/2))

where I use the prime notation (pwm') to indicate the complement of the duty cycle.  Scanning the above Karnaugh chart, it appears that I need both of the boolean signals for control1 and control2.  So it's just easier to factor out those common digital logic circuit for   reuse.

The 5V and 2.5V reference voltages are shown in a separate part of the schematic:

The NOT(control1) and NOT(control2) are implemented with an off-the-shelf inverters.

Then the 2.5V bias subtracted voltages are derived from Ref2V5 and off-the-shelf op-amps.

The duty cycle and the complementary duty cycle pairs for the buck, boost, and the charge mode are generated by the PWM-deadtime IC pairs; the only differences are the voltage level being commanded.  Here, for example, is the PWM pairs for the boost mode:

I am using the same dead-time of Td = 100ns for all PWM.

Finally, the 4 duty cycles can be generated from logic gates.  The most complex logic is the 3 AND gates being fed to an OR gate for duty(M_chg), but the benefit of carefully building up the intermediate signals pays off.

LC filter to damp out voltage ripple

The original power stage design  shown above has only one inductor, and a cap each on the input and output ends.  There are a few problems with this approach: to reduce the input and output voltage ripple requires huge inductance and capacitance, which in turn--since the natural frequency of LC tank is sort(LC)--slows down the transient response.  And for some reason, the LiPo battery model given for the project neglects the naturally huge (>> 1 mF) capacitance.  If such a large capacitance is modeled, the LC tank would slow down the response unacceptably during the transient to the charging mode.  Reducing the L is desirable for speeding up the transient and keeping the part cost down, but the current ripple at the inductor might go above the saturating inductor current.

I think one way to work around the physics of the problem is to put a cascaded LC filter on the input and output side of the power stage, as you can see in this schematic:

L2-Cout1 damps out the switching frequency ripple.  L2 can easily be an order of magnitude smaller  than the main  inductor in the power stage.

By using a small inductor-capacitor combination, the high frequency (at the switching frequency Fs) V_bus ripple can be reduced with negligible impact on the slower dynamics of the inner power stage circuit.  I want to do the same thing on the input end, but the assignment allows at most 2 inductors, so I held back.

Inductor design

In this assignment, the Ferroxcube 3F3 material (ferrite) is suggested as the core material.  Ferrite cores have the H-B (electric field intensity-to-magnetic field density) nonlinearity depicted below:

3 core material properties for the 3F3 are:

• Saturation flux density B_sat 0.33T ~ 0.43T, between 25 ºC ~ 100 ºC
• Remnant flux density B_r 0.12T
• Coercive force H_c ~ 12 A/m (relatively temperature insensitive)

Choosing the inductance value

At first, I tried to use L1 = 247 𝛍H calculated above, and go through the core geometrical constant method (the "K_g" method explained in Chapter 14 of the course textbook that I follow in the next section) is used to design the filter inductor L1.  But doing that, I wound up with a HUGE inductor (one that weighs hundreds of grams).  A USB charging IC is selling for less than $1 at Digikey, so I reasoned that a huge inductor that is an order or magnitude or more expensive than the power management IC is a bad design, and looked for ways to reduce L1.  The cascaded LC filter at the USB bus output end I explained earlier.  I read the rubric, and actually did not find a requirement for the 10 % current ripple on L1, so I empirically arrived at the L1 and L2 value that met the requirements. 

"K_g" method 

The step-by-step is to enumerate the requirements and constraints on the inductor:

• Winding material resistivity 𝛒 Assuming copper at 100 ºC, 2.3E-8 𝛀m.
• Desired inductance: L1 = 10 𝛍H, L2 = 1 𝛍H.
• Maximum current I_max looks to be about 30 A for L1 (the main inductor) and -30 A for L2 (the isolation inductor).
• B_max < B_sat, which is a material property of the magnetic core.  For ferrite core operating rather hot, B_sat = 0.4 T feels reasonable, so B_max = 0.3 T.
• Fill factor K_u = 0.5 if using not-so-thin wire.
• R should be determined by acceptable copper loss = I²_rms R.  Let's say I accept 0.5 W copper loss (1 W through L1 and L2) at 3 A boost or charge mode.  Then maximum R = 0.5/3² = 56 m𝛀 for each inductor.

The initial guess for K_g is (𝛒 / R K_u) (L I_max / B_max)², which works out to 8.2E-3 cm⁵ for L1, and 0.082E-3 cm⁵ for L2.  Appendix D of the course textbook lists some widely used standard ferrite cores.  Among them, the pot core type 1811 and 905 have K_g close to the values calculated above for L1 and L2 (9.4E-3 and 0.183E-3, according to the table below), and weigh 7.3 g and 1.0 g, respectively.

There are other core design that will also meet the K_g requirement (like the EE core), but since I don't yet understand the subtleties of core material choice, I am just going with the 1st choice.

The next step is to calculate the required air gap l_g (inductors store most of the magnetic energy in the air gap) with this formula: l_g = L (𝛍₀/A_c) (I_max / B_max)², where 𝛍₀ is the air's magnetic permeability 4𝛑 E-7 and A_c is the core's cross sectional area (0.433 cm² and 0.101 cm² in the above table).  Using the values for L1 and L2 above, I get 2.9 mm and 1.24 mm for inductors L1 and L2.

Next, N--the number of windings around this core--is calculated with N = (L I_max ) / (B_max A_c).  I get 23.1 for L1 and 9,9 for L2, so just round them to 23 and 10, respectively.  These correspond to  A_L = L 10⁹ / N²--inductance per turn that can be measured while grinding down the core post to create the air gap--of 18.7 nH and 10.2 nH for the 2 inductors.  A sanity check at this point is to approximate (assuming core permeability 𝛍_c >> 𝛍₀) the magnetic flux density B as ≈ 𝛍₀ (N / l_g) I; i.e. proportional to the current through the winding and inversely proportional to the air gap.  For L1 and L2 calculations made so far, B then approximates to 10 mT times the current, so that at 30 A, B < 0.3 T.

Next, the winding wire cross area A_wire should be less than K_u W_A / N, where W_A is the core's window area (bobbin winding area in the above table): 0.187 cm² and 0.097 cm² for the 2 cores chosen.  So maximum A_wire comes out to 4E-3 cm² and 1.7E-3 cm² for L1 and L2.  In the same Appendix D of the course textbook, table D.6 lists the AWG (American Wire Gauge) specs.  From this table, I picked out AWG #22 (A_wire = 3.243E-3 cm²; diameter 701 𝛍m) and AWG #25 (A_wire = 1.6E-3 cm²; diameter 505 𝛍m) as being reasonably close to the maximum A_wire calculated above.  To check that these wire chose does indeed meet the target copper loss, calculate R_ser = 𝛒 N (MLT) / A_wire, where MLT is the mean length per turn for the cores given in the above table: 3.71 cm and 2.90 cm for the 1811 and 1408 respectively.  So R_ser comes out to 60 m𝛀 for L1, and 27 m𝛀, which are reasonably close to the R_ser sought at the beginning.  For convenience, here is the comparison of the 2 inductors:

	"Kg method" formula	L1	L2
Target inductance L [𝛍H]	Input	10	1
Target serial resistance Rser [m𝛀]	Input	56	56
Target Kg [cm5]	(𝛒 / R Ku) (L Imax / Bmax)2	8.2E-3	0.082E-3
Off-the-shelf Kg [cm5]	Textbook Table D.1	9.4E-3	0.183E-3
Ferrite pot core size [mm2]	Textbook Table D.1	18x11	9x5
Core weight [g]	Textbook Table D.1	7.3	1.0
Cross section area Ac [cm2]	Textbook Table D.1	0.433	0.101
Winding (window) area WA [cm2]	Textbook Table D.1	0.187	0.034
Magnetic path length lm [cm]	Textbook Table D.1	2.6	1.26
Air gap lg [mm]	L (𝛍0/Ac) (Imax / Bmax)2	2.9	1.24
Number of winding N	round((L Imax ) / (Bmax Ac))	23	10
Inductance per turn AL [nH]	L 109 / N2	18.7	10.2
Bscale [mT/A]	𝛍0 (N / lg)	10	10
Maximum Awire [cm2]	Ku WA / N	0.187	0.097
AWG chosen	< Maximum Awire	#22	#25
AWG Awire [cm2]		3.2E-3	1.6E-3
AWG diameter [mm]		0.7	0.5
Winding resistance Rser [m𝛀]	𝛒 N (MLT) / Awire	60	27

In the schematic, I now replace the numeric value of L1 and L2 with SPICE model that includes H_c, B_r, B_s, A_c, l_m, l_g, N, and R_ser:

• L1 node1 node2 Hc=12 Br=0.12 Bs=0.33 A=43u Lm=26m Lg=2.9m N=23 Rser=60m
• L2 node1 node2 Hc=12 Br=0.12 Bs=0.33 A=10u Lm=12.6m Lg=1.24m N=10 Rser=27m

Here is an example of entering the SPICE line into the inductor component in LTSpice (^ + right click on the inductor component):

SPICE works in MKS unit, so some mental conversion from the above table is required.  I actually found that the "Value" line would not be deleted, so I wrote the SpiceLine on the Value line instead.

And here is the result of simulation that uses the non-linear, lossy inductors thus specified:

Vbus and Ibus settles quickly to 5 V/2 A and the 20 V/3 A steady state conditions for the buck and boost modes, and the maximum inductor current stays below the saturating current.

Extra feature: current limiting

Although not plotted above, I found that the current spikes on the battery was approaching 200 A--clearly undesirable (and also physically impossible, due to the huge capacitance that should normally accompany a regular LiPo battery), so I came up with  current limiting circuit.  The current sense chip I've been drawing above (but not utilizing yet) only detect current in the forward direction, so I threw in another one, like this:

Vcs measures the current in the forward direction, and Mcs measures in the opposite direction.

I then low pass (1 pole) it before throwing that into a current limit comparator:

The comparator has a large input impedance, so should not load the current sense chip.

The opposite direction works exactly like above, except for using Mcs as the input.  The current limit (iLlim) is hard wired to 90 % of 5 V, or 4.5 V, which should means I will cap the limit to 4.5 V / (20 x 0.01 𝛀) = 22.5 A, which is well below the 30 A saturation limit picked for the main inductor above.

To actually turn off the MOSFETs correctly when the current exceeds the threshold, the digital logic must be changed as follows:

Buck stage complementary signal pair modified for current limitation.

Boost stage complementary signal pair modified for current limitation.

I verified in LTSpice that the battery current is now capped at around 25 A, and the inductor current in either direction is also capped at about the same value.

Understanding the UKF SLAM in Matlab

In my previous post, I worked through a 2-D EKF SLAM (Simultaneous Localization and Mapping) homework assignment for the online course by Prof. Cyrill Stachniss.  In this blog entry, I get to solve the same problem using the UKF (Unscented Kalman Filter).  Being the same non-linear problem, the state formation and the equations of forward process evolution and observation are the same as in the EKF SLAM case.  It reading the exposition below, it might be helpful to pull up my previous post next to this post.

Prediction step

In EKF, I simply fed the current state estimate 𝛍^-[k] (and the external input u[k]) to the nonlinear function g(u[k]; 𝛍⁺[k-1]) to predict the next state:

𝛍^-[k] = g(u[k]; 𝛍⁺[k-1])

But in UKF, I have to go through the UT (Unscented Transform): generate an ensemble (called sigma points 𝛘⁺[k-1] in section 3.4.1 of the Probabilistic Robotics book) of 𝛍⁺[k-1], and feed all those points to g().  Then 𝛍^-[k] is the mean and the covariance of the transformed ensemble are the  new 𝛍^-[k] and 𝚺_𝛍𝛍^-[k].

Sigma points 𝛘

The ensemble 𝛘⁺[k-1] should be centered around 𝛍⁺[k-1], with enough "spread"--based on 𝚺_𝛍𝛍⁺[k-1].  𝛘⁺₀[k-1] is the current estimate 𝛍⁺[k-1].  Subsequent points are some + and - direction along each of dimension of the uncertainty.  I visualize this as the pair of points roughly 1~2 standard deviation on either side of 𝛍⁺[k-1].  A pair of points for each of the state dimension n produces 1 + 2n sample points, as you can see in the left ellipsoid below, which is rotated around the principle (x-y) axis because of non-zero cross-covariance term in 𝚺_𝛍𝛍⁺[k-1].

Unscented transform maps the sigma points through g()

Along with 𝛘⁺[k-1], we have to generate weights w_m and w_c to use when calculating the mean and the covariance of the transformed points, respectively.  In the book, w_m and w_c are the same except for w_c0 = w_m0 + (1 - 𝛂² + 𝛃).  Although the book presented only 1 sigma point generating and weighting algorithm, there are apparently many variations listed, for example, in Grewall and Andrews.  The only thing that did not understand is the extra (1 - 𝛂² + 𝛃) term on w_c0, because the weights are supposed to sum to 1 (normalized); I checked that w_m sums to 1, which means that w_c does not sum to 1.  Until I can tackle down the Ph.Ds in my team for a satisfying answer (don't hold you breadth), here is the Matlab code to generate the sigma points:

function [X, wm, wc] = compute_sigma_points(mu, sigma)

% Computes the 2n+1 sigma points according to the unscented transform,

% where n is the dimensionality of the mean vector mu.

% The sigma points should form the columns of sigma_points,

% i.e. sigma_points is an nx2n+1 matrix.

n = length(mu); % state dimension

global alpha kappa beta;

lambda = max(alpha^2 * (n + kappa) - n, 1);

scale = lambda + n;

[U, D, V] = svd(sigma); %[V,D] = eig(sigma);

D = sqrt(D);

D = max(D, 0.05 * eye(n)); % lower bound explained below

sigmasqr = ...sqrt(n+lambda) * Seems to be too big!

           ... sqrtm(sigma); %chol(sigma);

           U * D * V'; %V * D / V;

mureplicated = repmat(mu, 1, n);

X = [mu, mureplicated + sigmasqr, mureplicated - sigmasqr];

% Computing the weights for recovering the mean and variance.

% Note: weights are column vectors, to allow vector multiply w/ samples

wm = [lambda/scale; repmat(1/(2*scale), 2*n, 1)];

wc = wm;

wc(1) = wc(1) + 1 - alpha^2 + beta;

end

Prediction in SLAM

Unless some of the states are completely decoupled (the covariance between them is zero), one should feed the full 𝛍⁺[k-1] and 𝚺_𝛍𝛍⁺[k-1] to the compute_sigma_points.  But in SLAM, the landmarks do not move (one of the fundamental assumptions), so there is no need to predict the landmark positions, so I can just constrain the prediction to the robot pose.  In this 2-D planar robot, the I can use Matlab's vector operator to transform all 𝛘⁺[k-1] in just a few lines of code.

[X, wm, wc] = compute_sigma_points(mu(1:3), sigma(1:3,1:3));

theta = X(3, :);

ang = theta + rot1 + rot2;

pose_sample = [X(1, :) + trans * cos(theta + rot1);

               X(2, :) + trans * sin(theta + rot1);

               cos(ang); % expand out x,y separately

               sin(ang)];

% Be careful when computing the robot's orientation (sum up the sines and

% cosines and recover the 'average' angle via atan2)

mu(1:2) = pose_sample(1:2,:) * wm;

mu(3) = atan2(pose_sample(4,:) * wm, pose_sample(3,:) * wm);

I can see why w_m being normalized is important for keeping 𝛍^-[k] unbiased.  Prediction of the covariance is also concise in Matlab:

dSigma = X - mu(1:3) * ones(1,length(wm)); % subtract the mean

dSigma(3,:) = wrapToPi(dSigma(3,:)); % normalize angles again

sigma(1:3,1:3) = dSigma * diag(wc) * dSigma' ...

    + diag([0.01 0.01 deg2rad(0.5)]); % robot move uncertainty

Negative feedback loop between 𝚺_mm and sigma point spread

I first tried using the matrix square root or Cholesky decomposition to generate the pairs of points away from the mean, but I ran into a problem: there is a "negative" feedback between "uncertainty" (can be seen with the norm of the covariance matrix) and the sigma point spread.  Intuitively, if the sigma point spread scales linearly with the size of the hyper-ellipsoid, decreasing uncertainly leads to smaller spread in the sigma points, which (unless the nonlinear system equation is grossly non-linear or even singular) in turn leads to smaller covariance matrix norm; because 𝚺_mm does not grow during the prediction step like 𝚺_xx (see my earlier comment about the landmark stationarity assumption in SLAM), this leads to 𝚺_mm asymptotically decaying to 0. This contrasts sharply with the uncertainty of the landmark being lower bounded by the robot pose in EKF (they are completely correlated).  I by lower bounding the diagonal entries of the SVD(𝚺_𝛍𝛍), I tried to keep the landmark uncertainty disappearing to 0. I am not sure yet if this is the best way to deal with the problem, but 𝚺_mm --> 0 is a numerical problem at minimum because 𝚺_𝛍𝛍 becomes ill-conditioned.  Because I always think about "how am I going to do this on a 32-bit or 16-bit MCU", I am particularly alarmed about this inconvenient discovery that is usually not mentioned in the Kalman Filter fundamental books.  Even with this heuristics, the landmark position uncertainty has reduced dramatically compared to EKF.

Robot pose (magenta) and landmark position (cyan) estimate at the end of 331 time steps.  Note that the estimates have drifted less than in EKF SLAM.

Correction step

The unscented transform is used again during the correction step--this time using the a-priori mean 𝛍^-[k] and covariance 𝚺_𝛍𝛍^-[k] from the prediction step, and using all of them instead of just the robot pose portion, as in the SLAM prediction step.  To correct the states from observation, I need 2 things: the Kalman gain K[k] and the innovations from the landmark observations.  But at a given time step k, the observation consists of old landmarks (from which innovation can be formed) and new landmarks.  So I have to form K_p[k] and an ensemble of innovations (z_p[k] - hp(𝛘^-[k])) using just previously observed landmarks at time step k (subscript "p" for previously known).  Note that I cannot reuse the ensemble generated during prediction because the predicted state 𝛍^-[k] is different than 𝛍⁺[k-1].  Forming the innovations from z_p[k] and 𝛘^-[k] is relatively straight-forward, but K_p[k] involves more steps.  For clarity, let the ensemble of expected observations be

Z_p[k] = h_p(𝛘^-[k])

z_p^avg[k] = Z_p[k] w_m

Then loosely speaking, K_p[k] is the cross-correlation of 𝛘^-[k] with Z_p[k], divided by the covariance of Z[k]:

S_p[k] = (Z_p[k] - z_p^avg[k]) diag(w_c) (Z_p[k] - z_p^avg[k])^T + Q_p[k]

K_p[k] = (𝛘^-[k] - 𝛍^-[k]) diag(w_c) (Z_p[k] - z_p^avg[k])^T / S_p[k]

The uncertainty of measurement Q_p[k] works to de-weight the innovation for the correction, just like in EKF.  The state and covariance correction are then:

𝛍⁺[k] = 𝛍^-[k] + K_p[k] ( z[k] - z_p^avg[k] )

𝚺_𝛍𝛍⁺[k] = 𝚺_𝛍𝛍^-[k] - K_p[k] S_p[k] K_p^T[k]

Here is the Matlab implementation of the simple idea given above:

lId = z(:,1);

oz = z(ismember(lId, map), :);

if ~isempty(oz) % Have known landmark => can generate innovation

    [X, wm, wc] = compute_sigma_points(mu, sigma);

    nX = size(X,2); % number of sigma points

    % Reorder obs according to map, which is also the order in mu (state)

    [~,lIdx] = ismember(oz(:,1), map);

    nz = size(oz, 1);

    % line 7 on slide 32: Z = h(X)

    % Generate distribution of expected observation from sigma_point of

    % robot poses AND landmark positions. 

    Dx = X(2*lIdx+2,:) - ones(nz,1) * X(1,:); % nZ x nX

    Dy = X(2*lIdx+3,:) - ones(nz,1) * X(2,:); % nZ x nX

    

    Range = sqrt(Dx.^2 + Dy.^2); % predicted range and bearing distribution

    Bearing = atan2(Dy, Dx) - ones(nz,1) * X(3,:);

    % To average the angle, must go through unit circle

    Cos = cos(Bearing) * wm; Sin = sin(Bearing) * wm;

    Range_avg = Range * wm;

    % Rearrange according to the mu order

    Z_avg = zeros(2*nz, 1);

    Z_avg(1:2:end) = Range_avg;

    Z_avg(2:2:end) = atan2(Sin, Cos);

    q = zeros(2*nz,1);

    q(1:2:end) = Range_avg * 0.01;

    q(2:2:end) = deg2rad(1) * ones(nz,1);

    q = q .^ 2;

    

    % UKF correction algo step 9

    dZ = zeros(2*nz, nX);

    dZ(1:2:end, :) = Range;

    dZ(2:2:end, :) = Bearing;

    % innovation covariance matrix S, line 9 on slide 32

    dZ = dZ - Z_avg * ones(1,nX); % Subtract the mean of sample

    % Normalize the angles

    dZ(2:2:end) = wrapToPi(dZ(2:2:end));

    S = dZ * diag(wc) * dZ' + diag(q);

    % Compute Sigma_x_z, the Cross covariance, line 10 on slide 32

    % (which is equivalent to sigma times the Jacobian H transposed in EKF).    

    dSigma = X - mu * ones(1, nX); % subtract the mean

    dSigma(3,:) = wrapToPi(dSigma(3,:)); % normalize angles again

    Sigma_xz = dSigma * diag(wc) * dZ';

    % Compute the Kalman gain, line 11 on slide 32

    K = Sigma_xz / S;

    % Current observation

    %lId = oz(:,1);

    range = oz(:,2); bearing = oz(:,3);

    % Update mu and sigma, line 12 + 13 on slide 32

    innoZ = zeros(2*nz,1);

    innoZ(1:2:end) = range; % Form the innovation

    innoZ(2:2:end) = bearing;

    innoZ = innoZ - Z_avg;

    innoZ(2:2:end) = wrapToPi(innoZ(2:2:end));

    

    mu = mu + K * innoZ;

    mu(3) = wrapToPi(mu(3));

    sigma = sigma - K * S * K';

end

Accepting new landmarks

Previously unobserved landmarks are no use for correction, but still have to be accepted into an expanding state.  I have not yet figured out how to calculate the cross-correlation of the newly observed landmark with existing landmark or even the robot, but here's my code used to generate the above result:

z = z(~ismember(z(:,1), map), :); % Deal with only the new landmarks

nz = size(z,1);

for i=1:nz % For newly discovered landmarks in this timestep, sample around

    nMu = length(mu);

    lId = z(i,1); range = z(i,2); bearing = z(i,3); % raw measurement

    [X, wm, wc] = compute_sigma_points([range; bearing] ...

        , diag([0.01*range deg2rad(1)] .^ 2));

    % Transform range/bearing to Cartesian

    Range = X(1,:);

    Bearing = X(2,:);

    Dx = Range .* cos(Bearing + mu(3));

    Dy = Range .* sin(Bearing + mu(3));

    dx = Dx * wm;% Expected landmark position relative to robot position

    dy = Dy * wm;% = weighted average

    mu(nMu + (1:2)) = [mu(1) + dx; mu(2) + dy];

    dSigma = [Dx - dx; Dy - dy];

    % Stick the new weighted variance at the lower right corner of sigma

    sigma(nMu+(1:2),nMu+(1:2)) = dSigma * diag(wc) * dSigma';

    map(end+1) = lId; %add landmark to the map

end

Understanding the EKF SLAM

In April of this year, I attended the AR (augmented reality) workshop at Stanford, and learned the current issues and direction of the AR community.  I was curious whether I can just use VI (visual-inertial) SLAM (simultaneous localization and mapping) for the AR video game I want to write, and got confirmation that this would be possible after talking to Renato F. Salas-Moreno.  Since I work with inertial sensors in my current job, VI SLAM is particularly interesting.  While the VI SLAM on a single device is amply demonstrated by the slew of cutting edge products like Google Tango and Microsoft HoloLens, the interactive AR video game I have in mind requires merging of the map produced from multiple devices in the same room.  I learned from Marc Pollefeys that HoloLens solves the problem by using the map of a "master" device.  I got the feeling that this is not a completely solved problem, but since I am new to the whole SLAM problem, I am going to work through the EKF (extended Kalman filter) example from the online SLAM course, which uses the notation in the Probabilistic Robotics book.  In the equations to follow, I am forced to use the underline as the vector, instead of the usual overbar notation, because of the poor formatting capability of Blogger editor.

State formation

In SLAM, the device's pose (position and orientation) x, and the positions (but not orientation) of various landmarks (loosely defined as things that can be identified from camera images) m_i--both in the world frame are captured as the state vector 𝛍^true = [x^true; m^true₁; ...; m^true_N], or more compactly 𝛍^true = [x^true; m^true].  Since I don't know where the device or the landmarks are initially my initial state estimate is 𝛍[0] = [0; 0].  Most text will annotate this vector with a hat (^) for the estimate, but again due to the Formatting constraint in the Blogger editor, I will just omit the hat, and instead denote the (unknowable) as ^true, as I've done above.

Ignoring for now the management of the landmark associations (they are sometimes unobservable as the device moves around the world frame), it is clear that the initial covariance matrix 𝚺_mm[0] = diag(∞, ..., ∞) because at the outset I know nothing about the landmark positions--except the landmark positions are independent of each other, and the device position, for that matter.  So 𝚺_𝛍𝛍[0] = diag(0, 0, 0, ∞, ..., ∞).  Note that this initialization of the device pose covariance (setting to 0) is different than what may be found in other Kalman filter textbooks (which uses ∞) because of the different initialization method in the book (coming up shortly below).  In linear (the original) Kalman filter formulation, the state evolution and observable generation are linear stochastic processes, but in EKF, they are Gaussian nonlinear processes (that can nevertheless be linearized):

	Kalman Filter	Extended Kalman Filter
Process (state change) model	𝛍true = A 𝛍true + B u + r	𝛍true = g(𝛍true, u) + r
Measurement model	zk[k]= H 𝛍true + qk[k]	zk[k]= hk(𝛍true) + qk[k]

where  r ~ 𝓝(0, R[k]) and q_k[k] ~ 𝓝(0, Q_k[k]).  The subscripts k in the measurement model is necessary in SLAM because landmarks come and go at every time step: to me, it means "pertaining to all observations at time step k".

The zero initial value of the states pretty much assure that the prediction of the state for the next time step is still zero--unless the system is such that zero is not an equilibrium, or if there is an external input AKA control input.  In this example of a planar robot movement using odometer model, there is apparently a non-zero initial external input (the odometry data):

u[0] = [rot1; trans; rot2] = [0.1007    0.1001    0.0002]^T.

Prediction step

When first studying Kalman filter, I was confused whether to perform prediction right after outputting the control command, or at the beginning of the next step, right before processing the fresh measurement.  Because of my background in real-time control, I perform the non-time-critical tasks such as prediction after outputting the control command (i.e. minimize time delay between sensor measurement available and when the control command is output).  But in this case, the external input is not the control command but rather another sensor reading (odometry reading), which is only available at the next time step.  That is why the "prediction" step uses the a-posteriori estimate from the last time step x⁺[k-1] = [x⁺[k-1]; y⁺[[k-1]; 𝛉⁺[k-1] ], which in this example:

𝛍^-[k] = g(u[k]; 𝛍⁺[k-1])

For additional clarity, I adopted the "-" and "+" superscripts for the a-priori and a-posteriori estimates.

Even though g(u[k]; 𝛍⁺[k-1]) can be a BHF (big hairy function) in general, stationarity assumption for the landmarks (m[k] = m[k-1]) in SLAM simplifies the state update to only the device pose update:

𝛍^-[k] = 𝛍⁺[k-1] + [trans * cos(𝛉⁺[k-1] + rot1) ; trans * sin(𝛉⁺[k-1] + rot1) ; rot1 + rot2]

Observation prediction: has to wait till landmark observation in SLAM

We will see shortly the a-posterior update (correction) driven by the sensor measurement (observation).  The prediction of the observation h(𝛍[k]) is only necessary at that time, but in the spirit of calculating everything possible outside the time critical path from sensor measurement being available to outputting control command, I want to pre-calculate h(𝛍[k]) now--except that in SLAM, I don't know a-priori which landmarks I will observe, so I have to wait till discovering all observations for the time step.

Covariance prediction

To update the covariance, I need the Jacobian matrix G^-[k]:

G^-[k] = ∂𝛍[k] / ∂𝛍[k-1] @ 𝛍^-[k] = ∂g(u[k]; 𝛍[k-1]) / ∂𝛍[k-1] @ 𝛍^-[k]

Note that the partial derivative is derived from the prediction (i.e. without the new sensor input), AT 𝛍[k-1]--the last optimal state estimate.  Once again, the landmark stationarity and independence assumption allows simplification:

G^-[k] = diag(G^-_x[k], I_2N); where G^-_x[k] = ∂x[k] / ∂x[k-1] @ x^-[k]

In the prediction equation above, only 𝛉 affects change of x, so that only the dx/d𝛉 and dy/d𝛉 need to be considered for the Jacobian calculation:

dx/d𝛉 = -trans * sin(𝛉 + rot1); dy/d𝛉 = trans * cos(𝛉 + rot1)

G^-_x[k] = I₃ + trans * [ -sin(𝛉^-[k] + rot1) ; cos(𝛉^-[k] + rot1) ; 0]

I think it is helpful to remember separation of the device pose and the landmark positions manifest in the covariance matrix structure as well:

𝚺_𝛍𝛍 = [𝚺_xx 𝚺_xm ; 𝚺^T_xm 𝚺_mm], since 𝚺_mx = 𝚺^T_xm.

Covariance prediction step involves this huge matrix (dimension (3+2N)² in this example):

𝚺^-_𝛍𝛍[k] = G^-[k] 𝚺⁺_𝛍𝛍[k-1] G^-T[k] + R[k]

In R[k] = diag(R^-_x[k], 0), R^-_x characterizes the random noise in the device pose (imagine the device being knocked around by an accident), evaluated at 𝛍^-[k]. I have not seen a noise covariance modeled with such fidelity as to be sensitive to the current state estimate, but wanted clarity for now.  Note that unless the physical process has no noise (i.e. R[k] is 0: impossible!), the state uncertainly always grows during the prediction step, as you can see in the following plot of the robot location estimate overlaid against the known landmarks.

Red: prior device pose estimate and uncertainly (a-posteriori estimate from previous time step)
Magenta: predicted device pose and uncertainty, which is larger than the red ellipse

In SLAM, the huge covariance prediction calculation above in the first term above can be broken up into 3 isolated portions (the off-diagonal terms are transpose of each other) for simplicity.

G-[k] 𝚺+𝛍𝛍[k-1] G-T[k] =	G-x[k] 𝚺+xx[k-1] G-T[k]	, G-x[k] 𝚺+xm[k-1]
	(G-x[k] 𝚺+xm[k-1])T	, 𝚺+mm[k-1]]

In fact, the independence of the landmarks allows even more factoring of G^-[k] 𝚺^-_𝛍𝛍[k-1] G^-T[k]:

G-x[k] 𝚺+xx[k-1] G-T[k]	, G-x[k] 𝚺+x1[k-1]	, ...	, G-x[k] 𝚺+xN[k-1]
(G-x[k] 𝚺+x1[k-1])T	, 𝚺+11[k-1]	, ...	, 𝚺+1N[k-1]
...	, ...	, ...	, ...
(G-x[k] 𝚺+xN[k-1])T	, 𝚺+N1[k-1]	, ...	, 𝚺+NN[k-1]

This formulation better shows the correlation between the device pose and individual landmark, and the inter-landmark correlation; it might be a better suited to the when landmarks rapidly come in and out of view, but might be an overkill for the simple case of fixed and unchanging landmarks.

Correction step

In this example problem, the range and bearing of a subset of the known landmarks are observed at each time step.  Assuming that the association problem (which landmark is this for?) is solved somehow, we can calculate the difference between the measured observation and the predicted observation, which is called innovation.  If a given landmark was previously observed, its estimated position is already in 𝛍^-[k].  But a newly discovered landmark i's state m^-_i[k] is initialized with yet another function:

m^-_i[k] = initial(𝛍^-[k], z_i[k])

There doesn't seem to be a lot of discussion on this initialization function, but if we can initialize, then we can use the completed 𝛍^-[k] to calculate the predicted observation h_p(𝛍^true) and the measurement Jacobian H_p[k], which is the gradient of the observables z[k] WRT 𝛍^true:

H_p[k] = ∂h_p(𝛍[k]) / ∂𝛍[k] @ 𝛍^-[k]

Just like the process Jacobian, there are many ways to estimate the measurement Jacobian (analytical, numerical, and their respective children), but I've only used analytical models so far.

State correction

K_p[k] is the optimal gain under the zero-mean Gaussian noise hypothesis--called Kalman gain.

K_p[k] = 𝚺^-_𝛍𝛍[k] H_p^T[k] / (H_p[k] 𝚺^-_𝛍𝛍[k] H_p^T[k] + Q_p[k])

Note that the "/" operator is in fact matrix inversion.  I find Kalman "weight" to be a better reminder of its true nature, because that is how much weight is given to the innovation z[k] - h_p(𝛍^-[k]) to update the state estimate:

𝛍⁺[k] = 𝛍^-[k] + K_p[k] ( z[k] - h_p(𝛍^-[k]) )

Note that the unobserved landmarks do not contribute to correcting the state estimate.

Covariance correction

The Kalman weight also contributes to the state covariance correction:

𝚺⁺_pp[k] = (I_pp - K_p[k] H_p[k]) 𝚺^-_pp[k] = 𝚺^-_pp[k] - K_p[k] H_p[k] 𝚺^-_pp[k]

where 𝚺_pp = [𝚺_xx 𝚺_xp ; 𝚺^T_xp 𝚺_pp] is the combination of the device pose to device pose covariance, device pose to observed (at k) landmark positions, and the landmark position to landmark position covariance.  Note that the covariance decreases commensurate to the Kalman weight, in reverse of the always increasing covariance during the prediction step earlier.

Red: predicted device pose estimate and uncertainly (a-priori estimate from this time step)
Magenta: corrected device pose and uncertainty, which is smaller than the red ellipse
Blue: predicted landmark position estimate and uncertainty (a-priori estimate from this time step)
Cyan: corrected landmark position and uncertainty, which is smaller than the blue ellipse
Black: range and bearing measurements at that time step, drawn from the corrected device pose (maybe I should have drawn it from the predicted device pose)

This completely textbook implementation of EKF leaves much to be desired.  I had to futz around with the sensor and process uncertainties to avoid gross estimation error--to some extent, this is one of the deficiencies of KF I already heard about before.  But I was surprised by how much the solution drifts as the device moves away from the starting position, which is in the lower left corner below.

If I reduced the sensor covariance too much, the final landmark (at 9,2) doesn't even fall within the 2𝛔  ellipse.   The drift might be due to the sensor bias, and probably uncorrectable without another sensor or a loop closure (coming later in the course).

There is much, much more to Kalman filter

While reading GNSS books about a year ago, I realized that the Kalman filter that I and many other people know is just the tip of the iceberg, and I am not even talking about the UFK.  A year ago, I could not have imagined such things as the Kalman smoother, closed-loop Kalman filter, or constrained Kalman filter.  I hope to share my future discoveries on such topics with you.