I am new to particle filters and I have a particle filter based on this:
$$x_{k+1} = f_{k}(x_{k},\omega_{k})$$ $$y_{k+1} = h_{k}(x_{k},v_{k})$$ $$x = [\delta \phi, \delta \theta, \delta \psi, \delta v_n, \delta v_e, \delta v_d, \delta L, \delta \lambda, \delta h, \delta b_{ax}, \delta b_{ay}, \delta b_{az}, \delta b_{gx}, \delta b_{gy},\delta b_{gz}] $$ $$\tilde x = [\tilde \phi, \tilde \theta, \tilde \psi, \tilde v_n, \tilde v_e, \tilde v_d, \tilde L, \tilde \lambda, \tilde h, \tilde f_{x}, \tilde f_{y}, \tilde f_{z},\tilde w_{x}, \tilde w_{y}, \tilde w_{z}] $$ $$ \delta x = \tilde x - x$$ Where:
$\tilde x$ is the indicated parameter and $x$ is the true parameter.
$x$ = the state being from left to right, the errors in roll, pitch, yaw, velocity north, velocity east, velocity down, latitude, longitude, altitude, accelerometer bias body frame x y z, gyro bias body frame x y z.
$\tilde x$ = the parameters, from left to right, the roll, pitch, yaw, velocity north, velocity east, velocity down, latitude, longitude, altitude, accelerometer reading of specific force in body frame x y z, gyroscope reading of angular velocity in body frame x y z.
$k$ = the time index
$\omega_{k}$ is the process noise (zero mean, white noise with known pdf)
$y_k$ is the measurement
$v_k$ is the measurement noise (zero mean, white noise with known pdf).
The functions $f_k(.)$ and $h_k ( . )$ are time-varying nonlinear system and measurement equations. The noise sequences $\omega_k$ and $v_k$ are assumed to be independent and white with known pdf’s.
For example, take the latitude error model: $$ \dot \delta L = \frac {\delta V_n} {(\tilde R_e + \tilde h)} - \frac { \tilde V_n} {(\tilde R_e + \tilde h)}^2 \delta h$$ and the height parameter: $$ \tilde h_{k} = \tilde h_{k-1} - \frac {t}{2}(\tilde v_{d(k)}+ \tilde v_{d(k-1)}) $$ Where $t$ is the discrete time interval between predictions and $\tilde R_e$ is a function of $\tilde L$ and $\tilde h$.
Questions:
- How do the steps of the particle filter change when the state is an error state?
- Do the particles change the parameters and if so how and when?
- Do I have a set of parameters for each particle or one set of parameters which are separate to the particles?
- Should I predict my parameters forwards or my parameters minus particles forwards?
- Do I ever minus the weighted mean of the particles from the particles for each state except bias?
- When predicting the velocity parameter forwards, this requires the use of the accelerometer's specific force. it is a good idea to minus the bias first? If so, do I use the weighted mean of the accelerometer bias state of the particles?
- Do I use the model to predict the parameters and then the particles or the other way around?
- Is this the latitude error model in discrete: $$ \delta L_k = \delta L_{k-1} + (\frac {\delta V_n} {(\tilde R_e + \tilde h)} - \frac { \tilde V_n} {(\tilde R_e + \tilde h)}^2 \delta h)t$$