Pong game using Spartan 3e-500

Tracking the Trajectory of a mass using linear Kalman filter

The Kalman filter is an algorithm that estimates the state of a process from measured data. The word filter comes because of the estimations given for unknown variables, are more precise than noisy, inaccurate measurements taken from the environment. Kalman filter algorithm is a two stage process. The first stage (prediction) predicts the state of the system and the second stage (observation and update) updates the predicted state using the observed noisy measurements. In this post I explain an application of tracking the trajectory of a mass using linear Kalman filter. First let's have an idea about the kinematic model of a trajectory. 

Initial conditions

Now, let's look at the discrete linear Kalman filter equations.

Matlab code

 clear all; close all; clc;  
 N = 145; % no of iterations  
   
 %% parameters  
 v = 100; % initial velocity  
 T = 14.42096; % flight duration  
 deltaT = 0.1; % time slice  
 g = 9.80665;   
 theta = pi/4; % angle from the ground  
 F = [1 deltaT 0 0; 0 1 0 0; 0 0 1 deltaT; 0 0 0 1]; % state transition model  
 B = [0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1]; % control input matrix  
 u = [0; 0; 0.5*(-g)*(deltaT^2); (-g)*deltaT]; % control vector  
 H = [1 0 0 0; 0 0 1 0]; % observation matrix  
 phat = eye(4); % initial predicted estimate covariance  
 Q = zeros(4); % process covariance  
 R = 0.2*eye(2); % measurement error covariance  
 xhat = [0; v*cos(theta); 200; v*sin(theta)]; % initial estimate  
 x_estimate = xhat;  
   
 %% system model  
 t = 0:deltaT:T;  
 x_sys = zeros(2,N);  
 x_sys(1,:) = v*cos(theta)*t ;  
 x_sys(2,:) = v*(sin(theta)*t)-(0.5*g*(t.^2));  
   
 %% noisy measurements  
 sigma = 25;  
 z = x_sys + sigma*randn(2,N);  
   
 %% kalman filter  
 for i=1:N  
   % prediction  
   x_pred = F*xhat + B*u; % project the state ahead  
   p_pred = F*phat*F' + Q; % project the error covariance  
     
   % observation and update  
   kalman_gain = (p_pred*H')/(H*p_pred*H'+R); % compute the Kalman gain  
   xhat = x_pred + kalman_gain*(z(:,i)-H*x_pred); % update estimate with measurement z  
   phat = (eye(4)-kalman_gain*H)*p_pred; % update error covariance   
   x_estimate = [x_estimate xhat]; %#ok[agrow] (to ignore array grow in loop warning)  
 end  
   
 %% plot results  
 figure; hold on;  
 plot(x_sys(1,:),x_sys(2,:),'k');  
 plot(z(1,:),z(2,:),'.b');  
 plot(x_estimate(1,:),x_estimate(3,:),'--r');  
   
 xlabel('X (meters)');  
 ylabel('Y (meters)');  
 title('Trajectory with linear Kalman filter');  
 legend('System model', 'Measured', 'Estimated');  

Simulation Results

 * You can download the matlab file from here.

How to add an echo effect to an audio signal using Matlab

In this post I explain how to add an echo to an audio signal using Matlab. If you closely look at the below code, you can understand, what kind of a process is there. Initially the original signal x is delayed by 0.5 seconds and then multiplied by the attenuation constant alpha(0.65) to reduce the amplitude of the echo signal. Finally the delayed and attenuated signal is added back to the original signal to get the echo effect of the audio signal. You can visualize this process using the below mentioned Matlab simulink model.

Matlab code

 clear all;  
 %% Hallelujah Chorus  
 [x,Fs] = audioread('Hallelujah.wav');  
 sound(x,Fs);  
 pause(10);  
 delay = 0.5; % 0.5s delay  
 alpha = 0.65; % echo strength  
 D = delay*Fs;  
 y = zeros(size(x));  
 y(1:D) = x(1:D);  
   
 for i=D+1:length(x)  
   y(i) = x(i) + alpha*x(i-D);  
 end  
   
 %% using filter method.  
 % b = [1,zeros(1,D),alpha];  
 % y = filter(b,1,x);  
   
 %% echoed Hallelujah Chorus  
 sound(y,Fs);  

 * You can download Hallelujah.wav from here or else you can find more .wav files from WavSource.com. Varying the value of alpha would change the echo strength of the audio signal.

Original and echoed audio signals in Audacity

You can create the Matlab simulink model for echo generation as follows.

Matlab simulink model for echo generation

Model parameters

• From Multimedia File 

samples per audio channel - 8192
Audio output sampling mode - Frame based
Audio output data type - double

• Gain

 Gain - 0.65

• Delay

Delay length - 4096
Input Processing - Columns as channels (frame based)

* If you use any other .wav file other than Hallelujah.wav, then you should change the above parameters accordingly.

Frequency Divider

A frequency divider, also called a clock divider or scaler or prescaler, is a circuit that takes an input signal of a frequency, fin, and generates an output signal of a frequency :

Source - Wikipedia

Where n is an integer (Wikipedia). In this post I explain how to implement the digital design of a simple clock divider(fin/2).

Source - ElectronicsTutorials

Verilog module

 module clock_divider (clk_in, enable,reset, clk_out);  
   input clk_in; // input clock  
   input reset;  
   input enable;  
   output clk_out; // output clock  
   
   wire  clk_in;  
   wire  enable;  
   
   reg clk_out;  
   
   always @ (posedge clk_in)  
     if (reset)  
       begin  
         clk_out <= 1'b0;  
       end  
     else if (enable)  
       begin  
         clk_out <= !clk_out ;  
       end  
   
 endmodule  

Test-bench

 module tb_clock_divider;  
   reg clk_in, reset,enable;  
   wire clk_out;  
     
   clock_divider U0 (  
    .clk_in (clk_in),  
    .enable(enable),  
    .reset (reset),  
    .clk_out (clk_out)  
   );  
     
   initial  
     begin  
       clk_in = 0;  
       reset = 0;  
       enable = 1;  
       #10 reset = 1;  
       #10 reset = 0;  
       #100 $finish;  
     end  
       
   always #5 clk_in = ~clk_in;  
     
 endmodule  

Simulation results

Elaborated design

Verilog simulation and RTL analysis was done in Vivado 2014.2. If you want to divide the input frequency further, (fin/4, fin/8, fin/16), you can extend the same circuit as follows.
 

Source - ElectronicsTutorials

Merge Sort

The basic idea of the algorithm is to split the collection into smaller groups by halving it until the groups only have one element or no elements (which are both entirely sorted groups). Then merge the groups back together so that their elements are in order (Rosetta code). Merge sort follows divide-and-conquer approach.

Worst case performance - O(n log n)

Source - Wikipedia

 C++

 void merge_sort(int array[], int low, int high)  
 {  
   if (low < high)  
   {  
     int middle = (low + high) / 2;  
     merge_sort(array,low, middle);  
     merge_sort(array,middle + 1, high);  
     merge(array, low, middle, high);  
   }  
 }  
   
 void merge(int array[], int low, int middle, int high)  
 {  
   int size1 = middle - low + 1;  
   int size2 = high - middle;  
   int left[size1 + 1];  
   int right[size2 + 1];  
   for (int i = 0; i < size1; i++)  
   {  
     left[i] = array[low + i];  
   }  
   for (int j = 0; j < size2; j++)  
   {  
     right[j] = array[middle + j + 1];  
   }  
   left[size1] = numeric_limits<int>::max();;  
   right[size2] = numeric_limits<int>::max();;  
   int i = 0;  
   int j = 0;  
   for (int k = low; k <= high; k++)  
   {  
     if (left[i] <= right[j])  
     {  
       array[k] = left[i];  
       i++;  
     }  
     else  
     {  
       array[k] = right[j];  
       j++;  
     }  
   }  
 }   

Java

   static int[] array = {5, 4, 8, 3, 1, 2, 9, 6, 7, 10};  
   
   static void merge_sort(int[] array, int low, int high) {  
       if (low < high) {  
         int middle = (low + high) / 2;  
         merge_sort(array, low, middle);  
         merge_sort(array, middle + 1, high);  
         merge(array, low, middle, high);  
       }  
    }  
   
   static void merge(int[] array, int low, int middle, int high) {  
       int size1 = middle - low + 1;  
       int size2 = high - middle;  
       int[] left = new int[size1 + 1];  
       int[] right = new int[size2 + 1];  
       for (int i = 0; i < size1; i++) {  
         left[i] = array[low + i];  
       }  
       for (int j = 0; j < size2; j++) {  
         right[j] = array[middle + j + 1];  
       }  
       left[size1] = Integer.MAX_VALUE;  
       right[size2] = Integer.MAX_VALUE;  
       int i = 0;  
       int j = 0;  
       for (int k = low; k <= high; k++) {  
         if (left[i] <= right[j]) {  
           array[k] = left[i];  
           i++;  
         } else {  
           array[k] = right[j];  
           j++;  
         }  
       }  
    }  

Matlab

   function array = merge_sort(array,low,high)  
     if(low < high)  
       middle = floor((low + high)/2);  
       array = merge_sort(array,low, middle);  
       array = merge_sort(array,middle + 1, high);  
       array = merge(array, low, middle, high);  
     end  
   end  
   
   function array = merge(array,low,middle,high)  
     size1 = middle - low + 1;  
     size2 = high - middle;  
     left = zeros(1,size1+1);  
     right = zeros(1,size2+1);  
     for i=1:size1  
       left(i) = array(low+i-1);  
     end  
     for j=1:size2  
       right(j) = array(middle+j);  
     end  
     left(size1+1) = inf;  
     right(size2+1) = inf;  
     i = 1;  
     j = 1;  
     for k=low:high  
       if left(i)<= right(j)  
         array(k) = left(i);  
         i = i + 1;  
       else  
         array(k) = right(j);  
         j = j + 1;  
       end  
     end  
   end   

Python 

import math

def merge(array,low,middle,high):
    size1 = middle - low + 1
    size2 = high - middle
    left = [0]* (size1+1)
    right = [0]* (size2+1)
    for i in range(size1):
        left[i] = array[low+i]
    for j in range(size2):
        right[j] = array[middle+j+1]
    left[size1] = float("inf")
    right[size2] = float("inf")
    i = 0
    j = 0
    for k in range(low,high+1):
        if left[i]<= right[j]:
            array[k] = left[i]
            i = i + 1
        else:
            array[k] = right[j]
            j = j + 1
   
def merge_sort(array,low,high):
    if low<high:
        middle = int(math.floor((low+high)/2))
        merge_sort(array,low,middle)
        merge_sort(array,middle+1,high)
        merge(array,low,middle,high)

Bubble Sort

Bubble sort is a simple sorting algorithm that repeatedly steps through the list to be sorted, compares each pair of adjacent items and swaps them if they are in the wrong order. The pass through the list is repeated until no swaps are needed, which indicates that the list is sorted. (Wikipedia)

Worst case performance - O(n^2)

Source - Wikipedia

C++

void bubble_sort(int array[], int length)
{
    bool swapped;
    do
    {
        swapped = false;
        for (int i = 1; i < length; i++)
        {
            if (array[i - 1] > array[i])
            {
                int temp = array[i];
                array[i] = array[i - 1];
                array[i - 1] = temp;
                swapped = true;
            }
        }

    }
    while (swapped);
}

Java

public int[] bubble_sort(int[] array) {
    boolean swapped;
    do {
        swapped = false;
        for (int i = 1; i < array.length; i++) {
            if (array[i - 1] > array[i]) {
                int temp = array[i];
                array[i] = array[i - 1];
                array[i - 1] = temp;
                swapped = true;
            }
        }

    } while (swapped);
    return array;
}

Matlab

function array = bubble_sort(array)
    length = numel(array);
    swapped = true;
    while swapped
        swapped = false;
        for i=2:length
            if array(i-1)>array(i)
                array([(i-1) i]) = array([i (i-1)]);
                swapped = true;
            end
        end
    end
end

Python

def bubble_sort(array):
  swapped = True
 
  while swapped:
    swapped = False
   
    for i in range(1,len(array)):
      if array[i-1]>array[i]:
        temp = array[i]
        array[i] = array[i - 1]
        array[i - 1] = temp
        swapped = True
      

More generally, it can happen that more than one element is placed in their final position on a single pass. In particular, after every pass, all elements after the last swap are sorted, and do not need to be checked again. (Wikipedia). Therefore we can further optimize the algorithm. Here is the Java implementation of the optimized algorithm.

 public int[] bubble_sort(int[] array) {
    int n = array.length;
    int newLimit = 0;
    boolean swapped;
    do {
        swapped = false;
        for (int i = 1; i < n; i++) {
            if (array[i - 1] > array[i]) {
                int temp = array[i];
                array[i] = array[i - 1];
                array[i - 1] = temp;
                swapped = true;
                newLimit = i;
            }
        }
        n = newLimit;

    } while (swapped);
    return array;
}

Selection Sort

The main idea of the algorithm is quite simple as follows. The input data array is divided to two imaginary parts, sorted and unsorted. Initially the sorted part is empty. The algorithm searches for the smallest element of the unsorted part and adds it to the end of the sorted part. When the unsorted part becomes empty algorithm stops. However Selection sort is inefficient in sorting large sets.

Worst case performance - O(n^2)

Source - Quazoo

C++

void selection_sort(int array[], int length)
{
    for (int j = 0; j < length - 1; j++)
    {
        int min = j;
        for (int i = j + 1; i < length; i++)
        {
            if (array[i] < array[min])
            {
                min = i;
            }
        }
        if (min != j)
        {
            int temp = array[j];
            array[j] = array[min];
            array[min] = temp;
        }
    }
}

 Java

public int[] selection_sort(int[] array) {
    for (int j = 0; j < array.length - 1; j++) {
        int min = j;
        for (int i = j + 1; i < array.length; i++) {
            if (array[i] < array[min]) {
                min = i;
            }
        }
        if (min != j) {
            int temp = array[j];
            array[j] = array[min];
            array[min] = temp;
        }
    }
    return array;
}

Matlab

function array = selection_sort(array)
    length = numel(array);
    for i = (1:length-1)
        min = i;
        for j = (i+1:length)
            if array(j) <= array(min)
                min = j;
            end
        end
        if i ~= min
            array([min i]) = array([i min]);
        end
    end
end

Python

def selection_sort(array):
  for j in range(len(array)-1):
 
    min = j
    
    for i in range(j+1,len(array)):
      if array[i] < array[min]:
        min = i
        
    if min != j:
      temp = array[j]
      array[j] = array[min]
      array[min] = temp