Kalman Filter Matlab Code

clc

clear all

close all

warning off

error = 2;

Xnp = [1 2 3 4];

Zn = [1.1 1.9 3 4];

si = length(Xnp);

A = zeros(si,si);

P = zeros(si,si);

Q = zeros(si,si);

H = zeros(si,si);

R = zeros(si,si)*error;

I = zeros(si,si);

for i = 1:si

    A(i,i) = 1;

    P(i,i) = 1;

    H(i,i) = 1;

    R(i,i) = 1;

    I(i,i) = 1;

end

Xn = Xnp*A;

Pn = (A*P*A')+Q;

prevXn = [];

while(true)

    Kk = (Pn*H')*(((H*Pn*H')+R)');

    Xn = Xn+((Zn-(Xn*H))*Kk);

    Pn = (I-(Kk*H))*Pn;

    if(isempty(prevXn))

        prevXn = Xn;

    else

        con = sum(abs(Xn-prevXn));

        if(con==0)

            break;

        end

    end

end

FXn = Xn

Kindly Bookmark and Share it: