Scribd
Upload
10 views

Control System Homework 2

The document defines controllability and observability in state-space systems, emphasizing their mathematical conditions. It explains the Kalman filter as a recursive algorithm for estimating the state of dynamic systems from noisy measurements, detailing its prediction and update steps. Additionally, it discusses the implementation of a Kalman filter in Python and its applications in autonomous vehicles for localization, sensor fusion, object tracking, and path prediction.

Uploaded by

1903batuhancftc
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
10 views

Control System Homework 2

The document defines controllability and observability in state-space systems, emphasizing their mathematical conditions. It explains the Kalman filter as a recursive algorithm for estimating the state of dynamic systems from noisy measurements, detailing its prediction and update steps. Additionally, it discusses the implementation of a Kalman filter in Python and its applications in autonomous vehicles for localization, sensor fusion, object tracking, and path prediction.

Uploaded by

1903batuhancftc
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
You are on page 1/ 5

1.

Define Controllability and Observability

Controllability:

Controllability of a system refers to the ability to drive the system from


any initial state to any desired state within a finite time using an
appropriate input signal.

 Mathematical Condition: A state-space system is controllable if


the controllability matrix CCC has full rank: C=[B,AB,A2B,
…,An−1B]C = [B, AB, A^2B, \dots, A^{n-1}B]C=[B,AB,A2B,
…,An−1B] where:

o AAA is the state matrix,

o BBB is the input matrix,

o nnn is the number of states.

Observability:

Observability refers to the ability to determine the state of a system from


the outputs over time.

 Mathematical Condition: A state-space system is observable if the


observability matrix OOO has full rank: O=[CCACA2⋮CAn−1]O = \
begin{bmatrix} C \\ CA \\ CA^2 \\ \vdots \\ CA^{n-1} \
end{bmatrix}O=CCACA2⋮CAn−1 where:

o CCC is the output matrix,

o AAA is the state matrix.

2. Describe the Kalman Filter

Purpose:

The Kalman filter is a recursive algorithm used to estimate the state of a


dynamic system from noisy measurements. It combines predictions from a
model with observations to produce more accurate estimates.

How it Works:

1. Prediction Step:

o Predict the next state and its covariance based on the current
state and process model.
x^k∣k−1=Ax^k−1+Buk\hat{x}_{k|k-1} = A \hat{x}_{k-1} + B
u_kx^k∣k−1=Ax^k−1+Buk Pk∣k−1=APk−1AT+QP_{k|k-1} = A P_{k-1}
A^T + QPk∣k−1=APk−1AT+Q

where:

o x^k∣k−1\hat{x}_{k|k-1}x^k∣k−1: Predicted state,

o Pk∣k−1P_{k|k-1}Pk∣k−1: Predicted covariance,

o QQQ: Process noise covariance.

2. Update Step:

o Update the predicted state using the new measurement.

Kk=Pk∣k−1HT(HPk∣k−1HT+R)−1K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T +


R)^{-1}Kk=Pk∣k−1HT(HPk∣k−1HT+R)−1
x^k=x^k∣k−1+Kk(zk−Hx^k∣k−1)\hat{x}_k = \hat{x}_{k|k-1} + K_k (z_k -
H \hat{x}_{k|k-1})x^k=x^k∣k−1+Kk(zk−Hx^k∣k−1)
Pk=(I−KkH)Pk∣k−1P_k = (I - K_k H) P_{k|k-1}Pk=(I−KkH)Pk∣k−1

where:

o KkK_kKk: Kalman gain,

o zkz_kzk: Measurement,

o HHH: Measurement matrix,

o RRR: Measurement noise covariance.

Applications:

 Tracking and navigation systems,

 Sensor fusion in robotics and autonomous vehicles.

3. Implementation

Here's a Python implementation for the 1D motion model with a Kalman


filter:

Python Code:

import numpy as np

import matplotlib.pyplot as plt


# Simulate the system

np.random.seed(42)

dt = 1.0 # Time step

# Define system matrices

A = np.array([[1]]) # State transition matrix

B = np.array([[0]]) # Control input matrix

H = np.array([[1]]) # Measurement matrix

Q = np.array([[1e-5]]) # Process noise covariance

R = np.array([[0.1]]) # Measurement noise covariance

# Generate true state and noisy measurements

n_steps = 50

true_state = np.zeros((n_steps, 1))

measurements = np.zeros((n_steps, 1))

for k in range(1, n_steps):

true_state[k] = A @ true_state[k-1] + np.random.normal(0,


np.sqrt(Q[0, 0]))

measurements[k] = H @ true_state[k] +
np.random.normal(0, np.sqrt(R[0, 0]))

# Kalman filter

estimated_state = np.zeros_like(true_state)

P = np.eye(1) # Initial estimate covariance

for k in range(1, n_steps):

# Prediction step

x_pred = A @ estimated_state[k-1]

P_pred = A @ P @ A.T + Q
# Update step

K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)

estimated_state[k] = x_pred + K @ (measurements[k] - H @


x_pred)

P = (np.eye(1) - K @ H) @ P_pred

# Plot results

plt.figure(figsize=(10, 6))

plt.plot(true_state, label="True State")

plt.plot(measurements, label="Noisy Measurements",


linestyle="dotted")

plt.plot(estimated_state, label="Estimated State",


linestyle="dashed")

plt.legend()

plt.xlabel("Time step")

plt.ylabel("State")

plt.title("Kalman Filter: True State vs Noisy Measurements vs


Estimated State")

plt.show()56

4. Application in Autonomous Vehicles

Kalman Filtering is widely used in autonomous vehicles for:

 Localization: Estimating the position and velocity of the vehicle


using GPS and IMU (Inertial Measurement Unit) data.

 Sensor Fusion: Combining data from multiple sensors (e.g., LiDAR,


radar, cameras) to get a more accurate picture of the environment.

 Object Tracking: Tracking the movement of other vehicles,


pedestrians, or objects in the vicinity.

 Path Prediction: Estimating the future path of moving objects.

Example:
In a self-driving car, GPS data might be inaccurate due to noise, and IMU
data might drift over time. The Kalman filter combines these sources to
provide a reliable estimate of the car's position and velocity.

You might also like