#' 1-D Kalmanfilter
#'
#' @name kalmanfilter_1D
#' @param x observation from sensor 1
#' @param z observation from sensor 2
#' @param P variance of observation from sensor 1 at the previous time step
#' @param u control vector
#' @param a scale constant related to x
#' @param b scale constant related to u
#' @param h scale constant between sensor 1 and 2
#' @param Q noise of the observation from sensor 1
#' @param R noise of the observation from sensor 2
#' @return the list of two value; optimal estimate for x and P
kalmanfilter_1D <- function(x, u, a, b, z, h,
P, Q, R){
ht <- h
at <- a
xhat <- a * x + b * u
Phat <- a * P * at + Q
K <- Phat * ht * (h * Phat * ht + R)^-1
xnew <- xhat + K * (z - h * xhat)
Pnew <- (1 - K * h) * Phat
return( list(xhat = xnew, Phat = Pnew) )
}
#' Kalmanfilter general
#'
#' @name kalmanfilter
#' @importFrom magrittr "%>%"
#' @param x observation from sensor 1
#' @param z observation from sensor 2
#' @param P variance of observation from sensor 1 at the previous time step
#' @param u control vector
#' @param A scale constant related to x
#' @param B scale constant related to u
#' @param H scale constant between sensor 1 and 2
#' @param Q noise of the observation from sensor 1
#' @param R noise of the observation from sensor 2
#' @return the list of two value; optimal estimate for x and P
kalmanfilter <- function(x, P, z, u,
A, B, H, Q, R){
Ht <- t(H)
At <- t(A)
xhat <- A %*% x + B %*% u
Phat <- A %*% P %*% At + Q
K <- Phat %*% Ht %*% solve(H %*% Phat %*% Ht + R)
xnew <- xhat + K %*% (z - H %*% xhat)
Pnew <- (diag(dim(K)[1]) - K %*% H) %*% Phat
return( list(xhat = xnew, Phat = Pnew) )
}
#' Calibarate accelerometer y-axis using speed data
#'
#' @name kalmanfilter_ay
#' @param acc_data accelerometer data containing time, x, y, z
#' @param speed_data speed data containing time and speed.
#' @param gps_data gps data for weak signal
#' @return calibrated y-axis accelerometer data
#' @export
kalmanfilter_ay <- function(acc_data,
speed_data,
gps_data = NULL){
n <- length(acc_data$time)
if (!is.null(gps_data)){
weak_info <- rep(FALSE, n)
weak_info[gps_data$accuracy_horiz > 5 |
gps_data$accuracy_vert > 5] <- TRUE
} else {
weak_info <- rep(FALSE, n)
}
# for (i in which(weak_info == TRUE)){
# weak_info[max(0, (i - 200)):min((i + 200), n)] <- TRUE
# }
#
# points(acc_data$time[weak_info],
# rep(0, length(acc_data$time[weak_info])),
# col= "red")
stop_info <- stop_detection(acc_data,
interval_sec = 1,
var_check = 0.001)
speed_data <- smoothing_speed(speed_data, stop_info)
stop_mom <- 0
n <- dim(stop_info)[1]
for(i in 1:n){
stop_mom <- c(stop_mom,
stop_info$stop_start[i]:stop_info$stop_end[i])
}
acc_data$y[stop_mom] <- 0
n <- length(acc_data$y)
dt <- with(acc_data, utils::tail(time, -1) - utils::head(time, -1))
estimated_states <- matrix(0, nrow = n, ncol = 2)
estimated_states[1, ] <- 0
est_data <- 0
# P = 1; h = 2.23694;
P = 1; h = 1;
# flat road
# P = 1; Q = 0.0001; R = 1; h = 1;
# hills
# P = 1; Q = 1; R = 1; h = 1;
# P = 1; Q = 100; R = 0.001; h = 2.23694;
# plot(estimated_states[2800:2850,1] * 2.23694)
# i <- 2823
previous_speed <- 0
acc_data$y[1] <- 0
i <- 2
for (i in 2:n){
A <- 1
B <- dt[i-1]
u <- as.numeric(acc_data$y[i-1])
z <- as.numeric(speed_data$speed[i] * 0.44704)
if( weak_info[i] == TRUE){
Q = 0.0001; R = 1;
} else {
Q = 0.0001; R = 1;
}
result <- kalmanfilter_1D(est_data, u, A, B, z, h,
P, Q, R)
est_data <- as.numeric(result$xhat)
acc_y <- (est_data - previous_speed)/dt[i-1]
previous_speed <- est_data
P <- result$Phat
estimated_states[i, 1] <- est_data
estimated_states[i-1, 2] <- acc_y
}
as.numeric(estimated_states[, 2])
}
#' Correct accelerometer values for stop
#'
#' @name stop_correction_acc
#' @param acc_data accelerometer data containing time, x, y, z
#' @param stop_info information of stop
#' @return An accelerometer data whose values are zero at stop moments
#' @export
stop_correction_acc <- function(acc_data, stop_info){
n <- dim(stop_info)[1]
i <- 1
for(i in 1:n){
acc_data$y[stop_info$stop_start[i]:stop_info$stop_end[i]] <- 0
acc_data$x[stop_info$stop_start[i]:stop_info$stop_end[i]] <- 0
}
acc_data
}
#' Calibarate accelerometer x-axis using Gyroscope data
#'
#' @name kalmanfilter_ax
#' @param acc_data accelerometer data containing time, x, y, z
#' @param smth_speed_data speed data which has same number of points with accelerometer data.
#' @param gyro_data Gyroscope data containing time, x, y, z
#' @return calibrated x-axis accelerometer data
#' @export
kalmanfilter_ax <- function(acc_data, gyro_data, smth_speed_data){
n <- length(acc_data$x)
dt <- with(acc_data, utils::tail(time, -1) - utils::head(time, -1))
lat_acc <- -0.44704 * smth_speed_data * gyro_data$gyroZ.rad.s.
estimated_states <- rep(0, n)
estimated_states[1] <- 0
est_data <- 0
h <- 1; P <- 1; R <- 0.5; Q <- 1
A <- 1; B <- 1
i <- 2;
for (i in 2:n){
u <- as.numeric(acc_data$x[i] - estimated_states[i-1])
z <- as.numeric(lat_acc[i])
result <- kalmanfilter_1D(est_data, u, A, B, z,
h, P, Q, R)
est_data <- as.numeric(result$xhat)
P <- result$Phat
estimated_states[i] <- est_data
}
as.numeric(estimated_states)
}
#' Convert accelerometer value into lat., long. acceleration
#'
#' @name get_NE
#' @importFrom magrittr "%>%"
#' @param acc_data accelerometer data which has time, x and y
#' @param heading_data heading data based on magnetometer which has time and trueheading
#' @return a dataframe which has latudinal and longitudinal acceleration data.
get_NE <- function(acc_data, heading_data){
cbind.data.frame(heading_data %>% dplyr::select(TrueHeading), acc_data) %>%
dplyr::select(TrueHeading, x, y) %>%
dplyr::mutate(angle_y = dplyr::if_else(y > 0, TrueHeading, (TrueHeading + 180) %% 360)) %>%
dplyr::mutate(angle_x = dplyr::if_else(x > 0, (TrueHeading + 90) %% 360, (TrueHeading + 270) %% 360)) %>%
dplyr::mutate(angle_x = angle_x * (2 * pi / 180)) %>%
dplyr::mutate(angle_y = angle_y * (2 * pi / 180)) %>%
dplyr::mutate(a_est = abs(y) * sin(angle_y) + abs(x) * sin(angle_x)) %>%
dplyr::mutate(a_nor = abs(y) * cos(angle_y) + abs(x) * cos(angle_x)) %>%
dplyr::select(a_est, a_nor)
}
#' Calibarate GPS data using accelerometer data
#'
#' @name kalmanfilter_gps
#' @importFrom magrittr "%>%"
#' @param gps_data GPS data which has the same number of points with accelerometer data
#' @param acc_data accelerometer data containing time, x, y, z
#' @param heading_data heading data based on magnetometer which has time and trueheading data.
#' @return calibrated GPS data
#' @export
kalmanfilter_gps <- function(gps_data, acc_data, heading_data){
# library(ikhyd)
# heading_data <- get_trip(system.file("extdata", "trip2.csv", package = "ikhyd"),
# data_option = 6)
acc_data_2d_orig <- acc_data %>%
dplyr::mutate(x = x * 0.00001,
y = y * 0.00001)
dt <- utils::tail(gps_data$time, -1) - utils::head(gps_data$time, -1)
acc_data_NE <- get_NE(acc_data_2d_orig, heading_data)
v_e <- speed_from_acc(acc_data$time, acc_data_NE$a_est)
v_n <- speed_from_acc(acc_data$time, acc_data_NE$a_nor)
gps_data <- gps_data %>%
dplyr::select(x, y)
N <- dim(gps_data)[1]
estimated_states <- matrix(0, nrow = N, ncol = 4)
estimated_states[1,] <- as.numeric(c(gps_data[1,], 0, 0))
est_data <- as.numeric(c(gps_data[1,], 0, 0))
H <- diag(4); P <- diag(4)
# R <- diag(4); Q <- diag(4)
R <- diag(4) * 0.0008; Q <- diag(4)
i <- 2
for (i in 2:N){
A <- matrix(c(1, 0, dt[i-1], 0,
0, 1, 0, dt[i-1],
0, 0, 1, 0,
0, 0, 0, 1), ncol = 4, byrow = TRUE)
B <- matrix(c(0.5 * dt[i-1]^2, 0, dt[i-1], 0,
0, 0.5 * dt[i-1]^2, 0, dt[i-1]), ncol = 2)
u <- as.numeric(acc_data_NE[i-1,])
# z <- as.numeric(c(gps_data[i,], (gps_data[i,] - gps_data[i-1,]) / dt[i-1]))
z <- as.numeric(c(gps_data[i,], v_e[i], v_n[i]))
result <- kalmanfilter(est_data, P,
z, u, A, B, H, Q, R)
est_data <- as.numeric(result$xhat)
P <- result$Phat
estimated_states[i,] <- est_data
}
data.frame(time = acc_data$time,
x = estimated_states[,1],
y = estimated_states[,2])
}
#' Low pass filter
#'
#' @name low_pass
#' @param input_vec input vector that needs to be filtered
#' @param alpha constant parameter for low pass rate
#' @return filtered data
#' @export
low_pass <- function(input_vec, alpha){
n <- length(input_vec)
output_vec <- rep(0, n)
for (i in 1:n){
if (i == 1){
output_vec[i] = input_vec[i]
} else {
output_vec[i] = (1 - alpha) * output_vec[i-1] + alpha * input_vec[i]
}
}
output_vec
}
#' kalman filter with road grade for lon. accelerometer
#'
#' @name kalmanfilter_withalpha
#' @param acc_data acc data
#' @param speed_data speed data from gps
#' @param angle_data angle data from gyro
#' @param alt_data altitude data from baro
#' @return filtered acc data with road grade
#' @export
kalmanfilter_withalpha <- function(acc_data,
speed_data,
angle_data,
alt_data){
alpha <- angle_data$Pitch.rads.
dt <- utils::tail(acc_data$time, -1) -
utils::head(acc_data$time, -1)
n <- dim(acc_data)[1]
estimated_states <- matrix(0, nrow = n, ncol = 2)
estimated_states[1,] <- as.numeric(c(0, alpha[1]))
past_speed <- 0
alpha_est <- alpha[1]
est_data <- as.numeric(estimated_states[1,])
acc_y <- rep(0, n)
stop_info <- stop_detection(acc_data, interval_sec = 1)
stop_vec <- get_stopvec(stop_info)
H <- diag(2); P <- diag(2)
R <- matrix(c(1, 0,
0, 1), nrow = 2, byrow = T)
Q <- matrix(c(0.001, 0,
0, 0.1), nrow = 2, byrow = T)
# Q <- matrix(c(0.001, 0,
# 0, 1), nrow = 2, byrow = T)
i <- 2
for (i in 2:n){
A <- matrix(c(1, -9.81865 * dt[i-1],
0, 1), ncol = 2, byrow = TRUE)
B <- matrix(c(dt[i-1], 0), ncol = 1)
u <- as.numeric(acc_data$y[i-1])
if (abs(alpha[i] - est_data[2]) > 0.005){
alpha_est <- est_data[2]
} else {
alpha_est <- alpha[i]
}
if (i %in% stop_vec){
z <- as.numeric(c(0, alpha[i]))
R <- diag(2) * 0.001
} else {
z <- as.numeric(c(speed_data$speed[i] * 0.44704, alpha_est))
R <- matrix(c(1, 0,
0, 0.1), nrow = 2, byrow = T)
}
result <- kalmanfilter(est_data, P,
z, u, A, B, H, Q, R)
acc_y[i] <- (result$xhat[1] - past_speed) / dt[i]
past_speed <- result$xhat[1]
alpha_est <- (acc_data$y[i] - acc_y[i]) / 9.81865
alpha_est <- est_data[2] * 0.98 + 0.01 * result$xhat[2] +
alpha_est * 0.01
result$xhat[2] <- alpha_est
est_data <- as.numeric(result$xhat)
P <- result$Phat
estimated_states[i,] <- est_data
}
data.frame(time = acc_data$time,
speed = estimated_states[,1] * 2.23694,
alpha = estimated_states[,2],
acc_y = acc_y)
}
#' kalman filter with road grade for lon. accelerometer
#'
#' @name kalmanfilter_roadgrade
#' @param angle_data angle data from gyro
#' @return filtered road grade
#' @export
kalmanfilter_roadgrade <- function(angle_data){
n <- length(angle_data$time)
estimated_alpha <- rep(0, n)
estimated_alpha[1] <- angle_data$Pitch.rads.[1]
est_data <- mean(angle_data$Pitch.rads.[1:50])
P = 1; Q = 0.01; R = 1; h = 1;
A = 1; B = 0; u = 1;
previous_alpha <- 0
i <- 2
for (i in 2:n){
z <- angle_data$Pitch.rads.[i]
prev_th <- est_data
if ((z - prev_th) / abs(prev_th) > 0.1) {
z <- 0.98 * est_data + 0.02 * z
# z <- 1.01 * est_data
} else if ((z - prev_th) / abs(prev_th) < -0.1) {
z <- 0.98 * est_data + 0.02 * z
# z <- 0.99 * est_data
}
result <- kalmanfilter_1D(est_data, u, A, B, z, h,
P, Q, R)
est_data <- as.numeric(result$xhat)
P <- result$Phat
estimated_alpha[i] <- est_data
}
as.numeric(estimated_alpha)
}
#' kalman filter with road grade for lon. accelerometer
#'
#' @name kalmanfilter_accfirst
#' @param acc_data acc data
#' @param speed_data speed data from gps
#' @param angle_data angle data from gyro
#' @param alt_data altitude data from baro
#' @param roadgrade road grad estimation
#' @param gps_data gps_data
#' @return filtered acc data with road grade
#' @export
kalmanfilter_accfirst <- function(acc_data,
speed_data,
angle_data,
alt_data,
roadgrade,
gps_data = NULL){
# with(speed_data, plot(time, low_pass(a_y, 0.05),
# ylim = c(-4, 4), type="l"))
speed_data$a_y <- low_pass(speed_data$a_y, 0.05)
alpha <- angle_data$Pitch.rads.
dt <- utils::tail(acc_data$time, -1) -
utils::head(acc_data$time, -1)
n <- length(acc_data$time)
if (!is.null(gps_data)){
weak_info <- rep(FALSE, n)
weak_info[gps_data$accuracy_horiz > 5 |
gps_data$accuracy_vert > 5] <- TRUE
} else {
weak_info <- rep(FALSE, n)
}
estimated_states <- matrix(0, nrow = n, ncol = 3)
estimated_states[1,] <- as.numeric(c(0, alpha[1], 0))
past_speed <- 0
alpha_est <- alpha[1]
est_data <- as.numeric(c(0, alpha[1]))
acc_y <- rep(0, n)
stop_info <- stop_detection(acc_data, interval_sec = 1)
stop_vec <- get_stopvec(stop_info)
H <- diag(3); P <- diag(2)
i <- 2
for (i in 2:n){
if (weak_info[i]){
R <- matrix(c(1, 0, 0,
0, 1, 0,
0, 0, 1), nrow = 3, byrow = T)
Q <- matrix(c(0.001, 0, 0,
0, 0.001, 0,
0, 0, 0.001), nrow = 3, byrow = T)
} else {
R <- matrix(c(1, 0, 0,
0, 1, 0,
0, 0, 1), nrow = 3, byrow = T)
Q <- matrix(c(1, 0, 0,
0, 1, 0,
0, 0, 1), nrow = 3, byrow = T)
}
A <- matrix(c(1, -9.81865 * dt[i-1],
0, 1,
0, -9.81865), ncol = 2, byrow = TRUE)
B <- matrix(c(dt[i-1], 0, 1), ncol = 1)
u <- as.numeric(acc_data$y[i])
# if (abs(alpha[i] - est_data[2]) > 0.005){
# alpha_est <- est_data[2]
# } else {
# alpha_est <- alpha[i]
# }
alpha_est <- alpha[i]
if (i %in% stop_vec){
z <- as.numeric(c(0, alpha[i], speed_data$a_y[i]))
R <- matrix(c(0.001, 0, 0,
0, 0.001, 0,
0, 0, 1), nrow = 3, byrow = T)
} else {
z <- as.numeric(c(speed_data$speed[i] * 0.44704, alpha_est, speed_data$a_y[i]))
}
result <- kalmanfilter(est_data, P,
z, u, A, B, H, Q, R)
acc_y[i] <- result$xhat[3]
past_speed <- result$xhat[1]
alpha_est <- (acc_data$y[i] - acc_y[i]) / 9.81865
alpha_est <- est_data[2] * 0.98 + 0.01 * result$xhat[2] +
alpha_est * 0.01
# result$xhat[2] <- alpha_est
est_data <- as.numeric(result$xhat[1:2])
P <- result$Phat[1:2, 1:2]
estimated_states[i,] <- result$xhat
}
data.frame(time = acc_data$time,
speed = estimated_states[,1] * 2.23694,
alpha = estimated_states[,2],
acc_y = estimated_states[,3])
}
if(getRversion() >= "2.15.1") {
utils::globalVariables(c("time", "x", "y",
"speed", "TrueHeading", "gyroZ.rad.s.",
"angle_x", "angle_y", "a_est", "a_nor"))
}
Add the following code to your website.
For more information on customizing the embed code, read Embedding Snippets.