Understanding | Haobin Tan

OSI Model

Thu, 11 Mar 2021 00:00:00 +0000

Layer Nr	Layer Name
7	Application
6	Presentation
5	Session
4	Transport
3	Network
2	Data Link
1	Physical

Mnemonic

How does Data Flows the OSI Model Layers?

Client makes request to server

Server response

The round trip of how data flows through all these seven layers on both sides is a physical path, on which data actually and physically flows.

The OSI model also addresses another aspect how data flows on a logical path, layer to layer commnunication.

Layers	Sender	Receiver
Application	generate data	read data
Presentation	encrypt and compress data	decrypt and decompress data
Session
Transport	choke up data into segments	put segments together
Network	make packets	open packets
Data Link	make frames	open frames
Physical

OSI Model Layer by Layer

Application Layer

Non-technical: user’s application (E.g. Chrome, Firefox )
Technical: refers to application protocols
- E.g. HTTP, SMTP, POP3, IMAP4, …
- Facilitate communications between application and operation system
Application data is generated here

Presentation Layer

Provides a variety of coding and conversion functions on application data
- Ensure that information sent from the application layer of the client could be understood by the application layer of the server
  
  $\rightarrow$ Try to translate application data into a certain format that every different system could understand
Main functions
- Data conversion
- Data encryption
- Data compression
Protocols
- Images: JPEG, GIF, TIF, PNG, …
- Videos: MP4, AVI, …

Session Layer

Establish, manage, and terminate connections between the sender and the receiver
An intuitive example

Telephone call is a good example to explain session layer: First establish the connection and start the conversation. Then terminate the session

Transport Layer

Accept data from Session layer
Choke up data into segments
Add header information
- E.g. destination port number, source port number, sequence number, …
Protocols: TCP and UDP

Network Layer

Protocol: Internet Protocol (IP)
Take segment from Transport layer and add extra header information
- E.g. sender’s and receiver’s IP address
Create packet

Data Link Layer

When IP packet arrives at this layer, more header information will be added to the packet
- E.g. source and destination MAC address, FCS trailer
Ethernet frames are created

MAC address is physical address for your Network Interface Card (NIC). At this layer, NIC has crucial job of creating frames on the sender side, and reading or destroying frames on the receiver side.

Physical Layer

Accept frames from Data Linker layer and generate bits
These bits are made of electrical impulses or lights
Through the network media, the data travels to the receiver

$\rightarrow$ It completes the whole journey of seven layers on the sender side

Reference

OSI Model 👍

Circuit Switching Vs. Packet Switching

Thu, 11 Mar 2021 00:00:00 +0000

TL;DR

Switching Network	Characteristics	Suitable for	Use Cases
Circuit Switching	A dedicated channel or circuit is established for the duration of communications	Communications which require data to be transmitted in real time	Traditional telephone calls
Packet Switching	Connected through many routers, each serving different segment of network	More flexible and more efficient if some amount of delay is acceptable	Handles digital data

Circuit Switching

A dedicated channel or circuit is established for the duration of communications.
The method used by the old traditional telephone call, carried over the Public Switched Telephone Network (PSTN)
Also referred to as the Plain Old Telephone Service (POTS)
Ideal for communications which require data to be transmitted in real time
Normally used for traditional telephone calls

This is what a typical traditional telephone network look like.

The PSTN networks are connected through central offices, which act as telephone exchanges, each serving a certain geographical area.

When person A calls Person B:

Packet Switching

Packet switching networks are connected through many routers, each serving different segment of networks
How it works?
More flexible and more efficient if some amount of delay is acceptable
Normally handle digital data

Reference

Circuit Switching vs. Packet Switching

Multiprotocol Label Switching (MPLS)

Thu, 11 Mar 2021 00:00:00 +0000

Recall: Packet Switching and Circuit Switching

Suppose an IP packet is sent from Mumbai, India to Kansas City, Kansas using Packet switching

Packet switching is flexible and data path is not fixed. But processing IP information at every router slows down transmission.

In contrast, circuit switching method is a fixed-path switching method. It is reliable but more expensive.

MPLS

MPLS allows IP packets to be forwarded at layer 2 (switching level) without being passed up to layer 3 (routing level).

Let’s take a look how MPLS works with the same IP packet sent from Mumbai, India to Kansas City, Kansas.

These routers act like switches on a local network.

As a result, MPLS offers potentially faster transmission than traditionally packet switching networks 👏.

In summary, MPLS can create end-to-end paths that act like circuit-switched connections, but deliver layer 3 IP packet.

As we know, routing is the layer 3 function while switching is the layer 2 function. MPLS makes those routers on the Internet act like switches on a local network.

$\rightarrow$ MPLS is also called 2.5 layer protocol.

Reference

MPLS - Multiprotocol Label Switching (2.5 layer protocol)

Control Plane Vs. Data Plane

Fri, 12 Mar 2021 00:00:00 +0000

Abstract view on an IP router

![截屏2021-03-12 10.22.31](https://raw.githubusercontent.com/EckoTan0804/upic-repo/master/uPic/截屏2021-03-12 10.22.31.png)

Control Plane

Determines/controls how data packets are forwarded — meaning how data is sent from one place to another.

Responsible for
- Creating a routing table
- populating the routing table
- drawing network topology forwarding table and hence enabling the data plane functions
$\rightarrow$ Here the router makes its decision
Routers use various protocols to identify network paths, and they store these paths in routing tables.

Data Plane / Forwarding Plane

In contrast to the control plane, which determines how packets should be forwarded, the data plane actually forwards the packets.
Data plane packet goes through the router and incoming and outgoing of frames are done based on control plane logic.

Summary

Think of the control plane as being like the stoplights that operate at the intersections of a city. Meanwhile, the data plane (or the forwarding plane) is more like the cars that drive on the roads, stop at the intersections, and obey the stoplights.

Reference

TCP

Mon, 15 Mar 2021 00:00:00 +0000

TCP = Transmisson Control Protocol

How TCP starts and closes session?

Three stages of TCP

Session starting
Data transmission
Session ending

This three stages make TCP a connect-oriented and reliable protocol. 👏

Suppose a client wants to get web pages from a server. Three stages below will be gone through.

Session Starting

Three-way handshake to start a session

Client sends a single SYN packet to the server, asking for a session

Client: Hi, server, do you want to talk?

Server replies with a SYNACK packet (The server acknowledges the client’s request, and ask client for a talk)

Client: Hi, server, do you want to talk?

Server: Yes, I want to talk. Do you want to talk?
The client replies with ACK packet.

Client: Hi, server, do you want to talk?

Server: Yes, I want to talk. Do you want to talk?

Client: Yes, I want to talk.

Data Transmission

After three-way handshake, connection is established. And data packets are going to be transferred.

During data transmission, TCP also guarantees that data is successfully received and resembled in a correct order.

Session Ending

After server sends all packets to the client, a four-steps procedure is performed before the connection is closed

The Server sends FINACK packet to the client.

Server: I am done. Can you hear me?
The client responsed with ACK package.

Server: I am done. Can you hear me?

Client: Yes, I got your message, I can hear you.
When the client completes download in the webpage, it sends FINACK to the server

Server: I am done. Can you hear me?

Client: Yes, I got your message, I can hear you.

Client: I am done. Can you hear me?
The server responses with ACK

After this, the session between them can be properly close, unless the client continues to ask for another webpage.

TCP Three-way Handshake in Detail

Suppose the client wants to get web pages from the server. Before any web page transmission, TCP connection must be established through three-way handshake.

The client sends SYN segment to the server, asking for synchronization (synchronization means connection)
The server replies with SYN-ACK (synchronization and acknowledgement)
- The server acknowledges the client’s connection request
- The server also asks the client to open a connection too.
The client replies ACK, which is like “Yes”. Then the two-way connection is established between them.

More Technical View

The client sends a SYN segment with the initial sequence number 9001
- ACK is set to 0
- SYN is set to 1
The server replies with SYN-ACK
- The server’s SYN is set to 1
- ACK is set to 9002, which is the client’s sequence number plus 1.
  - By adding 1 to the client’s sequence number, the server simply acknowledges the client connection request)
- The server’s segment has its own initial sequence number 5001
The client responses
- SYN is set to 0 : There is NO more synchronization/connection request)
- ACK is set to 5002: The client acknowledges the server connection request by increasing the server-side sequence number by 1 (5001 + 1 = 5002)
- Seq is set to 9002: This is the second segment issued by the client at this point (9001 + 1 = 9002)

At this point, both client and server have agreed to open their connection to each other.

Step 1 and 2: establish the connection from client to server
Step 2 and 3: establish the connection from server to client

$\rightarrow$ Two-way connection channel is established and they are ready to exchange their messages.

TCP vs. UDP

TCP (Transmission Control Protocol) and UDP (User Datagram Protocol) are two protocols of the Transport layer (Layer 4) in the OSI model.

	TCP	UDP
Reliable	Yes	No
Connection	connection-oriented	connectionless
Character	reliable	faster, more efficient
Usage	dominant transport protocol	live streaming audio/video DNS queries, DHCP or VoIP Only a few applications use UDP

Reliablity

When TCP delivers data segments to destination, the protocol makes sure
- each segement is received,
- no error,
- and segments are put together in a correct order
$\rightarrow$ TCP is reliable 👍
When UDP delivers data segments to destination, it does NOT guarantee, or even NOT care if the data reach the destination. Once the data is sent off, UDP says “Goodbye, and good luck!”

$\rightarrow$ UDP is NOT reliable 🤪

Connection

TCP: connection-oriented

TCP uses three-way handshake to make sure the connection is established before data transmission.

After data is delivered, TCP will follow a 4-step procedure to make sure every bit of data is delivered and received before clossing the connection.

UDP: connectionless

No handshake to establish the connection
No procedure to close the connection

Example for Understanding TCP and UDP

TCP walks to a bar

TCP: I want a beer.

Bartender: You want a beer?

TCP: Yes, I want a beer.

UDP walks to a bar

UDP: I want a beer. (He does NOT care if the bartender hears him or not)

UDP might never get a beer…Well, he’s UDP. He doesn’t care.

TCP Details

Round Trip Time (RTT)

Stop-and-Wait ARQ Protocol

After transmitting one frame, the sender waits for an acknowledgement before transmitting the next frame
If the acknowledgement does NOT arrive after a certain period of time, the sender times out and retransmits the original frame

Drawbacks
- One frame at a time
- Poor utilization of bandwidth
- Poor performance

Sliding Window Protocol

Send multiple frames at a time
Number of frames to be sent is based on Window size
- Window size = # frames that can be sent before expecting ACK
Each frame is numbered (called sequence number)

Example

The sender wants to send 11 frames (Frame 0 to 10)
Window size is set to 4

The receiver sends 4 frames at the same time

Now the receiver sends back an ACK for frame 0. Once frame 0 is acknowledged, the sender can send frame 4. Now look at the sliding window, it slides a little bit to the left.

Now frame 2 is acknowledged.

The process works similarly. Frame 0 and 1 is acknowledged. Frame 2 - 5 are in the sliding window, meaning that they’re already sent, but not acknowledged.

Summary:

Reference: Sliding Window Protocol

Reference

How TCP starts and close session?

Three-time handshake

TCP vs. UDP

Ethernet Basics

Mon, 15 Mar 2021 00:00:00 +0000

CSMA/CD

CSMA/CD = Carrier Sense Multiple Access with Collision Detection

Media access control method used in early Ethernet technology

Carrier Sense Multiple Access

Carrier: transmission medium that carries data, e.g.
- Electronic bus in Ethernet network
- Band of the electronmagnetic spectrum (channel) in Wi-Fi network
Carrier Sense
A node (i.e. Network Interface Card, NIC) on a network has a sense: it can listen and hear. It can detect what is going on over the transmission medium.
Multiple Access
Every node in the network has equal right to access to and use the shared medium, but they must take turns
Putting them together, CSMA means Before a node transmits data, it checks or listens to the medium
- Medium not busy ➡️ the node sends its data
- When the node detects the medium is used ➡️ It will back off and wait for a random amount of time and try again

Collision Detection: A node can hear collision if it happens

Example

Both A and C want to transmit their data. They check the media and find it is not busy. So they send their message at the same time. Collision occurs. When these two nodes hear the collision, they will back off and use some kind of randomization to decide which would go first in order to avoid collision.

Ethernet Frame

Frame = a protocol data unit (PDU)

PDU in different layer of the OSI model is named differently.

Among the Ethernet family, frames can be different. For any two devices to communicate, they must have the same type of frames.

An Ethernet frames has seven main parts:

Preamble

A 64 bit header information telling the receiving node that a frame is coming and where the frame starts
Recipient MAC

Recipient’s MAC address
Sender MAc

Sender’s MAC address
Type

Tells the recipient the basic type of data, such as IPv4 or IPv6
Data
- Payload carried by frame, such as IP packet from Network layer.
- Limit is 1500 Bytes
Pad

Extra bits to make a frame at least bigger than 64 Bytes (Any data unit < 64 Bytes would be considered as collisions)
FCS = Frame Check Sequence

Used for error checking and the integrity verfication of a frame

Spanning Tree Protocol (STP)

Complete Graph and Spanning Tree

Complete Graph

A graph in which each pair of graph vertices is connected by a line
- I.e., when all the points are connected by the maximum number of lines, we get a complete graph
In networking field, a complete graph is like a fully meshed network

Spanning tree

All points are connected by a minimum number of lines

From the complete graph above, we can get three spanning trees:

All three points are connected and no loop is formed.
Basic features
- NO loop
- Minimumly connected (i.e., removing one line will leave some point disconnected)

Spanning Tree Protocol

Spanning Tree Protocol (STP)

Layer 2 protocol that runs on bridges and switches and builds a loop-free logical topology.
🎯 Main purpose: eliminate loops
Three basic steps:
1. Select one switch as root bridge (central point on the network)
2. Choose the shortest path (the least cost) from a switch to the root bridge
  - Path cost is calculated based on link bandwidth: the higher bandwidth, the lower the path cost.
3. Block links that cause loops while maintaining these links as backups

Example

Suppose we have a simple network

First, STP elects the root bridge. The lowest bridge ID (priority: MAC address) determins the root bridge. Here swithc A is elected as the root bridge.

Next each of other switches chooses the path to the root bridge with least path cost. Here we just skip the details of calculation and mark the path cost for each link.

Now let’s take a look at switch B. For switch B, there’re two paths to reach root bridge, switch A:

BDCA: costs 7 (2 + 4 + 1)
BA: costs 2

Therefore, the link BA is chosen as the path from switch B to root bridge A.

RP: Root port, the port with the least cost path to the root bridge
DP: designated port.

For switch C and D, the procedure is similar.

Note

A non-root switch can have many designated ports, but it can have only ONE root port.
All ports of the root bridge are designated ports. On the root bridge, there is NO root port.

Now every switch has found the best path to reach the root bridge. And the links between D and C should be blocked in order to eliminate a loop.

Let’s look at the blocked link DC

The port with the lowest switch ID would be selected as the designated port. The other end is blocking port. The blocking port can still receive frames, but it will not forward or send frames. It simply drops them.

How STP Elects Root Bridge?

Root bridge election is based on a 8 byte switch Bridge ID (BID)

2 Bytes Priority Field
6 Bytes Switch MAC address

Root bridge election process is simple: All interconnected switches exchange their BIDs

Whoever has the lowest priority field value would become the root bridge
If priority filed is equal, whoever has the lowest MAC address would become the root bridge

BPDU

Every switch multicasts its message, Hello BPDU, in which each swich declares ifself the root bridge.

Bridge Protocol Data Unit (BPDU) is a frame containing information about spanning ree protocol.

Hello BPDU is used by switches or bridges to share information about themselves. It is used for

electing a root bridge
determining ports roles and states
blocking unwanted links

In other words, Hello BPDU is used to configure a loop-free network.

Structure of BPDU:

Three important fields:

Root ID: Root Bridge BID
Root Path cost: The best path cost to the root bridges
Bridge ID: BPDU sender’s ID (Source BID)

The Election Process

One thing need to be kept in mind: Each port of a switch is uniquely identified.

Consider the example above:

Switch A, B, and C send out their Hello BPDUs. Basically each switch declares itself the root bridge.

Let’s take a look at Switch A first.

Switch A sends out its Hello BPDU to B and C

Switch A sets it Root ID to its own BID (“Hello everyone, I am the root bridge”)
Path cost value is set to 0

Switch B and C do the same thing.

Basically they all claim they are the root bridge (“the boss”) in their Hello BPDUs.

The problem is: ONLY one can be the root bridge.

What they do next is to compare their Hello BPDUs and to elect a real boss.

When Switch A receives Hello BPDUs from B and C, it checks and discards their BPDUs because its bridge ID is lower than B’s and C’s. So A keeps its original Hello BPDU and still believes it is the root bridge.
When Switch B receives the Hello BPDU from C, it compares and finds its Bridge ID is lower (i.e. B’s BPDU is superior) thus discards C’s Hello BPDU

When B receives A’s Hello BPDU, it finds A’s Bridge ID is lower. It would say “Well, Switch A is the winner”.

Therefore, it modifies its Root ID value by replacing its own bridge ID with Switch A’s bridge Id. It also calculates the path cost to switch A (let’s say 4), and then sends the modified Hello BPDU to others.
When Switch C receives Hello BPDU from A and B, C finds A’s is a superior BPDU. So C changes the value of the root ID to switch A’s Bridge ID. And it calculates the path cost to switch A (let’s say 1). Then it sends its modified Hello BPDU to others.

This way, A, B, and C exchange their BPDUs again and agree that the root bridge should be switch A.

Once the root bridge is decided, path cost to the root bridges are calculated. Root ports, designated ports, and blocked ports are determined. STP has created a loop-free network! 👏

Reference

CSMA/CD

7 Part of an Ethernet Frame

Spanning Tree Protocol (IEEE 802 1D)

IP Address & Subnet

Thu, 01 Apr 2021 00:00:00 +0000

Let’s take 10.0.0.0/8 as an example.

The address 10.0.0.0/8 comprises of two parts

IP address (10.0.0.0)

the global addressing scheme used under Internet Protocol
Subnet or IP block (/8)
- divide the IP addresses into small blocks/ranges.
- The “/” notation along with the number is called prefix.

Calculation of IP range

The “/ " notation after the IP address can be used to calculate the IP address range that falls under that category.
All you have to do is subtract the prefix from the number 32 (As IP addresses is a 32 bit number). Put the result as an exponent of 2 and you will get the number of IPs in that range.

For example

to find the IP range of “/8” prefix, we subtract The prefix 8 from 32. The result 24 is used as an exponent of 2. Hence, the IP range you get is “2 to the power 24” i.e 16777216 IPs.
Thus, “10.0.0.0/8” refers to an IP block ranging from “10.0.0.0” to “10.255.255.255”.

Understanding

Wed, 22 Jun 2022 00:00:00 +0000

Kalman Filter

Fri, 24 Jun 2022 00:00:00 +0000

The Kalman filter is an efficient recursive filter estimating the internal-state of a linear dynamic system from a series of noisy measurements.

Applications of Kalman filter include

Guidance
Navigation
Control of vehicles, aircraft, spacecraft, and ships positioned dynamically

💡 The basic idea of Kalman filter is to achieve the optimal estimate of the (hidden) internal state by weightedly combining the state prediction and the measurement.

Kalman Filter Summary

Kalman filter in a picture:

Summary of equations:

	Equation	Equation Name	Alternative Names
Predict	$\hat{\boldsymbol{x}}_{n, n-1}=\mathbf{F} \hat{\boldsymbol{x}}_{n-1, n-1} + \mathbf{G} \boldsymbol{u}_{n}$	State Extrapolation	Predictor Equation Transition Equation Prediction Equation Dynamic Model State Space Model
Predict	$\mathbf{P}_{n, n-1}=\mathbf{F} \mathbf{P}_{n-1, n-1} \mathbf{F}^{T}+\mathbf{Q}$	Covariance Extrapolation	Predictor Covariance Equation
Update	$\mathbf{K}_{n}=\mathbf{P}_{n, n-1} \mathbf{H}^{T}\left(\mathbf{H} \mathbf{P}_{n, n-1} \mathbf{H}^{T}+\mathbf{R}_{n}\right)^{-1}$	Kalman Gain	Weight Equation
	$\hat{\boldsymbol{x}}_{\boldsymbol{n}, \boldsymbol{n}}=\hat{\boldsymbol{x}}_{\boldsymbol{n}, \boldsymbol{n}-1}+\mathbf{K}_{\boldsymbol{n}}\left(\boldsymbol{z}_{n}-\mathbf{H} \hat{\boldsymbol{x}}_{\boldsymbol{n}, \boldsymbol{n}-1}\right)$	State Update	Filtering Equation
	$\mathbf{P}_{n, n}=\left(\mathbf{I}-\mathbf{K}_{n} \mathbf{H}\right) \mathbf{P}_{n, n-1}$	Covariance Update	Corrector Equation
Auxilliary	$\boldsymbol{z}_{n} = \mathbf{H} \boldsymbol{x}_n + \boldsymbol{v}_n$	Measurement Equation
	$\mathbf{R}_n = E\{\boldsymbol{v}_n \boldsymbol{v}_n^T\}$	Measurement Uncertainty	Measurement Error
	$\mathbf{Q}_n = E\{\boldsymbol{w}_n \boldsymbol{w}_n^T\}$	Process Noise Uncertainty	Process Noise Error
	$\mathbf{P}_{n, n}=E\left\{\boldsymbol{e}_{n} \boldsymbol{e}_{n}^{T}\right\}=E\left\{\left(\boldsymbol{x}_{n}-\hat{\boldsymbol{x}}_{n, n}\right)\left(\boldsymbol{x}_{n}-\hat{\boldsymbol{x}}_{n, n}\right)^{T}\right\}$	Estimation Uncertainty	Estimation Error

Summary of notations:

Term	Name	Alternative Term
$\boldsymbol{x}$	State vector
$\boldsymbol{z}$	Output vector	$\boldsymbol{y}$
$\mathbf{F}$	State transition matrix	$\mathbf{\Phi}$, $\mathbf{A}$
$\boldsymbol{u}$	Input variable
$\mathbf{G}$	Control matrix	$\boldsymbol{B}$
$\mathbf{P}$	Estimate uncertainty	$\boldsymbol{\Sigma}$
$\mathbf{Q}$	Process noise uncertainty
$\mathbf{R}$	Measurement uncertainty
$\boldsymbol{w}$	Process noise vector
$\boldsymbol{v}$	Measurement noise vector
$\mathbf{H}$	Observation matrix	$\mathbf{C}$
$\mathbf{K}$	Kalman Gain
$n$	Discrete time index	$k$

Multidimensional Kalman Filter in Detail

A Kalman filter works by a two-phase process, including 5 main equations:

Predict phase: produces prediction of the current state, along with thier uncertainties
- State extrapolation equation
- Covariance extrapolation equation
Update phase: checks how good the predicted result fits to the current measurement and refines the state prediction using a weighted average given measurements if necessary.

State extrapolation equation

The Kalman filter assumes that the true state of a system at time step $n$ evolved from the prior state at time step $n-1$ is

$$ \boldsymbol{x}_n = \mathbf{F} \boldsymbol{x}_{n-1} +\mathbf{G} \boldsymbol{u}_{n} + \boldsymbol{w}_n $$

$\boldsymbol{x}_{n}$ : state vector
$\boldsymbol{u}_{n}$ : control variable or input variable - a measurable (deterministic) input to the system
$\boldsymbol{w}_n$ : process noise or disturbance - an unmeasurable input that affects the state
$\mathbf{F}$ : state transition matrix - applies the effect of each system state parameter at time step $n-1$ on the system state at time step $n$
$\mathbf{G}$ : control matrix or input transition matrix (mapping control to state variables)

The state extrapolation equation

Predicts the next system state, based on the knowledge of the current state
Extrapolates the state vector from time step $n-1$ to $n$
Also called
- Predictor Equation
- Transition Equation
- Prediction Equation
- Dynamic Model
- State Space Model
The general form in a matrix notation
$$ \hat{\boldsymbol{x}}_{n, n-1}=\mathbf{F} \hat{\boldsymbol{x}}_{n-1, n-1}+\mathbf{G} \boldsymbol{u}_{n} $$
- $\hat{\boldsymbol{x}}_{n, n-1}$ : predicted system state vector at time step $n$
- $\hat{\boldsymbol{x}}_{n-1, n-1}$ : estimated system state vector at time step $n-1$
$\hat{\boldsymbol{x}}_{n, m}$ represents the estimate of $\boldsymbol{x}$ at time step $n$ given observation/measurements up to and including at time $m \leq n$

Example

Covariance extrapolation equation

The covariance extrapolation equation extrapolates the uncertainty in our state prediction.

$$ \mathbf{P}_{n, n-1}=\mathbf{F} \mathbf{P}_{n-1, n-1} \mathbf{F}^{T}+\mathbf{Q} $$

$\mathbf{P}_{n-1, n-1}$ : uncertainty (covariance matrix) of the estimate at time step $n-1$
$$ \begin{aligned} \mathbf{P}_{n-1, n-1} &= E\{\underbrace{(\boldsymbol{x}_{n-1, n-1} - \hat{\boldsymbol{x}}_{n-1, n-1})}_{=: \boldsymbol{e}_n} (\boldsymbol{x}_{n-1, n-1} - \hat{\boldsymbol{x}}_{n-1, n-1}) ^T\} \\ & = E\{\boldsymbol{e}_n \boldsymbol{e}_n^T\} \end{aligned} $$
$\mathbf{P}_{n, n-1}$ : uncertainty (covariance matrix) of the prediction at time step $n$
$\mathbf{F}$ : state transition matrix
$\mathbf{Q}$ : process noise matrix
$$ \mathbf{Q}_n = E\{\boldsymbol{w}_n \boldsymbol{w}_n^T\} $$
- $\boldsymbol{w}_n$ : process noise vector

Derivation

At time step $n$, the Kalman filter assumes

$$ \boldsymbol{x}\_n = \mathbf{F} \boldsymbol{x}\_{n-1} +\mathbf{G} \boldsymbol{u}\_{n} + \boldsymbol{w}\_n $$

The prediction of state is

$$ \hat{\boldsymbol{x}}\_{n, n-1}=\mathbf{F} \hat{\boldsymbol{x}}\_{n-1, n-1}+\mathbf{G} \boldsymbol{u}\_{n} $$

The difference between $\boldsymbol{x}\_n$ and $\hat{\boldsymbol{x}}\_{n, n-1}$ is

$$ \begin{aligned} \boldsymbol{x}\_{n}-\hat{\boldsymbol{x}}\_{n, n-1} &=\mathbf{F} \boldsymbol{x}\_{n-1}+\mathbf{G} \boldsymbol{u}\_{n}+\boldsymbol{w}\_{n}-\left(\mathbf{F} \hat{\boldsymbol{x}}\_{n-1, n-1}+\mathbf{G} \boldsymbol{u}\_{n}\right) \\\\ &=\mathbf{F}\left(\boldsymbol{x}\_{n-1}-\hat{\boldsymbol{x}}\_{n-1, n-1}\right)+\boldsymbol{w}\_{n} \end{aligned} $$

The variance associate with the prediction $\hat{\boldsymbol{x}}\_{n, n-1}$ of an unknow true state $\boldsymbol{x}\_n$ is

Noting that the state estimation errors and process noise are uncorrelated:

$$ E\left\\{\left(\boldsymbol{x}\_{n-1}-\hat{\boldsymbol{x}}\_{n-1, n-1}\right) \boldsymbol{w}\_{n}^{T}\right\\} = E\left\\{\boldsymbol{w}\_{n}\left(\boldsymbol{x}\_{n-1}-\hat{\boldsymbol{x}}\_{n-1, n-1}\right)^{T}\right\\} = 0 $$

Therefore

$$ \begin{aligned} \mathbf{P}\_{n, n-1} &=\underbrace{E\left\\{\left(\boldsymbol{x}\_{n-1}-\hat{\boldsymbol{x}}\_{n-1, n-1}\right)\left(\boldsymbol{x}\_{n-1}-\hat{\boldsymbol{x}}\_{n-1, n-1}\right)^{T}\right\\}}\_{=\mathbf{P}\_{n-1, n-1}} \mathbf{F}^{T}+\underbrace{E\left\\{w\_{n} w\_{n}^{T}\right\\}}\_{=\mathbf{Q}} \\\\ &=\mathbf{F} \mathbf{P}\_{n-1, n-1}\mathbf{F}^T+\mathbf{Q} \end{aligned} $$

Kalman Gain equation

The Kalman Gain is calculated so that it minimizes the covariance of the a posteriori state estimate.

$$ \mathbf{K}_{n}=\mathbf{P}_{n, n-1} \mathbf{H}^{T}\left(\mathbf{H} \mathbf{P}_{n, n-1} \mathbf{H}^{T}+\mathbf{R}_{n}\right)^{-1} $$

$\mathbf{P}_{n, n-1}$ : uncertainty (covariance) matrix of the current state prediction
$\mathbf{H}$ : observation matrix
$\mathbf{R}_{n}$ : measurement Uncertainty (measurement noise covariance matrix)

Derivation

Rearrange the covariance update equation

$$ \begin{array}{l} \mathbf{P}\_{n, n}=\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right) \mathbf{P}\_{n, n-1}{\color{DodgerBlue} \left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T}}+\mathbf{K}\_{n} \mathbf{R}\_{n} \mathbf{K}\_{n}^{T} \\\\\\\\ \mathbf{P}\_{n, n}=\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right) \mathbf{P}\_{n, n-1}{\color{DodgerBlue}\left(\mathbf{I}-\left(\mathbf{K}\_{n} \mathbf{H}\right)^{T}\right)}+\mathbf{K}\_{n} \mathbf{R}\_{n} \mathbf{K}\_{n}^{T} \qquad | \text{ } \mathbf{I} = \mathbf{I}^T \\\\\\\\ \mathbf{P}\_{n, n}={\color{ForestGreen}\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right) \mathbf{P}\_{n, n-1}}{\color{DodgerBlue}\left(\mathbf{I}-\mathbf{H}^{T} \mathbf{K}\_{n}^{T}\right)}+\mathbf{K}\_{n} \mathbf{R}\_{n} \mathbf{K}\_{n}^{T} \qquad | \text{ } (\mathbf{AB})^T = \mathbf{B}^T \mathbf{A}^T\\\\\\\\ \mathbf{P}\_{n, n}={\color{ForestGreen}\left(\mathbf{P}\_{n, n-1}-\mathbf{K}\_{n} \mathbf{H} \mathbf{P}\_{n, n-1}\right)}\left(\mathbf{I}-\mathbf{H}^{T} \mathbf{K}\_{n}^{T}\right)+\mathbf{K}\_{n} \mathbf{R}\_{n} \mathbf{K}\_{n}^{T} \\\\\\\\ \mathbf{P}\_{n, n}=\mathbf{P}\_{n, n-1}-\mathbf{P}\_{n, n-1} \mathbf{H}^{T} \mathbf{K}\_{n}^{T}-\mathbf{K}\_{n} \mathbf{H} \mathbf{P}\_{n, n-1} \\\\ +{\color{MediumOrchid}\mathbf{K}\_{n} \mathbf{H} \mathbf{P}\_{n, n-1} \mathbf{H}^{T} \mathbf{K}\_{n}^{T}+\mathbf{K}\_{n} \mathbf{R}\_{n} \mathbf{K}\_{n}^{T}} \qquad | \text{ } \mathbf{AB}\mathbf{A}^T + \mathbf{AC}\mathbf{A}^T = \mathbf{A}(\mathbf{B} + \mathbf{C})\mathbf{A}^T \\\\\\\\ \mathbf{P}\_{n, n}=\mathbf{P}\_{n, n-1}-\mathbf{P}\_{n, n-1} \mathbf{H}^{T} \mathbf{K}\_{n}^{T}-\mathbf{K}\_{n} \mathbf{H} \mathbf{P}\_{n, n-1} \\\\ +{\color{MediumOrchid}\mathbf{K}\_{n}\left(\mathbf{H} \mathbf{P}\_{n, n-1} \mathbf{H}^{T}+\boldsymbol{\mathbf{R}}\_{n}\right) \mathbf{K}\_{n}^{T}} \end{array} $$

As the Kalman Filter is an optimal filter, we will seek a Kalman Gain that minimizes the estimate variance.

In order to minimize the estimate variance, we need to minimize the main diagonal (from the upper left to the lower right) of the covariance matrix $\mathbf{P}\_{n, n}$ .

The sum of the main diagonal of the square matrix is the trace of the matrix. Thus, we need to minimize $tr(\mathbf{P}\_{n, n})$ . In order to find the conditions required to produce a minimum, we will differentiate $tr(\mathbf{P}\_{n, n})$ w.r.t. $\mathbf{K}\_n$ and set the result to zero.

$$ \begin{array}{l} tr\left(\mathbf{P}\_{\boldsymbol{n}, \boldsymbol{n}}\right)=tr\left(\mathbf{P}\_{\boldsymbol{n}, \boldsymbol{n}-1}\right)-{\color{DarkOrange}tr\left(\mathbf{P}\_{n, n-1} \mathbf{H}^{T} \mathbf{K}\_{n}^{T}\right)}\\\\ {\color{DarkOrange} -tr\left(\mathbf{K}\_{n} \mathbf{H} \mathbf{P}\_{n, n-1}\right)} + tr\left(\mathbf{K}\_{\boldsymbol{n}}\left(\mathbf{H} \mathbf{P}\_{\boldsymbol{n}, \boldsymbol{n}-\mathbf{1}} \mathbf{H}^{\boldsymbol{T}}+\mathbf{R}\_{\boldsymbol{n}}\right) \mathbf{K}\_{n}^{\boldsymbol{T}}\right) \qquad | \text{} tr(\mathbf{A}) = tr(\mathbf{A}^T)\\\\\\\\ tr\left(\mathbf{P}\_{n, n}\right)=tr\left(\mathbf{P}\_{n, n-1}\right)-{\color{DarkOrange}2 tr\left(\mathbf{K}\_{n} \mathbf{H} \mathbf{P}\_{n, n-1}\right)}\\\\ +tr\left(\mathbf{K}\_{n}\left(\mathbf{H} \mathbf{P}\_{n, n-1} \mathbf{H}^{\boldsymbol{T}}+\mathbf{R}\_{n}\right) \mathbf{K}\_{n}^{T}\right)\\\\\\\\ \frac{d}{d \mathbf{K}\_{n}}t r\left(\mathbf{P}\_{n, n}\right)={\color{DodgerBlue} \frac{d}{d \mathbf{K}\_{n}}t r\left(\mathbf{P}\_{n, n-1}\right)}-{\color{ForestGreen}\frac{d }{d \mathbf{K}\_{n}}2 t r\left(\mathbf{K}\_{n} \mathbf{H} \mathbf{P}\_{n, n-1}\right)} \\\\ +{\color{MediumOrchid}\frac{d tr(\mathbf{K}\_{n}(\mathbf{H}\mathbf{P}\_{n, n-1}\mathbf{H}^T + \mathbf{R}\_n)\mathbf{K}\_{n}^T)}{d\mathbf{K}\_{n}}} \overset{!}{=} 0 \quad \mid {\color{ForestGreen} \frac{d}{d \mathbf{A}}tr(\mathbf{A} \mathbf{B}) = \mathbf{B}^T},{\color{MediumOrchid} \frac{d}{d \mathbf{A}}tr(\mathbf{A} \mathbf{B} \mathbf{A}^T) = 2\mathbf{A} \mathbf{B}}\\\\\\\\ \frac{d\left(t r\left(\mathbf{P}\_{n, n}\right)\right)}{d \mathbf{K}\_{n}}={\color{DodgerBlue}0}-{\color{ForestGreen}2\left(\mathbf{H} \mathbf{P}\_{ n , n - 1 }\right)^{T}}\\\\ +{\color{MediumOrchid}2 \mathbf{K}\_{n}\left(\mathbf{H} \mathbf{P}\_{n, n-1} \mathbf{H}^{T}+\mathbf{R}\_{n}\right)}=0\\\\\\\\ {\color{ForestGreen}\left(\mathbf{H} \mathbf{P}\_{n, n-1}\right)^{T}}={\color{MediumOrchid}\mathbf{K}\_{n}\left(\mathbf{H} \mathbf{P}\_{n, n-1} \mathbf{H}^{T}+\mathbf{R}\_{n}\right)} \\\\\\\\ \mathbf{K}\_{n}=\left(\mathbf{H} \mathbf{P}\_{n, n-1}\right)^{T}\left(\mathbf{H} \mathbf{P}\_{n, n-1} \mathbf{H}^{T}+\mathbf{R}\_{n}\right)^{-1} \quad \mid (\mathbf{AB})^T = \mathbf{B}^T \mathbf{A}^T \\\\\\\\ \mathbf{K}\_{n}=\mathbf{P}\_{n, n-1}^{T} \mathbf{H}^{T}\left(\mathbf{H} \mathbf{P}\_{n, n-1} \mathbf{H}^{T}+\mathbf{R}\_{n}\right)^{-1} \quad | \text{ covariance matrix } \mathbf{P} \text{ symmetric } (\mathbf{P}^T = \mathbf{P})\\\\\\\\ \mathbf{K}\_{n}=\mathbf{P}\_{n, n-1} \mathbf{H}^{T}\left(\mathbf{H} \mathbf{P}\_{n, n-1} \mathbf{H}^{T}+\mathbf{R}\_{n}\right)^{-1} \end{array} $$

Kalman Gain intuition

The Kalman gain tells how much I should refine my prediction (i.e., the a priori estimate) by given a measurement.

We show the intuition of Kalman Gain with a one-dimensional Kalman filter.

The one-dimensional Kalman Gain is

$$ \boldsymbol{K}_{\boldsymbol{n}}=\frac{p_{n, n-1}}{p_{n, n-1}+r_{n}} \in [0, 1] $$

$p_{n, n-1}$ : variance of the state prediction $\hat{x}_{n, n-1}$
$r_n$ : variance of the measurement $z_n$

(Derivation see here)

Let’s rewrite the (one-dimensional) state update equation:

$$ \hat{x}_{n, n}=\hat{x}_{n, n-1}+K_{n}\left(z_{n}-\hat{x}_{n, n-1}\right)=\left(1-K_{n}\right) \hat{x}_{n, n-1}+K_{n} z_{n} $$

The Kalman Gain $K_n$ is the weight that we give to the measurement. And $(1 - K_n)$ is the weight that we give to the state prediction.

High Kalman Gain

A low measurement uncertainty (small $r_n$) relative to the prediction uncertainty would result in a high Kalman Gain (close to 1). The new estimate would be close to the measurement.

💡 Intuition

small $r_n \rightarrow$ accurate measurements $\rightarrow$ place more weight on the measurements and thus conform to them
Low Kalman Gain

A high measurement uncertainty (large $r_n$) relative to the prediction uncertainty would result in a low Kalman Gain (close to 0). The new estimate would be close to the prediction.

💡 Intuition

large $r_n \rightarrow$ measurements are not accurate $\rightarrow$ place more weight on the prediction and trust them more

State update equation

The state update equation updates/refines/corrects the state prediction with measurements.

$$ \hat{\boldsymbol{x}}_{\boldsymbol{n}, \boldsymbol{n}}=\hat{\boldsymbol{x}}_{\boldsymbol{n}, \boldsymbol{n}-1}+\mathbf{K}_{\boldsymbol{n}}\underbrace{\left(\boldsymbol{z}_{n}-\mathbf{H} \hat{\boldsymbol{x}}_{\boldsymbol{n}, \boldsymbol{n}-1}\right)}_{\text{innovation}} $$

$\hat{\boldsymbol{x}}_{n, n}$ : estimated system state vector at time step $n$
$\hat{\boldsymbol{x}}_{n, n-1}$ : predicted system state vector at time step $n$
$\mathbf{K}_{\boldsymbol{n}}$ : Kalman Gain
$\mathbf{H}$ : observation matrix
$\boldsymbol{z}_{n}$ : measurement at time step $n$
$$ \boldsymbol{z}_{n} = \mathbf{H} \boldsymbol{x}_n + \boldsymbol{v}_n $$
- $\boldsymbol{x}_n$ : true system state (hidden state)
- $\boldsymbol{v}_n$ : measurement noise
  
  $\rightarrow$ Measurement uncertainty $\mathbf{R}_n$ is given by
  $$ \mathbf{R}_n = E\{\boldsymbol{v}_n \boldsymbol{v}_n^T\} $$

Covariance update equation

The covariance update equation updates the uncertainty of state estimate on the base of covariance prediction

$$ \mathbf{P}_{n, n}=\left(\mathbf{I}-\mathbf{K}_{n} \mathbf{H}\right) \mathbf{P}_{n, n-1}\left(\mathbf{I}-\mathbf{K}_{n} \mathbf{H}\right)^{T}+\mathbf{K}_{n} \mathbf{R}_{n} \mathbf{K}_{n}^{T} $$

$\mathbf{P}_{n, n}$ : estimate uncertainty (covariance) matrix of the current state
$\mathbf{P}_{n, n-1}$ : uncertainty (covariance) matrix of the current state prediction
$\mathbf{K}_{n}$ : Kalman Gain
$\mathbf{H}$ : observation matrix
$\mathbf{R}_{n}$ : measurement Uncertainty (measurement noise covariance matrix)

Derivation

According to state update equation:

$$ \begin{aligned} \hat{\boldsymbol{x}}\_{n, n} &= \hat{\boldsymbol{x}}\_{n, n-1}+\mathbf{K}\_{n}\left(\boldsymbol{z}\_{n}-\mathbf{H} \hat{\boldsymbol{x}}\_{n, n-1}\right) \\\\\\\\ &= \hat{\boldsymbol{x}}\_{n, n-1}+\mathbf{K}\_{n}\left(\mathbf{H} \boldsymbol{x}\_n + \boldsymbol{v}\_n-\mathbf{H} \hat{\boldsymbol{x}}\_{n, n-1}\right) \end{aligned} $$

The estimation error between the true (hidden) state $\boldsymbol{x}\_n$ and estimate $\hat{\boldsymbol{x}}\_{n, n}$ is:

$$ \begin{aligned} \boldsymbol{e}\_n &= \boldsymbol{x}\_n - \hat{\boldsymbol{x}}\_{n, n} \\\\ &= \boldsymbol{x}\_n - \hat{\boldsymbol{x}}\_{n, n-1} - \mathbf{K}\_{n}\mathbf{H}\boldsymbol{x}\_n - \mathbf{K}\_{n}\boldsymbol{v}\_n + \mathbf{K}\_{n}\mathbf{H} \hat{\boldsymbol{x}}\_{n, n-1}\\\\ &= \boldsymbol{x}\_n - \hat{\boldsymbol{x}}\_{n, n-1} - \mathbf{K}\_{n}\mathbf{H}(\boldsymbol{x}\_n - \hat{\boldsymbol{x}}\_{n, n-1}) - \mathbf{K}\_{n}\boldsymbol{v}\_n \\\\ &= (\mathbf{I} - \mathbf{K}\_{n}\mathbf{H})(\boldsymbol{x}\_n - \hat{\boldsymbol{x}}\_{n, n-1}) - \mathbf{K}\_{n}\boldsymbol{v}\_n \end{aligned} $$

Estimate Uncertainty

$$ \begin{array}{l} \boldsymbol{\mathbf{P}}\_{n, n}=E\left(\boldsymbol{e}\_{n} \boldsymbol{e}\_{n}^{T}\right)=E\left(\left(\boldsymbol{x}\_{n}-\hat{\boldsymbol{x}}\_{n, n}\right)\left(\boldsymbol{x}\_{n}-\hat{\boldsymbol{x}}\_{n, n}\right)^{T}\right) \qquad | \text{ Plug in } \boldsymbol{e}\_n\\\\\\\\ \boldsymbol{\mathbf{P}}\_{n, n}=E\left(\left(\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)-\mathbf{K}\_{n} v\_{n}\right) \right.\\\\ \left.\times\left(\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)-\mathbf{K}\_{n} v\_{n}\right)^{T}\right)\\\\\\\\ \mathbf{P}\_{n, n}=E\left(\left(\left(\mathbf{I}-\boldsymbol{\mathbf{K}}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{\boldsymbol{x}}\_{n, n-1}\right)-\boldsymbol{\mathbf{K}}\_{n} v\_{n}\right) \right.\\\\ \left.\times\left(\left(\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)\right)^{T}-\left(\mathbf{K}\_{n} v\_{n}\right)^{T}\right)\right) \qquad | \text{ }(\mathbf{A} \mathbf{B})^{T}=\mathbf{B}^{T} \mathbf{A}^{T} \\\\\\\\ \mathbf{P}\_{n, n}=E\left(\left(\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)-\mathbf{K}\_{n} v\_{n}\right) \right. \\\\ \left.\times\left(\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)^{T}\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T}-\left(\mathbf{K}\_{n} v\_{n}\right)^{T}\right)\right)\\\\\\\\ \mathbf{P}\_{n, n}=E\left(\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)\left(\boldsymbol{x}\_{n}-\hat{\boldsymbol{x}}\_{n, n-1}\right)^{T}\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T}\right.\\\\ -\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)\left(\mathbf{K}\_{n} v\_{n}\right)^{T}\\\\ -\mathbf{K}\_{n} v\_{n}\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)^{T}\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T}\\\\ \left.+\mathbf{K}\_{n} v\_{n}\left(\mathbf{K}\_{n} v\_{n}\right)^{T}\right) \qquad | \text{ } E(X \pm Y)=E(X) \pm E(Y)\\\\\\\\ \mathbf{P}\_{n, n}=E\left(\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)^{T}\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T}\right)\\\\ -\color{red}{E\left(\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)\left(\mathbf{K}\_{n} v\_{n}\right)^{T}\right)}\\\\ -\color{red}{E\left(\mathbf{K}\_{n} v\_{n}\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)^{T}\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T}\right)}\\\\ +E\left(\mathbf{K}\_{n} v\_{n}\left(\mathbf{K}\_{n} v\_{n}\right)^{T}\right) \end{array} $$

$\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}$ is the error of the prior estimate in relation to the true value. It is uncorrelated with the current measurement noise $\boldsymbol{v}\_n$. The expectation value of the product of two independent variables is zero.

$$ \begin{aligned} \color{red}{E\left(\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)\left(\mathbf{K}\_{n} v\_{n}\right)^{T}\right)} = 0 \\\\ \color{red}{E\left(\mathbf{K}\_{n} v\_{n}\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)^{T}\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T}\right)} = 0 \end{aligned} $$

Therefore

$$ \begin{array}{l} \mathbf{P}\_{n, n}=E\left(\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)^{T}\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T}\right)\\\\ +{\color{DodgerBlue}{E\left(\mathbf{K}\_{n} v\_{n}\left(\mathbf{K}\_{n} v\_{n}\right)^{T}\right)}} \qquad | \text{ }(\mathbf{A} \mathbf{B})^{T}=\mathbf{B}^{T} \mathbf{A}^{T} \\\\\\\\ \mathbf{P}\_{n, n}=E\left(\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)^{T}\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T}\right)\\\\ +{\color{DodgerBlue}{E\left(\mathbf{K}\_{n} v\_{n} v\_{n}^T \mathbf{K}\_{n}^T\right)}} \qquad | \text{ } E(a X)=a E(X) \\\\\\\\ \mathbf{P}\_{n, n} = \left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right) {\color{ForestGreen}\underbrace{{E\left(\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)\left(\boldsymbol{x}\_{n}-\hat{x}\_{n, n-1}\right)^{T}\right)}}\_{=\mathbf{P}\_{n, n-1}}}\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T} +\mathbf{K}\_{n}{\color{DodgerBlue}{\underbrace{E\left( v\_{n} v\_{n}^T \right)}\_{=\mathbf{R}\_n}}} \mathbf{K}\_{n}^T \\\\\\\\ \mathbf{P}\_{n, n} = \left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right) {\color{ForestGreen}\mathbf{P}\_{n, n-1}} \left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T} +\mathbf{K}\_{n}{\color{DodgerBlue}\mathbf{R}\_n} \mathbf{K}\_{n}^T \end{array} $$

In many textbook you can see a simplified form:

$$ \mathbf{P}_{n, n}=\left(\mathbf{I}-\mathbf{K}_{n} \mathbf{H}\right) \mathbf{P}_{n, n-1} $$

This equation is elegant and easier to remember and in many cases it performs well.

However, even the smallest error in computing the Kalman Gain (due to round off) can lead to huge computation errors. The subtraction $\left(\mathbf{I}-\mathbf{K}_{n} \mathbf{H}\right)$ can lead to nonsymmetric matrices due to floating-point errors. Therefore this equation is numerically unstable!

Derivation of a simplified form of the Covariance Update Equation

$$ \begin{array}{l} \mathbf{P}\_{n, n} = \left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right) \mathbf{P}\_{n, n-1} \left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right)^{T} +\mathbf{K}\_{n}\mathbf{R}\_n \mathbf{K}\_{n}^T \\\\\\\\ \mathbf{P}\_{n, n}=\mathbf{P}\_{n, n-1}-\mathbf{P}\_{n, n-1} \boldsymbol{\mathbf{H}}^{T} \mathbf{K}\_{n}^{T}-\boldsymbol{\mathbf{K}}\_{n} \mathbf{H} \mathbf{P}\_{n, n-1} \\\\ +{\color{MediumOrchid}\mathbf{K}\_{n}}\left(\mathbf{H} \mathbf{P}\_{n, n-1} \mathbf{H}^{T}+\mathbf{R}\_{n}\right) \mathbf{K}\_{n}^{T} \qquad | \text{ Substitute Kalman Gain}\\\\\\\\ \mathbf{P}\_{n, n}=\mathbf{P}\_{n, n-1}-\mathbf{P}\_{n, n-1} \mathbf{H}^{T} \mathbf{K}\_{n}^{T}-\mathbf{K}\_{n} \mathbf{H} \mathbf{P}\_{n, n-1} \\\\ +{\color{MediumOrchid}\mathbf{P}\_{n, n-1}} \underbrace{{\color{MediumOrchid}{\mathbf{H}^{T}\left(\mathbf{H} \mathbf{P}\_{n, n-1} \mathbf{H}^{T}+\mathbf{R}\_{n}\right)^{-1}}}\left(\mathbf{H} \mathbf{P}\_{n, n-1} \mathbf{H}^{T}+\mathbf{R}\_{n}\right)}\_{=1} \mathbf{K}\_{n}^{T} \\\\\\\\ \mathbf{P}\_{n, n}=\mathbf{P}\_{n, n-1}-\mathbf{P}\_{n, n-1} \mathbf{H}^{T} \mathbf{K}\_{n}^{T}-\mathbf{K}\_{n} \mathbf{H} \mathbf{P}\_{n, n-1} \\\\ +\mathbf{P}\_{n, n-1} \mathbf{H}^{T} \mathbf{K}\_{n}^{T} \\\\\\\\ \mathbf{P}\_{n, n}=\mathbf{P}\_{n, n-1}-\mathbf{K}\_{n} \mathbf{H} \mathbf{P}\_{n, n-1} \\\\\\\\ \mathbf{P}\_{n, n}=\left(\mathbf{I}-\mathbf{K}\_{n} \mathbf{H}\right) \mathbf{P}\_{n, n-1} \end{array} $$

Reference

👍 kalmnnfilter.net: clear and detaied tutorial for Kalman filter
- The $\alpha-\beta-\gamma$ filter: detailed introduction to Kalman filter
- One-dimensional Kalman filter with serveral elaborated numerical examples
- Multidimensional Kalman filter
👍 How a Kalman filter works, in pictures: Kalman filter explained intuitively in pictures
Understanding Kalman Filters: a series of video tutorials that intuitively explains Kalman Filter step by step
Kalman and Bayesian Filters in Python: Kalman filter (in Python) explained using Jupyter Notebook
Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation
Wikipedia: Kalman Filter

Linear Kalman Filter

Tue, 19 Jul 2022 00:00:00 +0000

Intuition Example

Estimation of the 1D position of the vehicle.

Starting from an initial probabilistic estimate at time $k-1$

Note: The initial estimate, the predicted state, and the final corrected state are all random variabless that we will specify their means and covariances.

Use a motion model to predict our new state
Use observation model (e.g. derived from GPS) to correct that prediction of vehicle position at time $k$

In this way, we can think of the Kalman filter as a technique to fuse information from different sensors to produce a final estimate of some unknown state, taking the uncertainty in motion and measurements into account.

The Linear Dynamical System

Motion model:

$$ \mathbf{x}_{k}=\mathbf{F}_{k-1} \mathbf{x}_{k-1}+\mathbf{G}_{k-1} \mathbf{u}_{k-1}+\mathbf{w}_{k-1} $$

where

$\mathbf{u}_k$: control input
$\mathbf{w}_k$: process/motion noise. $\mathbf{w}_{k} \sim \mathcal{N}\left(\mathbf{0}, \mathbf{Q}_{k}\right)$

Measurement model

$$ \mathbf{y}_{k}=\mathbf{H}_{k} \mathbf{x}_{k}+\mathbf{v}_{k} $$

where

$\mathbf{v}_{k}$: measurement noise. $\mathbf{v}_{k} \sim \mathcal{N}\left(\mathbf{0}, \mathbf{R}_{k}\right)$

Kalman Filter Steps

Prediction

We use the process model to predict how our state evolves since the last time step, and will propagate our uncertainty.

$$ \begin{array}{l} \check{\mathbf{x}}_{k}=\mathbf{F}_{k-1} \mathbf{x}_{k-1}+\mathbf{G}_{k-1} \mathbf{u}_{k-1} \\ \check{\mathbf{P}}_{k}=\mathbf{F}_{k-1} \hat{\mathbf{P}}_{k-1} \mathbf{F}_{k-1}^{T}+\mathbf{Q}_{k-1} \end{array} $$

Correction

We use measurement to correct that prediction

Notation:

$\check{x}_k$: a prediction before the measurement is incorporated

$\hat{x}_k$: corrected prediction at time step $k$

Optimal Gain
$$ \mathbf{K}_{k}=\check{\mathbf{P}}_{k} \mathbf{H}_{k}^{T}\left(\mathbf{H}_{k} \check{\mathbf{P}}_{k} \mathbf{H}_{k}^{T}+\mathbf{R}_{k}\right)^{-1} $$
Correction
$$ \begin{aligned} \hat{\mathbf{x}}_{k} &=\check{\mathbf{x}}_{k}+\mathbf{K}_{k}\underbrace{\left(\mathbf{y}_{k}-\mathbf{H}_{k} \check{\mathbf{x}}_{k}\right)}_{\text{innovation}} \\ \hat{\mathbf{P}}_{k}&=\left(\mathbf{I}-\mathbf{K}_{k} \mathbf{H}_{k}\right) \check{\mathbf{P}}_{k} \end{aligned} $$

Summary

Example

Consider a self-driving vehicle estimating its own position.

The state vector includes the position and its first derivative, velocity.

$$ \mathbf{x}=\left[\begin{array}{c} p \\ \frac{d p}{d t}=\dot{p} \end{array}\right] $$

Input is the scalar acceleration

$$ \mathbf{u}=a=\frac{d^{2} p}{d t^{2}} $$

THe linear dynamical system is

Motion/Process model
$$ \mathbf{x}_{k}=\left[\begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array}\right] \mathbf{x}_{k-1}+\left[\begin{array}{c} 0 \\ \Delta t \end{array}\right] \mathbf{u}_{k-1}+\mathbf{w}_{k-1} $$
Position observation
$$ y_{k}=\left[\begin{array}{ll} 1 & 0 \end{array}\right] \mathbf{x}_{k}+v_{k} $$
Nose densities
$$ v_{k} \sim \mathcal{N}(0,0.05) \quad \mathbf{w}_{k} \sim \mathcal{N}\left(\mathbf{0},(0.1) \mathbf{1}_{2 \times 2}\right) $$

Given the data at time step $k=0$

$$ \begin{array}{l} \hat{\mathbf{x}}_{0} \sim \mathcal{N}\left(\left[\begin{array}{l} 0 \\ 5 \end{array}\right],\left[\begin{array}{cc} 0.01 & 0 \\ 0 & 1 \end{array}\right]\right) \\ \Delta t=0.5 \mathrm{~s} \\ u_{0}=-2\left[\mathrm{~m} / \mathrm{s}^{2}\right] \quad y_{1}=2.2[\mathrm{~m}] \end{array} $$

We want to estimate the state at time step $k=1$.

Prediction step

$$ \begin{aligned} \check{\mathbf{x}}_{k} &=\mathbf{F}_{k-1} \mathbf{x}_{k-1}+\mathbf{G}_{k-1} \mathbf{u}_{k-1} \\\\ {\left[\begin{array}{c} \check{p}_{1} \\ \check{p}_{1} \end{array}\right] } &=\left[\begin{array}{cc} 1 & 0.5 \\ 0 & 1 \end{array}\right]\left[\begin{array}{l} 0 \\ 5 \end{array}\right]+\left[\begin{array}{c} 0 \\ 0.5 \end{array}\right](-2)=\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right] \end{aligned} $$ $$ \begin{aligned} \check{\mathbf{P}}_{k} &=\mathbf{F}_{k-1} \hat{\mathbf{P}}_{k-1} \mathbf{F}_{k-1}^{T}+\mathbf{Q}_{k-1} \\\\ \check{\mathbf{P}}_{1} &=\left[\begin{array}{cc} 1 & 0.5 \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} 0.01 & 0 \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} 1 & 0.5 \\ 0 & 1 \end{array}\right]^{T}+\left[\begin{array}{cc} 0.1 & 0 \\ 0 & 0.1 \end{array}\right]=\left[\begin{array}{cc} 0.36 & 0.5 \\ 0.5 & 1.1 \end{array}\right] \end{aligned} $$

Correction step

Kalman Gain
$$ \begin{aligned} \mathbf{K}_{1} &=\check{\mathbf{P}}_{1} \mathbf{H}_{1}^{T}\left(\mathbf{H}_{1} \check{\mathbf{P}}_{1} \mathbf{H}_{1}^{T}+\mathbf{R}_{1}\right)^{-1} \\ &\left.=\left[\begin{array}{cc} 0.36 & 0.5 \\ 0.5 & 1.1 \end{array}\right]\left[\begin{array}{l} 1 \\ 0 \end{array}\right]\left(\begin{array}{ll} 1 & 0 \end{array}\right]\left[\begin{array}{cc} 0.36 & 0.5 \\ 0.5 & 1.1 \end{array}\right]\left[\begin{array}{l} 1 \\ 0 \end{array}\right]+0.05\right)^{-1} \\ &=\left[\begin{array}{l} 0.88 \\ 1.22 \end{array}\right] \end{aligned} $$
Correction of the state prediction
$$ \begin{aligned} \hat{\mathbf{x}}_{1} &=\check{\mathbf{x}}_{1}+\mathbf{K}_{1}\left(\mathbf{y}_{1}-\mathbf{H}_{1} \check{\mathbf{x}}_{1}\right) \\\\ {\left[\begin{array}{c} \hat{p}_{1} \\ \hat{\dot{p}}_{1} \end{array}\right] } &=\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]+\left[\begin{array}{c} 0.88 \\ 1.22 \end{array}\right]\left(2.2-\left[\begin{array}{ll} 1 & 0 \end{array}\right]\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]\right)=\left[\begin{array}{l} 2.24 \\ 3.63 \end{array}\right] \end{aligned} $$
Correction of covariance
$$ \begin{aligned} \hat{\mathbf{P}}_{1} &=\left(\mathbf{1}-\mathbf{K}_{1} \mathbf{H}_{1}\right) \check{\mathbf{P}}_{1} \\\\ &=\left[\begin{array}{ll} 0.04 & 0.06 \\ 0.06 & 0.49 \end{array}\right] \end{aligned} $$

Note that the final covariance (i.e. the covariance after correction) is smaller. That is, we are more certain about the car position after we incoporate the position measurement. This uncertainty reduction occurs because our measurement model is fairly accurate (the measurement noise variance is quite small).

Best Linear Unbiased Estimator (BLUE)

If we have white, uncorrelated zero-mean noise, the Kalman Fitler is the best (i.e., lowest variance) unbiased estimator that uses only a linear combination of measurements.

Bias

We repeat the above Kalman filter for $K$ times.

The bias is defined as the difference between true position and the mean of estimated position values.

An estimator of filter is unbiased if it produces an “average” error of zero at a particular time step $k$, over many trials.

$$ E\left[\hat{e}_{k}\right]=E\left[\hat{p}_{k}-p_{k}\right]=E\left[\hat{p}_{k}\right]-p_{k}=0 \qquad \forall k \in \mathbb{N} $$

Bias in Kalman filter state estimation

Consider the error dynamics

Predicted state error
$$ \check{\mathbf{e}}_{k}=\check{\mathbf{x}}_{k}-\mathbf{x}_{k} $$
Corrected state error
$$ \hat{\mathbf{e}}_{k}=\hat{\mathbf{x}}_{k}-\mathbf{x}_{k} $$

Using the Kalman Fitler equations, we can derive

$$ \begin{array}{l} \check{\mathbf{e}}_{k}=\mathbf{F}_{k-1} \check{\mathbf{e}}_{k-1}-\mathbf{w}_{k} \\ \hat{\mathbf{e}}_{k}=\left(\mathbf{1}-\mathbf{K}_{k} \mathbf{H}_{k}\right) \check{\mathbf{e}}_{k}+\mathbf{K}_{k} \mathbf{v}_{k} \end{array} $$

So long as

The initial state estimate is unbiased ($E\left[\hat{\mathbf{e}}_{0}\right]=\mathbf{0}$ )
The noise is white, uncorrelated and zero mean ($E[\mathbf{v}]=\mathbf{0}, E[\mathbf{w}]=\mathbf{0}$ )

Then the state estiamte is unbiased

$$ \begin{aligned} E\left[\check{\mathbf{e}}_{k}\right] &=E\left[\mathbf{F}_{k-1} \check{\mathbf{e}}_{k-1}-\mathbf{w}_{k}\right] \\ &=\mathbf{F}_{k-1} E\left[\check{\mathbf{e}}_{k-1}\right]-E\left[\mathbf{w}_{k}\right] \\ &=\mathbf{0} \end{aligned} $$ $$ \begin{aligned} E\left[\hat{\mathbf{e}}_{k}\right] &=E\left[\left(\mathbf{1}-\mathbf{K}_{k} \mathbf{H}_{k}\right) \check{\mathbf{e}}_{k}+\mathbf{K}_{k} \mathbf{v}_{k}\right] \\ &=\left(\mathbf{1}-\mathbf{K}_{k} \mathbf{H}_{k}\right) E\left[\check{\mathbf{e}}_{k}\right]+\mathbf{K}_{k} E\left[\mathbf{v}_{k}\right] \\ &=\mathbf{0} \end{aligned} $$

Consistency

A filter is consistent if for all $k$

$$ E\left[\hat{e}_{k}^{2}\right]=E\left[\left(\hat{p}_{k}-p_{k}\right)^{2}\right]=\hat{P}_{k} $$

This means that the filter is neither overconfident nor underconfident in the estimate it has produced.

The Kalman Fitler is consistent in state estimate.

$$ E\left[\check{\mathbf{e}}_{k} \check{\mathbf{e}}_{k}^{T}\right]=\check{\mathbf{P}}_{k} \qquad E\left[\hat{\mathbf{e}}_{k} \hat{\mathbf{e}}_{k}^{T}\right]=\hat{\mathbf{P}}_{k} $$

so long as

the initial state estimate is consistent ($E\left[\hat{\mathbf{e}}_{0} \hat{\mathbf{e}}_{0}^{T}\right]=\check{\mathbf{P}}_{0}$ )
the noise is white and zero-mean ($E[\mathbf{v}]=\mathbf{0}, E[\mathbf{w}]=\mathbf{0}$ )

Reference

The (Linear) Kalman Filter

Extended Kalman Filter

Wed, 20 Jul 2022 00:00:00 +0000

Motivation

Linear systems do not exist in reality. We have to deal with nonlinear discrete-time systems

$$ \begin{aligned} \underbrace{\mathbf{x}_{k}}_{\text{current state}}&=\mathbf{f}_{k-1}(\underbrace{\mathbf{x}_{k-1}}_{\text{previous state}}, \underbrace{\mathbf{u}_{k-1}}_{\text{inputs}}, \underbrace{\mathbf{w}_{k-1}}_{\text{process noise}}) \\\\ \underbrace{\mathbf{y}_{k}}_{\text{measurement}}&=\mathbf{h}_{k}(\mathbf{x}_{k}, \underbrace{\mathbf{v}_{k}}_{\text{measurement noise}}) \end{aligned} $$

How can we adapt Kalman Filter to nonlinear discrete-time systems?

💡 Idea: Linearizing a Nonlinear System

Choose an operating point $a$ and approxiamte the nonlinear function by a tangent line at that point.

We compute this linear approximation using a first-order Taylor expansion

$$ f(x) \approx f(a)+\left.\frac{\partial f(x)}{\partial x}\right|_{x=a}(x-a) $$

Extended Kalman Filter

For EKF, we choose the operationg point to be our most recent state estimate, our known input, and zero noise.

Linearized motion model
$$ \mathbf{x}_{k}=\mathbf{f}_{k-1}\left(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}, \mathbf{w}_{k-1}\right) \approx \mathbf{f}_{k-1}\left(\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{0}\right) + \underbrace{\left.\frac{\partial \mathbf{f}_{k-1}}{\partial \mathbf{x}_{k-1}}\right|_{\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{0}}}_{\mathbf{F}_{k-1}}\left(\mathbf{x}_{k-1}-\hat{\mathbf{x}}_{k-1}\right)+\underbrace{\left.\frac{\partial \mathbf{f}_{k-1}}{\partial \mathbf{w}_{k-1}}\right|_{\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{0}}}_{\mathbf{L}_{k-1}} \mathbf{w}_{k-1} $$
Linearized measurement model
$$ \mathbf{y}_{k}=\mathbf{h}_{k}\left(\mathbf{x}_{k}, \mathbf{v}_{k}\right) \approx \mathbf{h}_{k}\left(\check{\mathbf{x}}_{k}, \mathbf{0}\right)+\underbrace{\left.\frac{\partial \mathbf{h}_{k}}{\partial \mathbf{x}_{k}}\right|_{\check{\mathbf{x}}_{k}, \mathbf{0}}}_{\mathbf{H}_{k}}\left(\mathbf{x}_{k}-\check{\mathbf{x}}_{k}\right)+\underbrace{\left.\frac{\partial \mathbf{h}_{k}}{\partial \mathbf{v}_{k}}\right|_{\check{\mathbf{x}}_{k}, \mathbf{0}}}_{\mathbf{M}_{k}} \mathbf{v}_{k} $$

$\mathbf{F}_{k-1}, \mathbf{L}_{k-1}, \mathbf{H}_{k}, \mathbf{M}_{k}$ are Jacobian matrices.

Intuitively, the Jacobian matrix tells you how fast each output of the function is changing along each input dimension.

With our linearized models and Jacobians, we can now use the Kalman Filter equations.

We still use the nonlinear model to propagate the mean of the state estimate in prediction step and compute the measurement residual innovation in correction step.

Example

Similar to the self-driving car localisation example in Linear Kalman Fitler, but this time we use an onboard sensor, a camera, to measure the altitude of distant landmarks relative to the horizon.

($S$ and $D$ are known in advance)

Sate:

$$ \mathbf{x}=\left[\begin{array}{l} p \\ \dot{p} \end{array}\right] $$

Input:

$$ \mathbf{u}=\ddot{p} $$

Motion/Process model

$$ \begin{aligned} \mathbf{x}_{k} &=\mathbf{f}\left(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}, \mathbf{w}_{k-1}\right) \\\\ &=\left[\begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array}\right] \mathbf{x}_{k-1}+\left[\begin{array}{c} 0 \\ \Delta t \end{array}\right] \mathbf{u}_{k-1}+\mathbf{w}_{k-1} \end{aligned} $$

Landmark measurement model (nonlinear!)

$$ \begin{aligned} y_{k} &=\phi_{k}=h\left(p_{k}, v_{k}\right) \\\\ &=\tan ^{-1}\left(\frac{S}{D-p_{k}}\right)+v_{k} \end{aligned} $$

Noise densities

$$ v_{k} \sim \mathcal{N}(0,0.01) \quad \mathbf{w}_{k} \sim \mathcal{N}\left(\mathbf{0},(0.1) \mathbf{1}_{2 \times 2}\right) $$

The Jacobian matrices in this example are:

Motion model Jacobians
$$ \begin{array}{l} \mathbf{F}_{k-1}=\left.\frac{\partial \mathbf{f}}{\partial \mathbf{x}_{k-1}}\right|_{\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{0}}=\left[\begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array}\right] \\\\ \mathbf{L}_{k-1}=\left.\frac{\partial \mathbf{f}}{\partial \mathbf{w}_{k-1}}\right|_{\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{0}}=\mathbf{1}_{2 \times 2} \end{array} $$
Measurement model Jacobians
$$ \begin{array}{l} \mathbf{H}_{k}=\left.\frac{\partial h}{\partial \mathbf{x}_{k}}\right|_{\check{\mathbf{x}}_{k}, \mathbf{0}}=\left[\begin{array}{ll} \frac{S}{\left(D-\breve{p}_{k}\right)^{2}+S^{2}} & 0 \end{array}\right] \\\\ M_{k}=\left.\frac{\partial h}{\partial v_{k}}\right|_{\check{\mathbf{x}}_{k}, \mathbf{0}}=1 \end{array} $$

Given

$$ \begin{array}{l} \hat{\mathbf{x}}_{0} \sim \mathcal{N}\left(\left[\begin{array}{l} 0 \\ 5 \end{array}\right], \quad\left[\begin{array}{cc} 0.01 & 0 \\ 0 & 1 \end{array}\right]\right)\\ \Delta t=0.5 \mathrm{~s}\\ u_{0}=-2\left[\mathrm{~m} / \mathrm{s}^{2}\right] \quad y_{1}=\pi / 6[\mathrm{rad}]\\ S=20[m] \quad D=40[m] \end{array} $$

What is the position estimate $\hat{p}_1$?

Prediction:

$$ \begin{array}{c} \check{\mathbf{x}}_{1}=\mathbf{f}_{0}\left(\hat{\mathbf{x}}_{0}, \mathbf{u}_{0}, \mathbf{0}\right) \\ {\left[\begin{array}{c} \check{p}_{1} \\ \dot{p}_{1} \end{array}\right]=\left[\begin{array}{cc} 1 & 0.5 \\ 0 & 1 \end{array}\right]\left[\begin{array}{l} 0 \\ 5 \end{array}\right]+\left[\begin{array}{c} 0 \\ 0.5 \end{array}\right](-2)=\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]} \\\\ \check{\mathbf{P}}_{1}=\mathbf{F}_{0} \hat{\mathbf{P}}_{0} \mathbf{F}_{0}^{T}+\mathbf{L}_{0} \mathbf{Q}_{0} \mathbf{L}_{0}^{T} \\ \check{\mathbf{P}}_{1}=\left[\begin{array}{cc} 1 & 0.5 \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} 0.01 & 0 \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} 1 & 0 \\ 0.5 & 1 \end{array}\right]+\left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} 0.1 & 0 \\ 0 & 0.1 \end{array}\right]\left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} 0.36 & 0.5 \\ 0.5 & 1.1 \end{array}\right] \end{array} $$

Correction:

$$ \begin{aligned} \mathbf{K}_{1} &=\check{\mathbf{P}}_{1} \mathbf{H}_{1}^{T}\left(\mathbf{H}_{1} \check{\mathbf{P}}_{1} \mathbf{H}_{1}^{T}+\mathbf{M}_{1} \mathbf{R}_{1} \mathbf{M}_{1}^{T}\right)^{-1} \\ &=\left[\begin{array}{cc} 0.36 & 0.5 \\ 0.5 & 1.1 \end{array}\right]\left[\begin{array}{c} 0.011 \\ 0 \end{array}\right]\left(\left[\begin{array}{ll} 0.011 & 0 \end{array}\right]\left[\begin{array}{cc} 0.36 & 0.5 \\ 0.5 & 1.1 \end{array}\right]\left[\begin{array}{c} 0.011 \\ 0 \end{array}\right]+1(0.01)(1)\right)^{-1} \\ &=\left[\begin{array}{c} 0.40 \\ 0.55 \end{array}\right] \end{aligned} $$ $$ \begin{aligned} \hat{\mathbf{x}}_{1} &=\check{\mathbf{x}}_{1}+\mathbf{K}_{1}\left(\mathbf{y}_{1}-\mathbf{h}_{1}\left(\check{\mathbf{x}}_{1}, \mathbf{0}\right)\right) \\ {\left[\begin{array}{c} \hat{p}_{1} \\ \hat{\dot{p}}_{1} \end{array}\right]}&={\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]+\left[\begin{array}{c} 0.40 \\ 0.55 \end{array}\right](0.52-0.49)=\left[\begin{array}{c} 2.51 \\ 4.02 \end{array}\right] } \end{aligned} $$ $$ \begin{aligned} \hat{\mathbf{P}}_{1} &=\left(\mathbf{1}-\mathbf{K}_{1} \mathbf{H}_{1}\right) \check{\mathbf{P}}_{1} \\ &=\left[\begin{array}{cc} 0.36 & 0.50 \\ 0.50 & 1.1 \end{array}\right] \end{aligned} $$

Summary

The EKF uses linearization to adapt the Kalman Filter to nonlinear systems
Linearization works by computing a local linear apporximation to a nonlinear function using the first-order Taylor expansion on a chosen operating point (in this case, the last state estimate)

Reference

Going Nonlinear - The Extended Kalman Filter

Error State Extended Kalman Filter (ES-EKF)

Wed, 20 Jul 2022 00:00:00 +0000

What’s in a State?

We can think of the vehicle state as composed of two parts

$$ \underbrace{\mathbf{x}}_{\text{True state|}}=\underbrace{\hat{\mathbf{x}}}_{\text{Nominal state ("Large")}}+\underbrace{\delta \mathbf{x}}_{\text{Error state ("small")}} $$

💡 Idea

Instead of doing Kalman Filter in the full state (which might have lots of complicated nonlinear behaviours). we use the EKF to directly estimate the error state instead, and then use the estimate of the error state as a correction for nominal state.

ES-EKF Steps

Loop

Update nominal state with motion model (for a bunch of times until getting the measurement)
Propagate uncertainty (for a bunch of times until getting the measurement)
If a measurement is available
1. Compute Kalman Gain
2. Compute error state
  $$ \delta \hat{\mathbf{x}}_{k}=\mathbf{K}_{k}\left(\mathbf{y}_{k}-\mathbf{h}_{k}\left(\check{\mathbf{x}}_{k}, \mathbf{0}\right)\right) $$
3. Correct nominal state
  $$ \hat{\mathbf{x}}_{k}=\check{\mathbf{x}}_{k}+\delta \hat{\mathbf{x}}_{k} $$
4. Correct state covariance
  $$ \hat{\mathbf{P}}_{k}=\left(\mathbf{1}-\mathbf{K}_{k} \mathbf{H}_{k}\right) \check{\mathbf{P}}_{k} $$

Why Use the ES-EKF?

Better performance compared to the vanilla EKF

The “small” error state is more amenable to linear filtering than the “large” nominal state, which can be integrated nonlinearly
Easy to work with constrained quantities (e.g. rotations in 3D)

We can also break down the state using a generalized composition operator
$$ \underbrace{\mathbf{x}}_{\text{true state}}=\underbrace{\hat{\mathbf{x}}}_{\text{Nominal state (constrained)}} \bigoplus \underbrace{\delta\mathbf{x}}_{\text{Error state (unconstrained)}} $$

Summary

The error-state formulation separates the state into a “large” nominal state and a “small” error state
- nominal state: keeps track of the motion model, predicts what the state should be
- error state: captures the modeling errors and the process noise that accumulate over time
The ES-EKF uses local linearization to estimate the error state and uses it to correct the nominal state
The ES-EKF can perform better thant the vanilla EKF, and provides a natural way to handle constrained quantities like 3D rotations

Reference

An Improved EKF - The Error State Extended Kalman Filter

EKF Limitations

Thu, 21 Jul 2022 00:00:00 +0000

Linearization Error

Recap: The EKF works by linearizing the nonlinear motion and measurement models to update the mean and covariance of the state.

The difference between the linear approximation and the nonlinear function is called linearization error

In general, linearization error depends on

How nonlinear the function is
How far away from the operating poitn the linear approximation is being used

Example: Polar Coordinates $\rightarrow$ Cartesian Coordinates

Now we perform linearized transformation:

Compare the linearized and nonlinearized output distributions:

The mean of the linearized distribution is a very different place than the true mean
The linearized covariance seriously underestimates the spread of the true output distribution along the $y$-dimension

$\rightarrow$ In this case, the linearization error can cause our belief of the output distribution to completely miss the mark, and this can cause big problem in our estimator!

Limitations of the EKF

Linearization errors

The EKF is prone to linearization error when

The system dynamics are highly nonlinear
The sensor sampling is slow relative how fast the system is evolving

This has two important consequences

The estimated mean state can become very different from the true state
The estimated state covariance can fail to capture the true uncertainty in the state

$\Rightarrow$ Linearization error can cause the estimator to be overconfident in a wrong answer!

Computing Jacobians

Computing Jacobian matrices for complicated nonlinear functions is also a common source of error in EKF implementations!

Analytical differentiation is prone to human error
Numerical differentiation can be slow and unstable
Automatic differentiation (e.g., at compile time) can also behave unpredictably

And what if one or more of our models is non-differentiable (e.g. the step function)?

Summary

The EKF uses analytical local linearization and, as a result, is sensitive to linearization errors
For highly nonlinear systems, the EKF estimate can diverge and become unreliable
Computing complex Jacobin matrices is an error-prone process and must be done with substantial care

Reference

Limitations of the EKF

Unscented Kalman Filter

Thu, 21 Jul 2022 00:00:00 +0000

Intuition

“It is easier to approximate a probability distribution than it is to approximate an arbitrary nonlinear function”

Idea

We perform a nonlinear transformation $h(x)$ on the 1D gaussian distribution (left), the result is a more complicated 1D distribution (right). We already know the mean and the standard deviation of the input gaussian, and we want to figure out the mean and standard deviation of the output distribution using this information and the nonlinear function.

UKF gives us a way to do this ny three steps

Choose sigma points from our input distribution
Transform sigma points
Compute weighted mean and covariance of transformed sigma points, which will give us a good approximation of the true output distribution.

Unscented Transform

Choosing sigma points

For an $N$-dimensional PDF $\mathcal{N}\left(\mu_{x}, \Sigma_{x x}\right)$ , we need $2N+1$ sigma points

One for the mean
and the rest symmetrically distributed about the mean

1D:

2D:

Steps

Compute the Cholesky Decomposition of the covariance matrix
$$ \mathbf{L} \mathbf{L}^{T}=\boldsymbol{\Sigma}_{x x} $$
Calculate the sigma points
$$ \begin{aligned} \mathbf{x}_{0} &=\boldsymbol{\mu}_{x} & & \\ \mathbf{x}_{i} &=\boldsymbol{\mu}_{x}+\sqrt{N+\kappa} \operatorname{col}_{i} \mathbf{L} & & i=1, \ldots, N \\ \mathbf{x}_{i+N} &=\boldsymbol{\mu}_{x}-\sqrt{N+\kappa} \operatorname{col}_{i} \mathbf{L} & & i=1, \ldots, N \end{aligned} $$
- $\kappa$: tuning parameter. for Gaussian PDFs, setting $\kappa = 3 - N$ is a good choice.

Transforming

Pass each of the $2N + 1$ sigma points through the nonlinear function $\mathbf{h}(x)$

$$ \mathbf{y}_{i}=\mathbf{h}\left(\mathbf{x}_{i}\right) \quad i=0, \ldots, 2 N $$

Recombining

Compute the mean and covariance of the output PDF

mean
$$ \boldsymbol{\mu}_{y}=\sum_{i=0}^{2 N} \alpha_{i} \mathbf{y}_{i} $$
covariance
$$ \boldsymbol{\Sigma}_{y y}=\sum_{i=0}^{2 N} \alpha_{i}\left(\mathbf{y}_{i}-\boldsymbol{\mu}_{y}\right)\left(\mathbf{y}_{i}-\boldsymbol{\mu}_{y}\right)^{T} $$

with weights

$$ \alpha_{i}=\left\{\begin{array}{lr} \frac{\kappa}{N+\kappa} & i=0 \\ \frac{1}{2} \frac{1}{N+\kappa} & \text { otherwise } \end{array}\right. $$

Example

Compared to linearization (red), we can see the unscented transform (orange) gives a much better approximation!

The Unscented Kalman Filter (UKF)

💡Idea: Instead of approximating the system equations by linearizing, we will calculate sigma points and use the Unscented Transform to approxiamte the PDFs directly.

Prediction step

To propagate the state from time $k-1$ to time $k$, apply the Unscented Transform using the current best guess for the mean and covariance.

Compute sigma points
$$ \begin{array}{rlrl} \hat{\mathbf{L}}_{k-1} \hat{\mathbf{L}}_{k-1}^{T} & =\hat{\mathbf{P}}_{k-1} & (\text{Cholesky Decomposition})& \\ \hat{\mathbf{x}}_{k-1}^{(0)} & =\hat{\mathbf{x}}_{k-1} & & \\ \hat{\mathbf{x}}_{k-1}^{(i)} & =\hat{\mathbf{x}}_{k-1}+\sqrt{N+\kappa} \operatorname{col}_{i} \hat{\mathbf{L}}_{k-1} & i=1 \ldots N \\ \hat{\mathbf{x}}_{k-1}^{(i+N)} & =\hat{\mathbf{x}}_{k-1}-\sqrt{N+\kappa} \operatorname{col}_{i} \hat{\mathbf{L}}_{k-1} & i=1 \ldots N \end{array} $$
Propagate sigma points
$$ \breve{\mathbf{x}}_{k}^{(i)}=\mathbf{f}_{k-1}\left(\hat{\mathbf{x}}_{k-1}^{(i)}, \mathbf{u}_{k-1}, \mathbf{0}\right) \quad i=0 \ldots 2 N $$
Compute predicted mean and covariance (under the assumption of additive noise)
$$ \begin{aligned} \alpha^{(i)} &=\left\{\begin{array}{ll} \frac{\kappa}{N+\kappa} & i=0 \\ \frac{1}{2} \frac{1}{N+\kappa} & \text { otherwise } \end{array}\right.\\\\ \check{\mathbf{x}}_{k} &=\sum_{i=0}^{2 N} \alpha^{(i)} \check{\mathbf{x}}_{k}^{(i)} \\\\ \check{\mathbf{P}}_{k} &=\sum_{i=0}^{2 N} \alpha^{(i)}\left(\check{\mathbf{x}}_{k}^{(i)}-\check{\mathbf{x}}_{k}\right)\left(\check{\mathbf{x}}_{k}^{(i)}-\check{\mathbf{x}}_{k}\right)^{T}+\mathbf{Q}_{k-1} \end{aligned} $$

Correction step

To correct the state estimate using measurement at time $k$, use the nonlinear measurement model and the sigma points from the prediction step to predict the measurements.

Predict measurements from propagated sigma points
$$ \hat{\mathbf{y}}_{k}^{(i)}=\mathbf{h}_{k}\left(\check{\mathbf{x}}_{k}^{(i)}, \mathbf{0}\right) \quad i=0 \ldots 2 N $$
Estimate mean and covariance of predicted measurements
$$ \begin{array}{l} \hat{\mathbf{y}}_{k}=\displaystyle \sum_{i=0}^{2 N} \alpha^{(i)} \hat{\mathbf{y}}_{k}^{(i)} \\ \mathbf{P}_{y}=\displaystyle\sum_{i=0}^{2 N} \alpha^{(i)}\left(\hat{\mathbf{y}}_{k}^{(i)}-\hat{\mathbf{y}}_{k}\right)\left(\hat{\mathbf{y}}_{k}^{(i)}-\hat{\mathbf{y}}_{k}\right)^{T}+\mathbf{R}_{k} \end{array} $$
Compute cross-covariance and Kalman Gain
$$ \begin{aligned} \mathbf{P}_{x y} &=\sum_{i=0}^{2 N} \alpha^{(i)}\left(\check{\mathbf{x}}_{k}^{(i)}-\check{\mathbf{x}}_{k}\right)\left(\hat{\mathbf{y}}_{k}^{(i)}-\hat{\mathbf{y}}_{k}\right)^{T} \\\\ \mathbf{K}_{k} &=\mathbf{P}_{x y} \mathbf{P}_{y}^{-1} \end{aligned} $$
Compute corrected mean and covariance
$$ \begin{array}{l} \hat{\mathbf{x}}_{k}=\check{\mathbf{x}}_{k}+\mathbf{K}_{k}\left(\mathbf{y}_{k}-\hat{\mathbf{y}}_{k}\right) \\ \hat{\mathbf{P}}_{k}=\check{\mathbf{P}}_{k}-\mathbf{K}_{k} \mathbf{P}_{y} \mathbf{K}_{k}^{T} \end{array} $$

UKF Example

Prediction step

2D state estimate $\Rightarrow N=2, \quad \kappa=3-N=1$

Compute sigma points
$$ \begin{aligned} &\hat{\mathbf{L}}_{0} \hat{\mathbf{L}}_{0}^{T}=\hat{\mathbf{P}}_{0} \\ &{\left[\begin{array}{cc} 0.1 & 0 \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} 0.1 & 0 \\ 0 & 1 \end{array}\right]^{T}=\left[\begin{array}{cc} 0.01 & 0 \\ 0 & 1 \end{array}\right]} \end{aligned} $$ $$ \begin{aligned} \hat{\mathbf{x}}_{0}^{(0)} &=\hat{\mathbf{x}}_{0} \\ \hat{\mathbf{x}}_{0}^{(i)} &=\hat{\mathbf{x}}_{0}+\sqrt{N+\kappa} \operatorname{col}_{i} \hat{\mathbf{L}}_{0} \quad i=1 \ldots, N \\ \hat{\mathbf{x}}_{0}^{(i+N)} &=\hat{\mathbf{x}}_{0}-\sqrt{N+\kappa} \operatorname{col}_{i} \hat{\mathbf{L}}_{0} \quad i=1, \ldots, N \\ \hat{\mathbf{x}}_{0}^{(0)} &=\left[\begin{array}{l} 0 \\ 5 \end{array}\right] \\ \hat{\mathbf{x}}_{0}^{(1)} &=\left[\begin{array}{l} 0 \\ 5 \end{array}\right]+\sqrt{3}\left[\begin{array}{c} 0.1 \\ 0 \end{array}\right]=\left[\begin{array}{c} 0.2 \\ 5 \end{array}\right] \\ \hat{\mathbf{x}}_{0}^{(2)} &=\left[\begin{array}{l} 0 \\ 5 \end{array}\right]+\sqrt{3}\left[\begin{array}{c} 0 \\ 1 \end{array}\right]=\left[\begin{array}{c} 0 \\ 6.7 \end{array}\right] \\ \hat{\mathbf{x}}_{0}^{(3)} &=\left[\begin{array}{l} 0 \\ 5 \end{array}\right]-\sqrt{3}\left[\begin{array}{c} 0.1 \\ 0 \end{array}\right]=\left[\begin{array}{c} -0.2 \\ 5 \end{array}\right] \\ \hat{\mathbf{x}}_{0}^{(4)} &=\left[\begin{array}{l} 0 \\ 5 \end{array}\right]-\sqrt{3}\left[\begin{array}{l} 0 \\ 1 \end{array}\right]=\left[\begin{array}{c} 0 \\ 3.3 \end{array}\right] \end{aligned} $$
Propagate sigma points
$$ \begin{aligned} &\check{\mathbf{x}}_{1}^{(i)}=\mathbf{f}_{0}\left(\hat{\mathbf{x}}_{0}^{(i)}, \mathbf{u}_{0}, \mathbf{0}\right) \quad i=0, \ldots, 2 N \\ &\check{\mathbf{x}}_{1}^{(0)}=\left[\begin{array}{cc} 1 & 0.5 \\ 0 & 1 \end{array}\right]\left[\begin{array}{c} 0 \\ 5 \end{array}\right]+\left[\begin{array}{c} 0 \\ 0.5 \end{array}\right](-2)=\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right] \\ &\check{\mathbf{x}}_{1}^{(1)}=\left[\begin{array}{cc} 1 & 0.5 \\ 0 & 1 \end{array}\right]\left[\begin{array}{c} 0.2 \\ 5 \end{array}\right]+\left[\begin{array}{c} 0 \\ 0.5 \end{array}\right](-2)=\left[\begin{array}{c} 2.7 \\ 4 \end{array}\right] \\ &\check{\mathbf{x}}_{1}^{(2)}=\left[\begin{array}{cc} 1 & 0.5 \\ 0 & 1 \end{array}\right]\left[\begin{array}{c} 0 \\ 6.7 \end{array}\right]+\left[\begin{array}{c} 0 \\ 0.5 \end{array}\right](-2)=\left[\begin{array}{c} 3.4 \\ 5.7 \end{array}\right] \\ &\check{\mathbf{x}}_{1}^{(3)}=\left[\begin{array}{cc} 1 & 0.5 \\ 0 & 1 \end{array}\right]\left[\begin{array}{c} -0.2 \\ 5 \end{array}\right]+\left[\begin{array}{c} 0 \\ 0.5 \end{array}\right](-2)=\left[\begin{array}{c} 2.3 \\ 4 \end{array}\right] \\ &\check{\mathbf{x}}_{1}^{(4)}=\left[\begin{array}{cc} 1 & 0.5 \\ 0 & 1 \end{array}\right]\left[\begin{array}{c} 0 \\ 3.3 \end{array}\right]+\left[\begin{array}{c} 0 \\ 0.5 \end{array}\right](-2)=\left[\begin{array}{c} 1.6 \\ 2.3 \end{array}\right] \end{aligned} $$
Compute predicted mean and covariance (under the assumption of additive noise)
$$ \begin{aligned} \alpha^{(i)} &=\left\{\begin{array}{l} \frac{\kappa}{N+\kappa}=\frac{1}{2+1}=\frac{1}{3} \quad i=0 \\ \frac{1}{2} \frac{1}{N+\kappa}=\frac{1}{2} \frac{1}{2+1}=\frac{1}{6} \quad \text { otherwise } \end{array}\right.\\\\ \\ \check{\mathbf{x}}_{k}&=\sum_{i=0}^{2 N} \alpha^{(i)} \check{\mathbf{x}}_{k}^{(i)} \\ &=\frac{1}{3}\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]+\frac{1}{6}\left[\begin{array}{c} 2.7 \\ 4 \end{array}\right]+\frac{1}{6}\left[\begin{array}{l} 3.4 \\ 5.7 \end{array}\right]+\frac{1}{6}\left[\begin{array}{c} 2.3 \\ 4 \end{array}\right]+\frac{1}{6}\left[\begin{array}{l} 1.6 \\ 2.3 \end{array}\right]=\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right] \end{aligned} $$ $$ \begin{aligned} \check{\mathbf{P}}_{k}=& \sum_{i=0}^{2 N} \alpha^{(i)}\left(\check{\mathbf{x}}_{k}^{(i)}-\check{\mathbf{x}}_{k}\right)\left(\check{\mathbf{x}}_{k}^{(i)}-\check{\mathbf{x}}_{k}\right)^{T}+\mathbf{Q}_{k-1} \\ =& \frac{1}{3}\left(\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]-\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]\right)\left(\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]-\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]\right)^{T}+\\ & \frac{1}{6}\left(\left[\begin{array}{c} 2.7 \\ 4 \end{array}\right]-\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]\right)\left(\left[\begin{array}{c} 2.7 \\ 4 \end{array}\right]-\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]\right)^{T}+\frac{1}{6}\left(\left[\begin{array}{c} 3.4 \\ 5.7 \end{array}\right]-\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]\right)\left(\left[\begin{array}{c} 3.4 \\ 5.7 \end{array}\right]-\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]\right)^{T}+\\ & \frac{1}{6}\left(\left[\begin{array}{c} 2.3 \\ 4 \end{array}\right]-\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]\right)\left(\left[\begin{array}{c} 2.3 \\ 4 \end{array}\right]-\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]\right)^{T}+\frac{1}{6}\left(\left[\begin{array}{c} 1.6 \\ 2.3 \end{array}\right]-\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]\right)\left(\left[\begin{array}{c} 1.6 \\ 2.3 \end{array}\right]-\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]\right)^{T}+\left[\begin{array}{cc} 0.1 & 0 \\ 0 & 0.1 \end{array}\right] \\ =& {\left[\begin{array}{cc} 0.36 & 0.5 \\ 0.5 & 1.1 \end{array}\right] } \end{aligned} $$

Correction step

Predict measurements from propagated sigma points
Estimate mean and covariance of predicted measurements
Compute cross-covariance and Kalman Gain
Compute corrected mean and covariance

Summary

The UKF use the unscented transform to adpat the Kalman filter to nonlinear systems.
The unscented transform works by passing a small set of carefully chosen samples through a nonlinear system, and computing the mean and covariance of the outputs.
The unscented transform does a better job of approixmating the output distribution than analytical local linearization, for similar computational cost

Comparision of Nonlinear KF

Reference

An Alternative to the EKF - The Unscented Kalman Filter