I have implemented a 2-D verion of your problem in Matlab, the code follows:
Originally Posted by atlove
This generates a data set such that the third element is the sum of the first two plus a normally distributed random number of unit variance.
(the model is , I use d here as x is usually reserved for the state )
The above is the Kalman innovation process straight from the book X on input is the old state estimate, and on exit the new state estimate, and P on input is the prior state covariance matrix and on exit is the posterior state estimate.
% Kalman Filter Inovation Processing
H=[Data(1),Data(2)]; %extract the H matrix or vector from the data
z=Data(3); %extract the measurement from the data
% Below is the standard Kalman inovation equations in matrix form
S=H*P*H'+R; %note this is a scalar
The above loops over 10 sets of data, plotting the rows of the returned matrix will show the evolution of the state estimate, which tend to [1,1]'
P=[100,0;0,100]; %initial covariance of state estimate
X=[0;0]; %initial state estimate
R=1; %measurement variance
D=DataGen; % generate some data
rv=[rv,X]; %accumulate the state estimate
Note there is no need for a dynamic with this model.
The attached figure shows the evolution of the components of the state estimate.