组合导航matlab程序,组合导航的卡尔曼滤波器MATLAB程序

function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin)

% Kalman filter.

% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...)

%

% INPUTS:

% y(:,t)   - the observation at time t

% A - the system matrix

% C - the observation matrix

% Q - the system covariance

% R - the observation covariance

% init_x - the initial state (column) vector

% init_V - the initial state covariance

%

% OPTIONAL INPUTS (string/value pairs [default in brackets])

% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ]

%     In this case, all the above matrices take an additional final dimension,

%     i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m).

%     However, init_x and init_V are independent of model(1).

% 'u'     - u(:,t) the control signal at time t [ [] ]

% 'B'     - B(:,:,m) the input regression matrix for model m

%

% OUTPUTS (where X is the hidden state being estimated)

% x(:,t) = E[X(:,t) | y(:,1:t)]

% V(:,:,t) = Cov[X(:,t) | y(:,1:t)]

% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2

% loglik = sum{t=1}^T log P(y(:,t))

%

% If an input signal is specified, we also condition on it:

% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)]

% If a model sequence is specified, we also condition on it:

% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)]

clear

Ak=exp(-0.02);                          %各系数由前面确定；

Ck=1;

Qk=1-exp(-0.04);

Rk=1;

p(1)=1;                                  %各初值；

p1(1)=Ak*p(1)*Ak'+Qk;                    %由p1代表p'；

x(1)=0;                                  %设信号初值为0；

H(1)=p1(1)*Ck'*inv(Ck*p1(1)*Ck'+Rk);

[os T] = size(y);

ss = size(A,1); % size of state space

% set default params

model = ones(1,T);

u = [];

B = [];

ndx = [];

args = varargin;

nargs = length(args);

for i=1:2:nargs

switch args{i}

case 'model', model = args{i+1};

case 'u', u = args{i+1};

case 'B', B = args{i+1};

case 'ndx', ndx = args{i+1};

otherwise, error(['unrecognized argument ' args{i}])

end

end

x = zeros(ss, T);

V = zeros(ss, ss, T);

VV = zeros(ss, ss, T);

loglik = 0;

for t=1:T

m = model(t);

if t==1

%prevx = init_x(:,m);

%prevV = init_V(:,:,m);

prevx = init_x;

prevV = init_V;

initial = 1;

else

prevx = x(:,t-1);

prevV = V(:,:,t-1);

initial = 0;

end

if isempty(u)

[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...

kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial);

else

if isempty(ndx)

[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...

kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ...

'initial', initial, 'u', u(:,t), 'B', B(:,:,m));

else

i = ndx{t};

% copy over all elements; only some will get updated

x(:,t) = prevx;

prevP = inv(prevV);

prevPsmall = prevP(i,i);

prevVsmall = inv(prevPsmall);

[x(i,t), smallV, LL, VV(i,i,t)] = ...

kalman_update(A(i,i,m), C(:,i,m), Q(i,i,m), R(:,:,m), y(:,t), prevx(i), prevVsmall, ...

'initial', initial, 'u', u(:,t), 'B', B(i,:,m));

smallP = inv(smallV);

prevP(i,i) = smallP;

V(:,:,t) = inv(prevP);

end

end

loglik = loglik + LL;

end