Kalman Filter

Pada bilgin.esme.org/BitsBytes/KalmanFilterforDummies.aspx kalman filter mulai dijelaskan dari bentuk yg sederhana agar lebih mudah dimengerti terutama untuk belajar awal.kalman1

kalman2

Pada page tersebut dijelaskan dengan sederhana tentang kalman filter. Pada bagian akhir ada sebuah contoh perhitungan untuk menghitung sebuah model sederhana 1 dimensi kalman filter.

kalman3

kalman4

kalman5

Dari contoh tersebut saya mencoba mensimulasikan konsepnya pada MATLAB dengan input random. Berikut program yang saya buat.

clc
clear all
close all
xk = 0;
Pk = 1;
R = 0.1;
buff_xk = [];
buff_zk = [];
pure_zk = [];
for k=1:100

    zk = (rand-rand);

    Pk = Pk + 0.000001;

    Kk = Pk/(Pk+R);
    xk = xk + (Kk*(zk - xk));
    Pk = (1-Kk)*Pk;

    buff_xk = [buff_xk xk];
    pure_zk = [pure_zk 1];
    buff_zk = [buff_zk zk];
end
figure;
plot(buff_xk,'r');
hold on;
plot(buff_zk,'g');
plot(pure_zk,'b');

Hasil plot

kalman6

Hasilnya cukup lumayan.

Berikut saya coba simulasikan untuk aplikasi mikrokontroler.

kalman7

Dan Berikut kodenya.

/*****************************************************
Chip type           : ATmega32
Program type        : Application
Clock frequency     : 16.000000 MHz
Memory model        : Small
External SRAM size  : 0
Data Stack size     : 512
*****************************************************/

#include

#define RXB8 1
#define TXB8 0
#define UPE 2
#define OVR 3
#define FE 4
#define UDRE 5
#define RXC 7

#define FRAMING_ERROR (1<<FE)
#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<OVR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)

// USART Receiver buffer
#define RX_BUFFER_SIZE 8
char rx_buffer[RX_BUFFER_SIZE];

#if RX_BUFFER_SIZE<256
unsigned char rx_wr_index,rx_rd_index,rx_counter;
#else
unsigned int rx_wr_index,rx_rd_index,rx_counter;
#endif

// This flag is set on USART Receiver buffer overflow
bit rx_buffer_overflow;

// USART Receiver interrupt service routine
interrupt [USART_RXC] void usart_rx_isr(void)
{
char status,data;
status=UCSRA;
data=UDR;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
   {
   rx_buffer[rx_wr_index]=data;
   if (++rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0;
   if (++rx_counter == RX_BUFFER_SIZE)
      {
      rx_counter=0;
      rx_buffer_overflow=1;
      };
   };
}

#ifndef _DEBUG_TERMINAL_IO_
// Get a character from the USART Receiver buffer
#define _ALTERNATE_GETCHAR_
#pragma used+
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index];
if (++rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#asm("cli")
--rx_counter;
#asm("sei")
return data;
}
#pragma used-
#endif

// USART Transmitter buffer
#define TX_BUFFER_SIZE 8
char tx_buffer[TX_BUFFER_SIZE];

#if TX_BUFFER_SIZE<256
unsigned char tx_wr_index,tx_rd_index,tx_counter;
#else
unsigned int tx_wr_index,tx_rd_index,tx_counter;
#endif

// USART Transmitter interrupt service routine
interrupt [USART_TXC] void usart_tx_isr(void)
{
if (tx_counter)
   {
   --tx_counter;
   UDR=tx_buffer[tx_rd_index];
   if (++tx_rd_index == TX_BUFFER_SIZE) tx_rd_index=0;
   };
}

#ifndef _DEBUG_TERMINAL_IO_
// Write a character to the USART Transmitter buffer
#define _ALTERNATE_PUTCHAR_
#pragma used+
void putchar(char c)
{
while (tx_counter == TX_BUFFER_SIZE);
#asm("cli")
if (tx_counter || ((UCSRA & DATA_REGISTER_EMPTY)==0))
   {
   tx_buffer[tx_wr_index]=c;
   if (++tx_wr_index == TX_BUFFER_SIZE) tx_wr_index=0;
   ++tx_counter;
   }
else
   UDR=c;
#asm("sei")
}
#pragma used-
#endif

// Standard Input/Output functions
#include

// Declare your global variables here
float sensor[10] = {    0.39,
        	        0.50,
        	        0.48,
        	        0.29,
        	        0.25,
        	        0.32,
        	        0.34,
        	        0.48,
        	        0.41,
        	        0.45};
float   Kk,
        Xk_topi,
        Pk,
        R = 0.1,
        Q = 0,
        Xk_topi_prev    = 0,
        Pk_prev         = 1,
        Pk_update,
        Xk_topi_update;
float kalman_filter_ku(float Zk)
{
//Time Update
        Xk_topi_update = Xk_topi_prev;
        Pk_update = Pk_prev + Q;
//Measurement Update
        Kk = Pk_update/(Pk_update + R);
        Xk_topi = Xk_topi_update + (Kk*(Zk-Xk_topi_update));
        Pk = (1-Kk)*Pk_update;
        Xk_topi_prev = Xk_topi;
        Pk_prev = Pk;
        return(Xk_topi);
}

void main(void)
{
// Declare your local variables here
int i;
// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x00;

// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD=0x00;
DDRD=0x00;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=FFh
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer 1 Stopped
// Mode: Normal top=FFFFh
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer 1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer 2 Stopped
// Mode: Normal top=FFh
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// INT2: Off
MCUCR=0x00;
MCUCSR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;

// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud rate: 9600
UCSRA=0x00;
UCSRB=0xD8;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x67;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

// Global enable interrupts
#asm("sei")

for(i=0;i<10;i++)
{
        printf("Sensor:%f\n\r",sensor[i]);
        printf("Kalman:%f\n\r\n\r",kalman_filter_ku(sensor[i]));
}

while (1)
      {
      // Place your code here

      };
}

Secara proses dan hasil, memang kurang memuaskan karena disain program dan sensor masih jauh dari kriteria sebuah aplikasi kalman filter.
Iseng-iseng saya coba aplikasikan kodingan di atas untuk memfilter data accelerometer pada modul STM32F4 Discovery dengan cara mengirim data hasil pengukuran via serial port kemudian ditampilkan dengan aplikasi yang dibuat dengan Visual C# Express 2010.

kalman-kalman-an_3
Berikut hasilnya.

kalman-kalman-an_1
Dan saya coba aplikasikan untuk mengontrol sebuah game di PC dengan bantuan library Vjoystick.

kalman-kalman-an_2

Berikut videonya.

++++++++++++++++++++++++++++++++++++

DAFTAR PUSTAKA

cs.cornell.edu/Courses/cs4758/2012sp/materials/MI63slides.pdf

bilgin.esme.org/BitsBytes/KalmanFilterforDummies.aspx

One Response to Kalman Filter

  1. Pingback: Membuat Virtual Joystik C# | WanGReadY

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s