constaccjac

Jacobian for constant-acceleration motion

Description

example

jacobian = constaccjac(state) returns the updated Jacobian , jacobian, for a constant-acceleration Kalman filter motion model. The step time is one second. The state argument specifies the current state of the filter.

example

jacobian = constaccjac(state,dt) also specifies the time step, dt.

[jacobian,noisejacobian] = constaccjac(state,w,dt) specifies the state noise, w, and returns the Jacobian, noisejacobian, of the state with respect to the noise.

Examples

collapse all

Compute the state Jacobian for two-dimensional constant-acceleration motion.

Define an initial state and compute the state Jacobian for a one second update time.

state = [1,1,1,2,1,0];
jacobian = constaccjac(state)
jacobian = 6×6

1.0000    1.0000    0.5000         0         0         0
0    1.0000    1.0000         0         0         0
0         0    1.0000         0         0         0
0         0         0    1.0000    1.0000    0.5000
0         0         0         0    1.0000    1.0000
0         0         0         0         0    1.0000

Compute the state Jacobian for two-dimensional constant-acceleration motion. Set the step time to 0.5 seconds.

state = [1,1,1,2,1,0].';
jacobian = constaccjac(state,0.5)
jacobian = 6×6

1.0000    0.5000    0.1250         0         0         0
0    1.0000    0.5000         0         0         0
0         0    1.0000         0         0         0
0         0         0    1.0000    0.5000    0.1250
0         0         0         0    1.0000    0.5000
0         0         0         0         0    1.0000

Input Arguments

collapse all

Kalman filter state vector for constant-acceleration motion, specified as a real-valued 3N-element vector. N is the number of spatial degrees of freedom of motion. For each spatial degree of motion, the state vector takes the form shown in this table.

Spatial DimensionsState Vector Structure
1-D[x;vx;ax]
2-D[x;vx;ax;y;vy;ay]
3-D[x;vx;ax;y;vy;ay;z;vz;az]

For example, x represents the x-coordinate, vx represents the velocity in the x-direction, and ax represents the acceleration in the x-direction. If the motion model is in one-dimensional space, the y- and z-axes are assumed to be zero. If the motion model is in two-dimensional space, values along the z-axis are assumed to be zero. Position coordinates are in meters. Velocity coordinates are in meters/second. Acceleration coordinates are in meters/second2.

Example: [5;0.1;0.01;0;-0.2;-0.01;-3;0.05;0]

Data Types: double

Time step interval of filter, specified as a positive scalar. Time units are in seconds.

Example: 0.5

Data Types: single | double

State noise, specified as a scalar or real-valued real valued N-by-1 vector. N is the number of motion dimensions. For example, N = 2 for the 2-D motion. If specified as a scalar, the scalar value is expanded to a N-by-1 vector.

Data Types: single | double

Output Arguments

collapse all

Constant-acceleration motion Jacobian, returned as a real-valued 3N-by-3N matrix.

Constant acceleration motion noise Jacobian, returned as a real-valued 3N-by-N matrix. N is the number of spatial degrees of motion. For example, N = 2 for the 2-D motion. The Jacobian is constructed from the partial derivatives of the state at the updated time step with respect to the noise components.

Algorithms

For a two-dimensional constant-acceleration process, the Jacobian matrix after a time step, T, is block diagonal:

$\left[\begin{array}{cccccc}1& T& \frac{1}{2}{T}^{2}& 0& 0& 0\\ 0& 1& T& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& T& \frac{1}{2}{T}^{2}\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

The block for each spatial dimension has this form:

$\left[\begin{array}{ccc}1& T& \frac{1}{2}{T}^{2}\\ 0& 1& T\\ 0& 0& 1\end{array}\right]$